diff options
181 files changed, 18127 insertions, 1536 deletions
diff --git a/.gitmodules b/.gitmodules index c869803b7..6a4f815d4 100644 --- a/.gitmodules +++ b/.gitmodules @@ -16,3 +16,6 @@ [submodule "unittests/gtest"] path = unittests/gtest url = https://github.com/sjwilks/gtest.git +[submodule "src/lib/eigen"] + path = src/lib/eigen + url = https://github.com/PX4/eigen.git diff --git a/.travis.yml b/.travis.yml index a994d3471..d659bf650 100644 --- a/.travis.yml +++ b/.travis.yml @@ -37,6 +37,7 @@ env: # Email address: $PX4_EMAIL - secure: "ei3hKAw6Pk+vEkQBI5Y2Ak74BRAaXcK2UHVnVadviBHI4EVPwn1YGP6A4Y0wnLe4U7ETTl0UiijRoVxyDW0Mq896Pv0siw02amNpjSZZYu+RfN1+//MChB48OxsLDirUdHVrULhl/bOARM02h2Bg28jDE2g7IqmJwg3em3oMbjU=" - PX4_REPORT=report.txt + - BUILD_URI=https://px4-travis.s3.amazonaws.com/archives/Firmware/$TRAVIS_BRANCH/$TRAVIS_BUILD_ID/Firmware.zip script: - arm-none-eabi-gcc --version @@ -58,11 +59,14 @@ after_script: # upload newest build for this branch with s3 index - ./CI-Tools/s3cmd-put Images/px4*.px4 CI-Tools/directory/index.html Firmware/$TRAVIS_BRANCH/ # archive newest build by date with s3 index - - ./CI-Tools/s3cmd-put Firmware.zip archives/Firmware/$TRAVIS_BRANCH/`date "+%Y-%m-%d"`-$TRAVIS_BUILD_ID/ + - ./CI-Tools/s3cmd-put Firmware.zip archives/Firmware/$TRAVIS_BRANCH/$TRAVIS_BUILD_ID/ - ./CI-Tools/s3cmd-put CI-Tools/directory/index.html archives/Firmware/$TRAVIS_BRANCH/ # upload top level index.html and timestamp.html - ./CI-Tools/s3cmd-put CI-Tools/index.html index.html - ./CI-Tools/s3cmd-put CI-Tools/timestamp.html timestamp.html + - echo "" + - echo "Binaries have been posted to:" + - echo https://px4-travis.s3.amazonaws.com/archives/Firmware/$TRAVIS_BRANCH/$TRAVIS_BUILD_ID/Firmware.zip deploy: provider: releases diff --git a/Firmware.sublime-project b/Firmware.sublime-project index 7292307d5..091bfb128 100644 --- a/Firmware.sublime-project +++ b/Firmware.sublime-project @@ -38,10 +38,18 @@ "build_systems": [ { - "name": "PX4", + "name": "PX4: make all", "working_dir": "${project_path}", "file_regex": "^(..[^:]*):([0-9]+):?([0-9]+)?:? (.*)$", - "cmd": ["make"] + "cmd": ["make"], + "shell": true + }, + { + "name": "PX4: make and upload", + "working_dir": "${project_path}", + "file_regex": "^(..[^:]*):([0-9]+):?([0-9]+)?:? (.*)$", + "cmd": ["make upload px4fmu-v2_default -j8"], + "shell": true } ] } diff --git a/NuttX b/NuttX -Subproject 11afcdfee6a3961952dd92f02c1abaa4756b115 +Subproject 94818ce16f63c4728a1d9316628a3651287f16d diff --git a/ROMFS/px4fmu_common/init.d/10019_sk450_deadcat b/ROMFS/px4fmu_common/init.d/10019_sk450_deadcat index e68f57f25..c6861c2d4 100644 --- a/ROMFS/px4fmu_common/init.d/10019_sk450_deadcat +++ b/ROMFS/px4fmu_common/init.d/10019_sk450_deadcat @@ -29,3 +29,10 @@ set MIXER sk450_deadcat set PWM_OUT 1234 set PWM_MIN 1050 + +set PWM_AUX_OUT 1234 +# set PWM_AUX_MIN 900 +# set PWM_AUX_MAX 2100 +set PWM_AUX_RATE 100 + +gimbal start diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_defaults b/ROMFS/px4fmu_common/init.d/rc.fw_defaults index e6de64054..156711c26 100644 --- a/ROMFS/px4fmu_common/init.d/rc.fw_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.fw_defaults @@ -15,4 +15,11 @@ then param set FW_T_RLL2THR 15 param set FW_T_SRATE_P 0.01 param set FW_T_TIME_CONST 5 + + param set PE_VELNE_NOISE 0.3 + param set PE_VELD_NOISE 0.5 + param set PE_POSNE_NOISE 0.5 + param set PE_POSD_NOISE 0.5 + param set PE_GBIAS_PNOISE 0.000001 + param set PE_ABIAS_PNOISE 0.0002 fi diff --git a/ROMFS/px4fmu_common/init.d/rc.interface b/ROMFS/px4fmu_common/init.d/rc.interface index eceff0f99..5101acc07 100644 --- a/ROMFS/px4fmu_common/init.d/rc.interface +++ b/ROMFS/px4fmu_common/init.d/rc.interface @@ -11,7 +11,7 @@ then # Load main mixer # - if [ $MIXER_AUX == none -a $USE_IO == yes ] + if [ $MIXER_AUX == none -a $USE_IO == yes ] then set MIXER_AUX $MIXER.aux fi @@ -103,6 +103,7 @@ then # set MIXER_AUX_FILE none + set OUTPUT_AUX_DEV /dev/pwm_output1 if [ -f $SDCARD_MIXERS_PATH/$MIXER_AUX.mix ] then @@ -119,10 +120,43 @@ then then if fmu mode_pwm then - mixer load /dev/pwm_output1 $MIXER_AUX_FILE + mixer load $OUTPUT_AUX_DEV $MIXER_AUX_FILE else tone_alarm $TUNE_ERR fi + + if [ $PWM_AUX_OUT != none ] + then + # + # Set PWM_AUX output frequency + # + if [ $PWM_AUX_RATE != none ] + then + pwm rate -c $PWM_AUX_OUT -r $PWM_AUX_RATE -d $OUTPUT_AUX_DEV + fi + + # + # Set disarmed, min and max PWM_AUX values + # + if [ $PWM_AUX_DISARMED != none ] + then + pwm disarmed -c $PWM_AUX_OUT -p $PWM_AUX_DISARMED -d $OUTPUT_AUX_DEV + fi + if [ $PWM_AUX_MIN != none ] + then + pwm min -c $PWM_AUX_OUT -p $PWM_AUX_MIN -d $OUTPUT_AUX_DEV + fi + if [ $PWM_AUX_MAX != none ] + then + pwm max -c $PWM_AUX_OUT -p $PWM_AUX_MAX -d $OUTPUT_AUX_DEV + fi + fi + + if [ $FAILSAFE_AUX != none ] + then + pwm failsafe -d $OUTPUT_AUX_DEV $FAILSAFE + fi + fi fi unset OUTPUT_DEV diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_defaults b/ROMFS/px4fmu_common/init.d/rc.mc_defaults index 24d9650a0..1f34282ae 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.mc_defaults @@ -40,6 +40,8 @@ then param set PE_VELD_NOISE 0.7 param set PE_POSNE_NOISE 0.5 param set PE_POSD_NOISE 1.0 + param set PE_GBIAS_PNOISE 0.000001 + param set PE_ABIAS_PNOISE 0.0001 param set NAV_ACC_RAD 2.0 param set RTL_RETURN_ALT 30.0 diff --git a/ROMFS/px4fmu_common/init.d/rc.vtol_defaults b/ROMFS/px4fmu_common/init.d/rc.vtol_defaults index 6dc5a65db..844d540bf 100644 --- a/ROMFS/px4fmu_common/init.d/rc.vtol_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.vtol_defaults @@ -32,6 +32,13 @@ then param set FW_RR_I 0.00 param set FW_RR_IMAX 0.2 param set FW_RR_P 0.02 + + param set PE_VELNE_NOISE 0.5 + param set PE_VELD_NOISE 0.7 + param set PE_POSNE_NOISE 0.5 + param set PE_POSD_NOISE 1.0 + param set PE_GBIAS_PNOISE 0.000001 + param set PE_ABIAS_PNOISE 0.0001 fi set PWM_DISARMED 900 diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index b6db3a0e4..111965bd5 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -109,6 +109,11 @@ then set PWM_DISARMED none set PWM_MIN none set PWM_MAX none + set PWM_AUX_OUT none + set PWM_AUX_RATE none + set PWM_AUX_DISARMED none + set PWM_AUX_MIN none + set PWM_AUX_MAX none set MK_MODE none set FMU_MODE pwm set MAVLINK_F default @@ -200,8 +205,13 @@ then if px4io start then + # try to safe px4 io so motor outputs dont go crazy if px4io safety_on then + # success! no-op + else + # px4io did not respond to the safety command + px4io stop fi fi diff --git a/ROMFS/px4fmu_common/mixers/sk450_deadcat.aux.mix b/ROMFS/px4fmu_common/mixers/sk450_deadcat.aux.mix new file mode 100644 index 000000000..11f3813ad --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/sk450_deadcat.aux.mix @@ -0,0 +1,38 @@ +Gimbal / payload mixer for PX4FMU +=========================== + +Configuration with 2 gimbals: + - 2 axes inline GoPro gimbal (pitch, roll) + - 2 axes FPV gimbal (pitch, yaw), physically attached GoPro gimbal's roll stabilization +----------------------------------------------------- + +# gimbal roll +M: 1 +O: 10000 10000 0 -10000 10000 +S: 2 0 -11500 -10000 900 -10000 10000 + +# gimbal pitch +M: 1 +O: 10000 10000 0 -10000 10000 +S: 2 1 12000 12000 2000 -10000 10000 + +# FPV gimbal yaw (not implemented, yet) +M: 1 +O: 10000 10000 0 -10000 10000 +S: 2 2 10000 10000 0 -10000 10000 + +# FPV gimbal pitch +M: 1 +O: 10000 10000 0 -10000 10000 +S: 2 1 -12000 -12000 -3000 -10000 10000 + +# reserved, not used +M: 1 +O: 10000 10000 0 -10000 10000 +S: 2 4 10000 10000 0 -10000 10000 + +# parachute +M: 1 +O: 10000 10000 0 -10000 10000 +S: 2 7 10000 10000 0 -10000 10000 + diff --git a/ROMFS/px4fmu_common/mixers/sk450_deadcat.main.mix b/ROMFS/px4fmu_common/mixers/sk450_deadcat.main.mix index a8c5b716d..8daf81005 100644 --- a/ROMFS/px4fmu_common/mixers/sk450_deadcat.main.mix +++ b/ROMFS/px4fmu_common/mixers/sk450_deadcat.main.mix @@ -1,4 +1,4 @@ -Multirotor mixer for PX4FMU +Multirotor mixer for PX4IO =========================== This file defines a single mixer for a quadrotor in SK450 DeadCat configuration. All controls are mixed 100%. diff --git a/ROMFS/px4fmu_test/init.d/rcS b/ROMFS/px4fmu_test/init.d/rcS index 233a098b8..6d72ecc6c 100644 --- a/ROMFS/px4fmu_test/init.d/rcS +++ b/ROMFS/px4fmu_test/init.d/rcS @@ -230,3 +230,5 @@ else fi ver all + +free diff --git a/Tools/Matlab/motors.m b/Tools/Matlab/motors.m new file mode 100644 index 000000000..6d688a307 --- /dev/null +++ b/Tools/Matlab/motors.m @@ -0,0 +1,58 @@ +clear all; +close all; + +% Measurement data +% 1045 propeller +% Robbe Roxxy Motor (1100 kV, data collected in 2010) +data = [ 45, 7.4;... + 38, 5.6;... + 33, 4.3;... + 26, 3.0;... + 18, 2.0;... + 10, 1.0 ]; + +% Normalize the data, as we're operating later +% anyways in normalized units +data(:,1) = data(:,1) ./ max(data(:,1)); +data(:,2) = data(:,2) ./ max(data(:,2)); + +% Fit a 2nd degree polygon to the data and +% print the x2, x1, x0 coefficients +p = polyfit(data(:,2), data(:,1),2) + +% Override the first coffefficient for testing +% purposes +pf = 0.62; + +% Generate plotting data +px1 = linspace(0, max(data(:,2))); +py1 = polyval(p, px1); + +pyt = zeros(size(data, 1), 1); +corr = zeros(size(data, 1), 1); + +% Actual code test +% the two lines below are the ones needed to be ported to C: +% pf: Power factor parameter. +% px1(i): The current normalized motor command (-1..1) +% corr(i): The required correction. The motor speed is: +% px1(i) +for i=1:size(px1, 2) + + % The actual output throttle + pyt(i) = -pf * (px1(i) * px1(i)) + (1 + pf) * px1(i); + + % Solve for input throttle + % y = -p * x^2 + (1+p) * x; + % +end + +plot(data(:,2), data(:,1), '*r'); +hold on; +plot(px1, py1, '*b'); +hold on; +plot([0 px1(end)], [0 py1(end)], '-k'); +hold on; +plot(px1, pyt, '-b'); +hold on; +plot(px1, corr, '-m'); diff --git a/Tools/boot_now.py b/Tools/boot_now.py new file mode 100755 index 000000000..5a9e608da --- /dev/null +++ b/Tools/boot_now.py @@ -0,0 +1,59 @@ +#!/usr/bin/env python +############################################################################ +# +# Copyright (C) 2012-2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# send BOOT command to a device + +import argparse +import serial, sys + +from sys import platform as _platform + +# Parse commandline arguments +parser = argparse.ArgumentParser(description="Send boot command to a device") +parser.add_argument('--baud', action="store", type=int, default=115200, help="Baud rate of the serial port") +parser.add_argument('port', action="store", help="Serial port(s) to which the FMU may be attached") +args = parser.parse_args() + +REBOOT = b'\x30' +EOC = b'\x20' + +print("Sending reboot to %s" % args.port) +try: + port = serial.Serial(args.port, args.baud, timeout=0.5) +except Exception: + print("Unable to open %s" % args.port) + sys.exit(1) +port.write(REBOOT + EOC) +port.close() +sys.exit(0) diff --git a/Tools/check_code_style.sh b/Tools/check_code_style.sh index 8d1ab6363..2353a5232 100755 --- a/Tools/check_code_style.sh +++ b/Tools/check_code_style.sh @@ -3,7 +3,7 @@ set -eu failed=0 for fn in $(find . -path './src/lib/uavcan' -prune -o \ -path './src/lib/mathlib/CMSIS' -prune -o \ - -path './src/modules/attitude_estimator_ekf/codegen/' -prune -o \ + -path './src/modules/attitude_estimator_ekf/codegen' -prune -o \ -path './NuttX' -prune -o \ -path './Build' -prune -o \ -path './mavlink' -prune -o \ diff --git a/Tools/check_submodules.sh b/Tools/check_submodules.sh index 5e6e57164..4b251642c 100755 --- a/Tools/check_submodules.sh +++ b/Tools/check_submodules.sh @@ -74,6 +74,28 @@ else git submodule update; fi +if [ -d src/lib/eigen ] +then + STATUSRETVAL=$(git submodule summary | grep -A20 -i eigen | grep "<") + if [ -z "$STATUSRETVAL" ] + then + echo "Checked Eigen submodule, correct version found" + else + echo "" + echo "" + echo "New commits required:" + echo "$(git submodule summary)" + echo "" + echo "" + echo "eigen sub repo not at correct version. Try 'git submodule update'" + echo "or follow instructions on http://pixhawk.org/dev/git/submodules" + exit 1 + fi +else + git submodule init; + git submodule update; +fi + if [ -d Tools/gencpp ] then STATUSRETVAL=$(git submodule summary | grep -A20 -i gencpp | grep "<") diff --git a/Tools/px_uploader.py b/Tools/px_uploader.py index 859c821e5..3cdd9d1a6 100755 --- a/Tools/px_uploader.py +++ b/Tools/px_uploader.py @@ -160,6 +160,8 @@ class uploader(object): GET_CRC = b'\x29' # rev3+ GET_OTP = b'\x2a' # rev4+ , get a word from OTP area GET_SN = b'\x2b' # rev4+ , get a word from SN area + GET_CHIP = b'\x2c' # rev5+ , get chip version + SET_BOOT_DELAY = b'\x2d' # rev5+ , set boot delay REBOOT = b'\x30' INFO_BL_REV = b'\x01' # bootloader protocol revision @@ -258,7 +260,7 @@ class uploader(object): self.__getSync() return value - # send the GET_OTP command and wait for an info parameter + # send the GET_SN command and wait for an info parameter def __getSN(self, param): t = struct.pack("I", param) # int param as 32bit ( 4 byte ) char array. self.__send(uploader.GET_SN + t + uploader.EOC) @@ -266,6 +268,13 @@ class uploader(object): self.__getSync() return value + # send the GET_CHIP command + def __getCHIP(self): + self.__send(uploader.GET_CHIP + uploader.EOC) + value = self.__recv_int() + self.__getSync() + return value + def __drawProgressBar(self, label, progress, maxVal): if maxVal < progress: progress = maxVal @@ -397,6 +406,12 @@ class uploader(object): raise RuntimeError("Program CRC failed") self.__drawProgressBar(label, 100, 100) + def __set_boot_delay(self, boot_delay): + self.__send(uploader.SET_BOOT_DELAY + + struct.pack("b", boot_delay) + + uploader.EOC) + self.__getSync() + # get basic data about the board def identify(self): # make sure we are in sync before starting @@ -416,7 +431,12 @@ class uploader(object): def upload(self, fw): # Make sure we are doing the right thing if self.board_type != fw.property('board_id'): - raise IOError("Firmware not suitable for this board") + msg = "Firmware not suitable for this board (board_type=%u board_id=%u)" % ( + self.board_type, fw.property('board_id')) + if args.force: + print("WARNING: %s" % msg) + else: + raise IOError(msg) if self.fw_maxsize < fw.property('image_size'): raise RuntimeError("Firmware image is too large for this board") @@ -446,6 +466,7 @@ class uploader(object): self.sn = self.sn + x print(binascii.hexlify(x).decode('Latin-1'), end='') # show user print('') + print("chip: %08x" % self.__getCHIP()) except Exception: # ignore bad character encodings pass @@ -458,6 +479,9 @@ class uploader(object): else: self.__verify_v3("Verify ", fw) + if args.boot_delay is not None: + self.__set_boot_delay(args.boot_delay) + print("\nRebooting.\n") self.__reboot() self.port.close() @@ -486,6 +510,8 @@ else: parser = argparse.ArgumentParser(description="Firmware uploader for the PX autopilot system.") parser.add_argument('--port', action="store", required=True, help="Serial port(s) to which the FMU may be attached") parser.add_argument('--baud', action="store", type=int, default=115200, help="Baud rate of the serial port (default is 115200), only required for true serial ports.") +parser.add_argument('--force', action='store_true', default=False, help='Override board type check and continue loading') +parser.add_argument('--boot-delay', type=int, default=None, help='minimum boot delay to store in flash') parser.add_argument('firmware', action="store", help="Firmware file to be uploaded") args = parser.parse_args() diff --git a/integrationtests/demo_tests/direct_manual_input_test.py b/integrationtests/demo_tests/direct_manual_input_test.py index 6d115316b..394500e94 100755 --- a/integrationtests/demo_tests/direct_manual_input_test.py +++ b/integrationtests/demo_tests/direct_manual_input_test.py @@ -37,7 +37,6 @@ # PKG = 'px4' -import sys import unittest import rospy @@ -50,36 +49,40 @@ from manual_input import ManualInput # class ManualInputTest(unittest.TestCase): + def setUp(self): + self.actuator_status = actuator_armed() + self.control_mode = vehicle_control_mode() + # # General callback functions used in tests # def actuator_armed_callback(self, data): - self.actuatorStatus = data + self.actuator_status = data def vehicle_control_mode_callback(self, data): - self.controlMode = data + self.control_mode = data # # Test arming # def test_manual_input(self): rospy.init_node('test_node', anonymous=True) - rospy.Subscriber('px4_multicopter/actuator_armed', actuator_armed, self.actuator_armed_callback) - rospy.Subscriber('px4_multicopter/vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback) + rospy.Subscriber('iris/actuator_armed', actuator_armed, self.actuator_armed_callback) + rospy.Subscriber('iris/vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback) - man = ManualInput() + man_in = ManualInput() # Test arming - man.arm() - self.assertEquals(self.actuatorStatus.armed, True, "did not arm") + man_in.arm() + self.assertEquals(self.actuator_status.armed, True, "did not arm") # Test posctl - man.posctl() - self.assertTrue(self.controlMode.flag_control_position_enabled, "flag_control_position_enabled is not set") + man_in.posctl() + self.assertTrue(self.control_mode.flag_control_position_enabled, "flag_control_position_enabled is not set") # Test offboard - man.offboard() - self.assertTrue(self.controlMode.flag_control_offboard_enabled, "flag_control_offboard_enabled is not set") + man_in.offboard() + self.assertTrue(self.control_mode.flag_control_offboard_enabled, "flag_control_offboard_enabled is not set") if __name__ == '__main__': diff --git a/integrationtests/demo_tests/direct_offboard_posctl_test.py b/integrationtests/demo_tests/direct_offboard_posctl_test.py index e09550bbc..734bcf590 100755 --- a/integrationtests/demo_tests/direct_offboard_posctl_test.py +++ b/integrationtests/demo_tests/direct_offboard_posctl_test.py @@ -37,23 +37,22 @@ # PKG = 'px4' -import sys import unittest import rospy +import rosbag from numpy import linalg import numpy as np from px4.msg import vehicle_local_position from px4.msg import vehicle_control_mode -from px4.msg import actuator_armed from px4.msg import position_setpoint_triplet from px4.msg import position_setpoint -from sensor_msgs.msg import Joy -from std_msgs.msg import Header +from px4.msg import vehicle_local_position_setpoint from manual_input import ManualInput from flight_path_assertion import FlightPathAssertion +from px4_test_helper import PX4TestHelper # # Tests flying a path in offboard control by directly sending setpoints @@ -66,33 +65,42 @@ class DirectOffboardPosctlTest(unittest.TestCase): def setUp(self): rospy.init_node('test_node', anonymous=True) - rospy.Subscriber('px4_multicopter/vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback) - rospy.Subscriber("px4_multicopter/vehicle_local_position", vehicle_local_position, self.position_callback) - self.pubSpt = rospy.Publisher('px4_multicopter/position_setpoint_triplet', position_setpoint_triplet, queue_size=10) + self.helper = PX4TestHelper("direct_offboard_posctl_test") + self.helper.setUp() + + rospy.Subscriber('iris/vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback) + self.sub_vlp = rospy.Subscriber("iris/vehicle_local_position", vehicle_local_position, self.position_callback) + self.pub_spt = rospy.Publisher('iris/position_setpoint_triplet', position_setpoint_triplet, queue_size=10) self.rate = rospy.Rate(10) # 10hz + self.has_pos = False + self.local_position = vehicle_local_position() + self.control_mode = vehicle_control_mode() def tearDown(self): - if (self.fpa): + if self.fpa: self.fpa.stop() + self.helper.tearDown() + # # General callback functions used in tests # + def position_callback(self, data): - self.hasPos = True - self.localPosition = data + self.has_pos = True + self.local_position = data def vehicle_control_mode_callback(self, data): - self.controlMode = data + self.control_mode = data # # Helper methods # def is_at_position(self, x, y, z, offset): - rospy.logdebug("current position %f, %f, %f" % (self.localPosition.x, self.localPosition.y, self.localPosition.z)) + rospy.logdebug("current position %f, %f, %f" % (self.local_position.x, self.local_position.y, self.local_position.z)) desired = np.array((x, y, z)) - pos = np.array((self.localPosition.x, self.localPosition.y, self.localPosition.z)) + pos = np.array((self.local_position.x, self.local_position.y, self.local_position.z)) return linalg.norm(desired - pos) < offset def reach_position(self, x, y, z, timeout): @@ -105,12 +113,13 @@ class DirectOffboardPosctlTest(unittest.TestCase): pos.position_valid = True stp = position_setpoint_triplet() stp.current = pos - self.pubSpt.publish(stp) + self.pub_spt.publish(stp) + self.helper.bag_write('px4/position_setpoint_triplet', stp) # does it reach the position in X seconds? count = 0 - while(count < timeout): - if(self.is_at_position(pos.x, pos.y, pos.z, 0.5)): + while count < timeout: + if self.is_at_position(pos.x, pos.y, pos.z, 0.5): break count = count + 1 self.rate.sleep() @@ -121,22 +130,23 @@ class DirectOffboardPosctlTest(unittest.TestCase): # Test offboard position control # def test_posctl(self): - manIn = ManualInput() + man_in = ManualInput() # arm and go into offboard - manIn.arm() - manIn.offboard() - self.assertTrue(self.controlMode.flag_armed, "flag_armed is not set") - self.assertTrue(self.controlMode.flag_control_offboard_enabled, "flag_control_offboard_enabled is not set") - self.assertTrue(self.controlMode.flag_control_position_enabled, "flag_control_position_enabled is not set") + man_in.arm() + man_in.offboard() + self.assertTrue(self.control_mode.flag_armed, "flag_armed is not set") + self.assertTrue(self.control_mode.flag_control_offboard_enabled, "flag_control_offboard_enabled is not set") + self.assertTrue(self.control_mode.flag_control_position_enabled, "flag_control_position_enabled is not set") # prepare flight path positions = ( - (0,0,0), - (2,2,-2), - (2,-2,-2), - (-2,-2,-2), - (2,2,-2)) + (0, 0, 0), + (0, 0, -2), + (2, 2, -2), + (2, -2, -2), + (-2, -2, -2), + (2, 2, -2)) # flight path assertion self.fpa = FlightPathAssertion(positions, 1, 0) @@ -147,12 +157,10 @@ class DirectOffboardPosctlTest(unittest.TestCase): self.assertFalse(self.fpa.failed, "breached flight path tunnel (%d)" % i) # does it hold the position for Y seconds? - positionHeld = True count = 0 timeout = 50 - while(count < timeout): - if(not self.is_at_position(2, 2, -2, 0.5)): - positionHeld = False + while count < timeout: + if not self.is_at_position(2, 2, -2, 0.5): break count = count + 1 self.rate.sleep() diff --git a/integrationtests/demo_tests/direct_tests.launch b/integrationtests/demo_tests/direct_tests.launch index 1a7d843fd..f332332cf 100644 --- a/integrationtests/demo_tests/direct_tests.launch +++ b/integrationtests/demo_tests/direct_tests.launch @@ -2,7 +2,7 @@ <arg name="headless" default="true"/> <arg name="gui" default="false"/> <arg name="enable_logging" default="false"/> - <arg name="enable_ground_truth" default="false"/> + <arg name="enable_ground_truth" default="true"/> <arg name="log_file" default="iris"/> <include file="$(find px4)/launch/gazebo_iris_empty_world.launch"> diff --git a/integrationtests/demo_tests/flight_path_assertion.py b/integrationtests/demo_tests/flight_path_assertion.py index 485de8c41..244324cd0 100644 --- a/integrationtests/demo_tests/flight_path_assertion.py +++ b/integrationtests/demo_tests/flight_path_assertion.py @@ -35,7 +35,6 @@ # # @author Andreas Antener <andreas@uaventure.com> # -import sys import rospy import threading @@ -48,7 +47,6 @@ from geometry_msgs.msg import Twist from numpy import linalg import numpy as np -import math # # Helper to test if vehicle stays on expected flight path. @@ -62,30 +60,52 @@ class FlightPathAssertion(threading.Thread): # TODO: yaw validation # TODO: fail main test thread # - def __init__(self, positions, tunnelRadius = 1, yawOffset = 0.2): + def __init__(self, positions, tunnelRadius=1, yaw_offset=0.2): threading.Thread.__init__(self) - rospy.Subscriber("px4_multicopter/vehicle_local_position", vehicle_local_position, self.position_callback) - self.spawnModel = rospy.ServiceProxy('gazebo/spawn_sdf_model', SpawnModel) - self.setModelState = rospy.ServiceProxy('gazebo/set_model_state', SetModelState) - self.deleteModel = rospy.ServiceProxy('gazebo/delete_model', DeleteModel) + rospy.Subscriber("iris/vehicle_local_position", vehicle_local_position, self.position_callback) + self.spawn_model = rospy.ServiceProxy('gazebo/spawn_sdf_model', SpawnModel) + self.set_model_state = rospy.ServiceProxy('gazebo/set_model_state', SetModelState) + self.delete_model = rospy.ServiceProxy('gazebo/delete_model', DeleteModel) self.positions = positions - self.tunnelRadius = tunnelRadius - self.yawOffset = yawOffset - self.hasPos = False - self.shouldStop = False + self.tunnel_radius = tunnelRadius + self.yaw_offset = yaw_offset + self.has_pos = False + self.should_stop = False self.center = positions[0] - self.endOfSegment = False + self.end_of_segment = False self.failed = False + self.local_position = vehicle_local_position def position_callback(self, data): - self.hasPos = True - self.localPosition = data + self.has_pos = True + self.local_position = data def spawn_indicator(self): - self.deleteModel("indicator") - xml = "<?xml version='1.0'?><sdf version='1.4'><model name='indicator'><static>true</static><link name='link'><visual name='visual'><transparency>0.7</transparency><geometry><sphere><radius>%f</radius></sphere></geometry><material><ambient>1 0 0 0.5</ambient><diffuse>1 0 0 0.5</diffuse></material></visual></link></model></sdf>" % self.tunnelRadius - self.spawnModel("indicator", xml, "", Pose(), "") + self.delete_model("indicator") + xml = ( + "<?xml version='1.0'?>" + + "<sdf version='1.4'>" + + "<model name='indicator'>" + + "<static>true</static>" + + "<link name='link'>" + + "<visual name='visual'>" + + "<transparency>0.7</transparency>" + + "<geometry>" + + "<sphere>" + + "<radius>%f</radius>" + + "</sphere>" + + "</geometry>" + + "<material>" + + "<ambient>1 0 0 0.5</ambient>" + + "<diffuse>1 0 0 0.5</diffuse>" + + "</material>" + + "</visual>" + + "</link>" + + "</model>" + + "</sdf>") % self.tunnel_radius + + self.spawn_model("indicator", xml, "", Pose(), "") def position_indicator(self): state = SetModelState() @@ -97,7 +117,7 @@ class FlightPathAssertion(threading.Thread): state.pose = pose state.twist = Twist() state.reference_frame = "" - self.setModelState(state) + self.set_model_state(state) def distance_to_line(self, a, b, pos): v = b - a @@ -111,7 +131,7 @@ class FlightPathAssertion(threading.Thread): c2 = np.dot(v, v) if c2 <= c1: # after b self.center = b - self.endOfSegment = True + self.end_of_segment = True return linalg.norm(pos - b) x = c1 / c2 @@ -120,52 +140,58 @@ class FlightPathAssertion(threading.Thread): return linalg.norm(pos - l) def stop(self): - self.shouldStop = True + self.should_stop = True def run(self): rate = rospy.Rate(10) # 10hz self.spawn_indicator() current = 0 - - while not self.shouldStop: - if (self.hasPos): + count = 0 + while not self.should_stop: + if self.has_pos: # calculate distance to line segment between first two points - # if distances > tunnelRadius + # if distances > tunnel_radius # exit with error - # advance current pos if not on the line anymore or distance to next point < tunnelRadius + # advance current pos if not on the line anymore or distance to next point < tunnel_radius # exit if current pos is now the last position self.position_indicator() - pos = np.array((self.localPosition.x, - self.localPosition.y, - self.localPosition.z)) - aPos = np.array((self.positions[current][0], - self.positions[current][1], - self.positions[current][2])) - bPos = np.array((self.positions[current + 1][0], - self.positions[current + 1][1], - self.positions[current + 1][2])) + pos = np.array((self.local_position.x, + self.local_position.y, + self.local_position.z)) + a_pos = np.array((self.positions[current][0], + self.positions[current][1], + self.positions[current][2])) + b_pos = np.array((self.positions[current + 1][0], + self.positions[current + 1][1], + self.positions[current + 1][2])) - dist = self.distance_to_line(aPos, bPos, pos) - bDist = linalg.norm(pos - bPos) + dist = self.distance_to_line(a_pos, b_pos, pos) + b_dist = linalg.norm(pos - b_pos) - rospy.logdebug("distance to line: %f, distance to end: %f" % (dist, bDist)) + rospy.logdebug("distance to line: %f, distance to end: %f" % (dist, b_dist)) - if (dist > self.tunnelRadius): - msg = "left tunnel at position (%f, %f, %f)" % (self.localPosition.x, self.localPosition.y, self.localPosition.z) + if dist > self.tunnel_radius: + msg = "left tunnel at position (%f, %f, %f)" % (self.local_position.x, self.local_position.y, self.local_position.z) rospy.logerr(msg) self.failed = True break - if (self.endOfSegment or bDist < self.tunnelRadius): + if self.end_of_segment or b_dist < self.tunnel_radius: rospy.loginfo("next segment") - self.endOfSegment = False + self.end_of_segment = False current = current + 1 - if (current == len(self.positions) - 1): + if current == len(self.positions) - 1: rospy.loginfo("no more positions") break rate.sleep() + count = count + 1 + + if count > 10 and not self.has_pos: # no position after 1 sec + rospy.logerr("no position") + self.failed = True + break diff --git a/integrationtests/demo_tests/manual_input.py b/integrationtests/demo_tests/manual_input.py index 9b2471a00..167221285 100755 --- a/integrationtests/demo_tests/manual_input.py +++ b/integrationtests/demo_tests/manual_input.py @@ -35,7 +35,6 @@ # # @author Andreas Antener <andreas@uaventure.com> # -import sys import rospy from px4.msg import manual_control_setpoint @@ -46,17 +45,12 @@ from std_msgs.msg import Header # # Manual input control helper # -# FIXME: this is not the way to do it! ATM it fakes input to iris/command/attitude because else -# the simulator does not instantiate our controller. Probably this whole class will disappear once -# arming works correctly. -# -class ManualInput: +class ManualInput(object): def __init__(self): rospy.init_node('test_node', anonymous=True) - self.pubMcsp = rospy.Publisher('px4_multicopter/manual_control_setpoint', manual_control_setpoint, queue_size=10) - self.pubOff = rospy.Publisher('px4_multicopter/offboard_control_mode', offboard_control_mode, queue_size=10) - self.pubAtt = rospy.Publisher('iris/command/attitude', CommandAttitudeThrust, queue_size=10) + self.pub_mcsp = rospy.Publisher('iris/manual_control_setpoint', manual_control_setpoint, queue_size=10) + self.pub_off = rospy.Publisher('iris/offboard_control_mode', offboard_control_mode, queue_size=10) def arm(self): rate = rospy.Rate(10) # 10hz @@ -81,11 +75,9 @@ class ManualInput: rospy.loginfo("zeroing") time = rospy.get_rostime().now() pos.timestamp = time.secs * 1e6 + time.nsecs / 1000 - self.pubMcsp.publish(pos) - # Fake input to iris commander - self.pubAtt.publish(att) + self.pub_mcsp.publish(pos) rate.sleep() - count = count + 1 + count = count + 1 pos.r = 1 count = 0 @@ -93,7 +85,7 @@ class ManualInput: rospy.loginfo("arming") time = rospy.get_rostime().now() pos.timestamp = time.secs * 1e6 + time.nsecs / 1000 - self.pubMcsp.publish(pos) + self.pub_mcsp.publish(pos) rate.sleep() count = count + 1 @@ -118,7 +110,7 @@ class ManualInput: rospy.loginfo("triggering posctl") time = rospy.get_rostime().now() pos.timestamp = time.secs * 1e6 + time.nsecs / 1000 - self.pubMcsp.publish(pos) + self.pub_mcsp.publish(pos) rate.sleep() count = count + 1 @@ -147,7 +139,7 @@ class ManualInput: rospy.loginfo("setting offboard mode") time = rospy.get_rostime().now() mode.timestamp = time.secs * 1e6 + time.nsecs / 1000 - self.pubOff.publish(mode) + self.pub_off.publish(mode) rate.sleep() count = count + 1 @@ -169,7 +161,7 @@ class ManualInput: rospy.loginfo("triggering offboard") time = rospy.get_rostime().now() pos.timestamp = time.secs * 1e6 + time.nsecs / 1000 - self.pubMcsp.publish(pos) + self.pub_mcsp.publish(pos) rate.sleep() count = count + 1 diff --git a/integrationtests/demo_tests/mavros_offboard_attctl_test.py b/integrationtests/demo_tests/mavros_offboard_attctl_test.py index a52f7ffc1..ef875ce61 100755 --- a/integrationtests/demo_tests/mavros_offboard_attctl_test.py +++ b/integrationtests/demo_tests/mavros_offboard_attctl_test.py @@ -37,79 +37,63 @@ # PKG = 'px4' -import sys import unittest import rospy -import math - -from numpy import linalg -import numpy as np +import rosbag from px4.msg import vehicle_control_mode +from px4.msg import vehicle_local_position +from px4.msg import vehicle_attitude_setpoint +from px4.msg import vehicle_attitude from std_msgs.msg import Header from std_msgs.msg import Float64 from geometry_msgs.msg import PoseStamped, Quaternion from tf.transformations import quaternion_from_euler -from mavros.srv import CommandBool - -from manual_input import ManualInput +from px4_test_helper import PX4TestHelper # -# Tests flying a path in offboard control by sending position setpoints +# Tests flying a path in offboard control by sending attitude and thrust setpoints # over MAVROS. # -# For the test to be successful it needs to reach all setpoints in a certain time. -# FIXME: add flight path assertion (needs transformation from ROS frame to NED) +# For the test to be successful it needs to cross a certain boundary in time. # class MavrosOffboardAttctlTest(unittest.TestCase): def setUp(self): rospy.init_node('test_node', anonymous=True) - rospy.wait_for_service('mavros/cmd/arming', 30) - rospy.Subscriber('px4_multicopter/vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback) - rospy.Subscriber("mavros/position/local", PoseStamped, self.position_callback) - self.pubAtt = rospy.Publisher('mavros/setpoint/attitude', PoseStamped, queue_size=10) - self.pubThr = rospy.Publisher('mavros/setpoint/att_throttle', Float64, queue_size=10) - self.cmdArm = rospy.ServiceProxy("mavros/cmd/arming", CommandBool) + rospy.wait_for_service('iris/mavros/cmd/arming', 30) + self.helper = PX4TestHelper("mavros_offboard_attctl_test") + self.helper.setUp() + + rospy.Subscriber('iris/vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback) + rospy.Subscriber("iris/mavros/position/local", PoseStamped, self.position_callback) + self.pub_att = rospy.Publisher('iris/mavros/setpoint/attitude', PoseStamped, queue_size=10) + self.pub_thr = rospy.Publisher('iris/mavros/setpoint/att_throttle', Float64, queue_size=10) self.rate = rospy.Rate(10) # 10hz - self.rateSec = rospy.Rate(1) - self.hasPos = False - self.controlMode = vehicle_control_mode() + self.has_pos = False + self.control_mode = vehicle_control_mode() + self.local_position = PoseStamped() + + def tearDown(self): + self.helper.tearDown() # # General callback functions used in tests # def position_callback(self, data): - self.hasPos = True - self.localPosition = data + self.has_pos = True + self.local_position = data def vehicle_control_mode_callback(self, data): - self.controlMode = data - - - # - # Helper methods - # - def arm(self): - return self.cmdArm(value=True) + self.control_mode = data # # Test offboard position control # def test_attctl(self): - # FIXME: this must go ASAP when arming is implemented - manIn = ManualInput() - manIn.arm() - manIn.offboard_attctl() - - self.assertTrue(self.arm(), "Could not arm") - self.rateSec.sleep() - self.rateSec.sleep() - self.assertTrue(self.controlMode.flag_armed, "flag_armed is not set after 2 seconds") - # set some attitude and thrust att = PoseStamped() - att.header = Header() + att.header = Header() att.header.frame_id = "base_footprint" att.header.stamp = rospy.Time.now() quaternion = quaternion_from_euler(0.15, 0.15, 0) @@ -121,21 +105,27 @@ class MavrosOffboardAttctlTest(unittest.TestCase): # does it cross expected boundaries in X seconds? count = 0 timeout = 120 - while(count < timeout): + while count < timeout: # update timestamp for each published SP att.header.stamp = rospy.Time.now() - self.pubAtt.publish(att) - self.pubThr.publish(throttle) - - if (self.localPosition.pose.position.x > 5 - and self.localPosition.pose.position.z > 5 - and self.localPosition.pose.position.y < -5): + + self.pub_att.publish(att) + self.helper.bag_write('mavros/setpoint/attitude', att) + self.pub_thr.publish(throttle) + self.helper.bag_write('mavros/setpoint/att_throttle', throttle) + self.rate.sleep() + + if (self.local_position.pose.position.x > 5 + and self.local_position.pose.position.z > 5 + and self.local_position.pose.position.y < -5): break count = count + 1 - self.rate.sleep() + self.assertTrue(self.control_mode.flag_armed, "flag_armed is not set") + self.assertTrue(self.control_mode.flag_control_attitude_enabled, "flag_control_attitude_enabled is not set") + self.assertTrue(self.control_mode.flag_control_offboard_enabled, "flag_control_offboard_enabled is not set") self.assertTrue(count < timeout, "took too long to cross boundaries") - + if __name__ == '__main__': import rostest diff --git a/integrationtests/demo_tests/mavros_offboard_posctl_test.py b/integrationtests/demo_tests/mavros_offboard_posctl_test.py index a3739ae5c..1383cd04c 100755 --- a/integrationtests/demo_tests/mavros_offboard_posctl_test.py +++ b/integrationtests/demo_tests/mavros_offboard_posctl_test.py @@ -37,21 +37,21 @@ # PKG = 'px4' -import sys import unittest import rospy import math +import rosbag from numpy import linalg import numpy as np from px4.msg import vehicle_control_mode +from px4.msg import vehicle_local_position +from px4.msg import vehicle_local_position_setpoint from std_msgs.msg import Header from geometry_msgs.msg import PoseStamped, Quaternion from tf.transformations import quaternion_from_euler -from mavros.srv import CommandBool - -from manual_input import ManualInput +from px4_test_helper import PX4TestHelper # # Tests flying a path in offboard control by sending position setpoints @@ -64,43 +64,53 @@ class MavrosOffboardPosctlTest(unittest.TestCase): def setUp(self): rospy.init_node('test_node', anonymous=True) - rospy.wait_for_service('mavros/cmd/arming', 30) - rospy.Subscriber('px4_multicopter/vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback) - rospy.Subscriber("mavros/position/local", PoseStamped, self.position_callback) - self.pubSpt = rospy.Publisher('mavros/setpoint/local_position', PoseStamped, queue_size=10) - self.cmdArm = rospy.ServiceProxy("mavros/cmd/arming", CommandBool) + self.helper = PX4TestHelper("mavros_offboard_posctl_test") + self.helper.setUp() + + rospy.Subscriber('iris/vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback) + rospy.Subscriber("iris/mavros/position/local", PoseStamped, self.position_callback) + self.pub_spt = rospy.Publisher('iris/mavros/setpoint/local_position', PoseStamped, queue_size=10) self.rate = rospy.Rate(10) # 10hz - self.rateSec = rospy.Rate(1) - self.hasPos = False - self.controlMode = vehicle_control_mode() + self.has_pos = False + self.local_position = PoseStamped() + self.control_mode = vehicle_control_mode() + + def tearDown(self): + self.helper.tearDown() # # General callback functions used in tests # def position_callback(self, data): - self.hasPos = True - self.localPosition = data + self.has_pos = True + self.local_position = data def vehicle_control_mode_callback(self, data): - self.controlMode = data + self.control_mode = data # # Helper methods # def is_at_position(self, x, y, z, offset): - if(not self.hasPos): + if not self.has_pos: return False - rospy.logdebug("current position %f, %f, %f" % (self.localPosition.pose.position.x, self.localPosition.pose.position.y, self.localPosition.pose.position.z)) + rospy.logdebug("current position %f, %f, %f" % + (self.local_position.pose.position.x, + self.local_position.pose.position.y, + self.local_position.pose.position.z)) + desired = np.array((x, y, z)) - pos = np.array((self.localPosition.pose.position.x, self.localPosition.pose.position.y, self.localPosition.pose.position.z)) + pos = np.array((self.local_position.pose.position.x, + self.local_position.pose.position.y, + self.local_position.pose.position.z)) return linalg.norm(desired - pos) < offset def reach_position(self, x, y, z, timeout): # set a position setpoint pos = PoseStamped() - pos.header = Header() + pos.header = Header() pos.header.frame_id = "base_footprint" pos.pose.position.x = x pos.pose.position.y = y @@ -114,59 +124,47 @@ class MavrosOffboardPosctlTest(unittest.TestCase): # does it reach the position in X seconds? count = 0 - while(count < timeout): + while count < timeout: # update timestamp for each published SP pos.header.stamp = rospy.Time.now() - self.pubSpt.publish(pos) - - if(self.is_at_position(pos.pose.position.x, pos.pose.position.y, pos.pose.position.z, 0.5)): + self.pub_spt.publish(pos) + self.helper.bag_write('mavros/setpoint/local_position', pos) + + if self.is_at_position(pos.pose.position.x, pos.pose.position.y, pos.pose.position.z, 0.5): break count = count + 1 self.rate.sleep() self.assertTrue(count < timeout, "took too long to get to position") - def arm(self): - return self.cmdArm(value=True) - # # Test offboard position control # def test_posctl(self): - # FIXME: this must go ASAP when arming is implemented - manIn = ManualInput() - manIn.arm() - manIn.offboard_posctl() - - self.assertTrue(self.arm(), "Could not arm") - self.rateSec.sleep() - self.rateSec.sleep() - self.assertTrue(self.controlMode.flag_armed, "flag_armed is not set after 2 seconds") - # prepare flight path positions = ( - (0,0,0), - (2,2,2), - (2,-2,2), - (-2,-2,2), - (2,2,2)) + (0, 0, 0), + (2, 2, 2), + (2, -2, 2), + (-2, -2, 2), + (2, 2, 2)) for i in range(0, len(positions)): self.reach_position(positions[i][0], positions[i][1], positions[i][2], 120) - - # does it hold the position for Y seconds? - positionHeld = True + count = 0 timeout = 50 - while(count < timeout): - if(not self.is_at_position(2, 2, 2, 0.5)): - positionHeld = False + while count < timeout: + if not self.is_at_position(2, 2, 2, 0.5): break count = count + 1 self.rate.sleep() + self.assertTrue(self.control_mode.flag_armed, "flag_armed is not set") + self.assertTrue(self.control_mode.flag_control_position_enabled, "flag_control_position_enabled is not set") + self.assertTrue(self.control_mode.flag_control_offboard_enabled, "flag_control_offboard_enabled is not set") self.assertTrue(count == timeout, "position could not be held") - + if __name__ == '__main__': import rostest diff --git a/integrationtests/demo_tests/mavros_tests.launch b/integrationtests/demo_tests/mavros_tests.launch index 4651f0dc9..cc4918307 100644 --- a/integrationtests/demo_tests/mavros_tests.launch +++ b/integrationtests/demo_tests/mavros_tests.launch @@ -2,8 +2,9 @@ <arg name="headless" default="true"/> <arg name="gui" default="false"/> <arg name="enable_logging" default="false"/> - <arg name="enable_ground_truth" default="false"/> - <arg name="log_file" default="iris"/> + <arg name="enable_ground_truth" default="true"/> + <arg name="ns" default="iris"/> + <arg name="log_file" default="$(arg ns)"/> <include file="$(find px4)/launch/gazebo_iris_empty_world.launch"> <arg name="headless" value="$(arg headless)"/> @@ -11,8 +12,11 @@ <arg name="enable_logging" value="$(arg enable_logging)" /> <arg name="enable_ground_truth" value="$(arg enable_ground_truth)" /> <arg name="log_file" value="$(arg log_file)"/> + <arg name="ns" value="$(arg ns)"/> + </include> + <include file="$(find px4)/launch/mavros_sitl.launch"> + <arg name="ns" value="$(arg ns)"/> </include> - <include file="$(find px4)/launch/mavros_sitl.launch" /> <test test-name="mavros_offboard_posctl_test" pkg="px4" type="mavros_offboard_posctl_test.py" /> <test test-name="mavros_offboard_attctl_test" pkg="px4" type="mavros_offboard_attctl_test.py" /> diff --git a/integrationtests/demo_tests/px4_test_helper.py b/integrationtests/demo_tests/px4_test_helper.py new file mode 100644 index 000000000..4dc886634 --- /dev/null +++ b/integrationtests/demo_tests/px4_test_helper.py @@ -0,0 +1,111 @@ +#!/usr/bin/env python +#*************************************************************************** +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +#***************************************************************************/ + +# +# @author Andreas Antener <andreas@uaventure.com> +# +PKG = 'px4' + +import unittest +import rospy +import rosbag + +from px4.msg import vehicle_local_position +from px4.msg import vehicle_attitude_setpoint +from px4.msg import vehicle_attitude +from px4.msg import vehicle_local_position_setpoint + +from threading import Condition + +# +# Test helper +# +class PX4TestHelper(object): + + def __init__(self, test_name): + self.test_name = test_name + + def setUp(self): + self.condition = Condition() + self.closed = False + + rospy.init_node('test_node', anonymous=True) + self.bag = rosbag.Bag(self.test_name + '.bag', 'w', compression="lz4") + + self.sub_vlp = rospy.Subscriber("iris/vehicle_local_position", + vehicle_local_position, self.vehicle_position_callback) + self.sub_vasp = rospy.Subscriber("iris/vehicle_attitude_setpoint", + vehicle_attitude_setpoint, self.vehicle_attitude_setpoint_callback) + self.sub_va = rospy.Subscriber("iris/vehicle_attitude", + vehicle_attitude, self.vehicle_attitude_callback) + self.sub_vlps = rospy.Subscriber("iris/vehicle_local_position_setpoint", + vehicle_local_position_setpoint, self.vehicle_local_position_setpoint_callback) + + + def tearDown(self): + try: + self.condition.acquire() + self.closed = True + + self.sub_vlp.unregister() + self.sub_vasp.unregister() + self.sub_va.unregister() + self.sub_vlps.unregister() + self.bag.close() + + finally: + self.condition.release() + + def vehicle_position_callback(self, data): + self.bag_write('px4/vehicle_local_position', data) + + def vehicle_attitude_setpoint_callback(self, data): + self.bag_write('px4/vehicle_attitude_setpoint', data) + + def vehicle_attitude_callback(self, data): + self.bag_write('px4/vehicle_attitude', data) + + def vehicle_local_position_setpoint_callback(self, data): + self.bag_write('px4/vehicle_local_position_setpoint', data) + + def bag_write(self, topic, data): + try: + self.condition.acquire() + if not self.closed: + self.bag.write(topic, data) + else: + rospy.logwarn("Trying to write to bag but it's already closed") + finally: + self.condition.release() + diff --git a/launch/ardrone.launch b/launch/ardrone.launch index 3173e7cf1..56030dd1f 100644 --- a/launch/ardrone.launch +++ b/launch/ardrone.launch @@ -1,14 +1,19 @@ <launch> +<arg name="ns"/> -<include file="$(find px4)/launch/multicopter_x.launch" /> +<include file="$(find px4)/launch/multicopter_x.launch"> + <arg name="ns" value="$(arg ns)"/> +</include> -<group ns="px4_multicopter"> +<group ns="$(arg ns)"> <param name="MPC_XY_P" type="double" value="1.0" /> - <param name="MPC_XY_FF" type="double" value="0.0" /> + <param name="MPC_XY_FF" type="double" value="0.1" /> <param name="MPC_XY_VEL_P" type="double" value="0.01" /> <param name="MPC_XY_VEL_I" type="double" value="0.0" /> <param name="MPC_XY_VEL_D" type="double" value="0.01" /> <param name="MPC_XY_VEL_MAX" type="double" value="2.0" /> + <param name="MPC_Z_VEL_P" type="double" value="0.3" /> + <param name="MPC_Z_P" type="double" value="2" /> <param name="vehicle_model" type="string" value="ardrone" /> </group> diff --git a/launch/gazebo_ardrone_empty_world.launch b/launch/gazebo_ardrone_empty_world.launch index 22bfb0c79..7243e7476 100644 --- a/launch/gazebo_ardrone_empty_world.launch +++ b/launch/gazebo_ardrone_empty_world.launch @@ -3,16 +3,19 @@ <arg name="headless" default="false"/> <arg name="gui" default="true"/> <arg name="enable_logging" default="false"/> - <arg name="enable_ground_truth" default="false"/> - <arg name="log_file" default="ardrone"/> + <arg name="enable_ground_truth" default="true"/> + <arg name="ns" default="ardrone"/> + <arg name="log_file" default="$(arg ns)"/> - <include file="$(find rotors_gazebo)/launch/ardrone_empty_world_with_joy.launch"> + <include file="$(find rotors_gazebo)/launch/ardrone_empty_world.launch"> <arg name="headless" value="$(arg headless)"/> <arg name="gui" value="$(arg gui)"/> <arg name="enable_logging" value="$(arg enable_logging)" /> <arg name="enable_ground_truth" value="$(arg enable_ground_truth)" /> <arg name="log_file" value="$(arg log_file)"/> </include> - <include file="$(find px4)/launch/ardrone.launch" /> + <include file="$(find px4)/launch/ardrone.launch"> + <arg name="ns" value="$(arg ns)"/> + </include> </launch> diff --git a/launch/gazebo_ardrone_empty_world_offboard_attitudedemo.launch b/launch/gazebo_ardrone_empty_world_offboard_attitudedemo.launch index 717244abf..1309529b6 100644 --- a/launch/gazebo_ardrone_empty_world_offboard_attitudedemo.launch +++ b/launch/gazebo_ardrone_empty_world_offboard_attitudedemo.launch @@ -1,9 +1,14 @@ - <launch> - -<include file="$(find px4)/launch/gazebo_ardrone_empty_world.launch" /> -<include file="$(find px4)/launch/mavros_sitl.launch" /> - -<node pkg="px4" name="demo_offboard_attitude_setpoints" type="demo_offboard_attitude_setpoints"/> +<arg name="ns" default="ardrone"/> + +<include file="$(find px4)/launch/gazebo_ardrone_empty_world.launch"> + <arg name="ns" value="$(arg ns)"/> +</include> +<include file="$(find px4)/launch/mavros_sitl.launch"> + <arg name="ns" value="$(arg ns)"/> +</include> +<group ns="$(arg ns)"> + <node pkg="px4" name="demo_offboard_attitude_setpoints" type="demo_offboard_attitude_setpoints"/> +</group> </launch> diff --git a/launch/gazebo_ardrone_empty_world_offboard_positiondemo.launch b/launch/gazebo_ardrone_empty_world_offboard_positiondemo.launch index 9ff7f7d1f..0371306ce 100644 --- a/launch/gazebo_ardrone_empty_world_offboard_positiondemo.launch +++ b/launch/gazebo_ardrone_empty_world_offboard_positiondemo.launch @@ -1,9 +1,14 @@ - <launch> - -<include file="$(find px4)/launch/gazebo_ardrone_empty_world.launch" /> -<include file="$(find px4)/launch/mavros_sitl.launch" /> - -<node pkg="px4" name="demo_offboard_position_setpoints" type="demo_offboard_position_setpoints"/> +<arg name="ns" default="ardrone"/> + +<include file="$(find px4)/launch/gazebo_ardrone_empty_world.launch"> + <arg name="ns" value="$(arg ns)"/> +</include> +<include file="$(find px4)/launch/mavros_sitl.launch"> + <arg name="ns" value="$(arg ns)"/> +</include> +<group ns="$(arg ns)"> + <node pkg="px4" name="demo_offboard_position_setpoints" type="demo_offboard_position_setpoints"/> +</group> </launch> diff --git a/launch/gazebo_ardrone_house_world.launch b/launch/gazebo_ardrone_house_world.launch index c636868eb..89f27c901 100644 --- a/launch/gazebo_ardrone_house_world.launch +++ b/launch/gazebo_ardrone_house_world.launch @@ -4,15 +4,18 @@ <arg name="gui" default="true"/> <arg name="enable_logging" default="false"/> <arg name="enable_ground_truth" default="true"/> - <arg name="log_file" default="ardrone"/> + <arg name="ns" default="ardrone"/> + <arg name="log_file" default="$(arg ns)"/> - <include file="$(find rotors_gazebo)/launch/ardrone_house_world_with_joy.launch"> + <include file="$(find rotors_gazebo)/launch/ardrone_house_world.launch"> <arg name="headless" value="$(arg headless)"/> <arg name="gui" value="$(arg gui)"/> <arg name="enable_logging" value="$(arg enable_logging)" /> <arg name="enable_ground_truth" value="$(arg enable_ground_truth)" /> <arg name="log_file" value="$(arg log_file)"/> </include> - <include file="$(find px4)/launch/ardrone.launch" /> + <include file="$(find px4)/launch/ardrone.launch"> + <arg name="ns" value="$(arg ns)"/> + </include> </launch> diff --git a/launch/gazebo_iris_empty_world.launch b/launch/gazebo_iris_empty_world.launch index 8a4c128b9..0e80208e6 100644 --- a/launch/gazebo_iris_empty_world.launch +++ b/launch/gazebo_iris_empty_world.launch @@ -4,15 +4,18 @@ <arg name="gui" default="true"/> <arg name="enable_logging" default="false"/> <arg name="enable_ground_truth" default="true"/> - <arg name="log_file" default="iris"/> + <arg name="ns" default="iris"/> + <arg name="log_file" default="$(arg ns)"/> - <include file="$(find rotors_gazebo)/launch/iris_empty_world_with_joy.launch"> + <include file="$(find rotors_gazebo)/launch/iris_empty_world.launch"> <arg name="headless" value="$(arg headless)"/> <arg name="gui" value="$(arg gui)"/> <arg name="enable_logging" value="$(arg enable_logging)" /> <arg name="enable_ground_truth" value="$(arg enable_ground_truth)" /> <arg name="log_file" value="$(arg log_file)"/> </include> - <include file="$(find px4)/launch/iris.launch" /> + <include file="$(find px4)/launch/iris.launch"> + <arg name="ns" value="$(arg ns)"/> + </include> </launch> diff --git a/launch/gazebo_iris_house_world.launch b/launch/gazebo_iris_house_world.launch index e4f9f6532..d0e824d4b 100644 --- a/launch/gazebo_iris_house_world.launch +++ b/launch/gazebo_iris_house_world.launch @@ -4,7 +4,8 @@ <arg name="gui" default="true"/> <arg name="enable_logging" default="false"/> <arg name="enable_ground_truth" default="true"/> - <arg name="log_file" default="iris"/> + <arg name="ns" default="iris"/> + <arg name="log_file" default="$(arg ns)"/> <include file="$(find rotors_gazebo)/launch/iris_house_world_with_joy.launch"> <arg name="headless" value="$(arg headless)"/> @@ -13,6 +14,8 @@ <arg name="enable_ground_truth" value="$(arg enable_ground_truth)" /> <arg name="log_file" value="$(arg log_file)"/> </include> - <include file="$(find px4)/launch/iris.launch" /> + <include file="$(find px4)/launch/iris.launch"> + <arg name="ns" value="$(arg ns)"/> + </include> </launch> diff --git a/launch/gazebo_iris_outdoor_world.launch b/launch/gazebo_iris_outdoor_world.launch index 22d55502d..e75e57b87 100644 --- a/launch/gazebo_iris_outdoor_world.launch +++ b/launch/gazebo_iris_outdoor_world.launch @@ -4,7 +4,8 @@ <arg name="gui" default="true"/> <arg name="enable_logging" default="false"/> <arg name="enable_ground_truth" default="true"/> - <arg name="log_file" default="iris"/> + <arg name="ns" default="iris"/> + <arg name="log_file" default="$(arg ns)"/> <include file="$(find rotors_gazebo)/launch/iris_outdoor_world_with_joy.launch"> <arg name="headless" value="$(arg headless)"/> @@ -13,6 +14,8 @@ <arg name="enable_ground_truth" value="$(arg enable_ground_truth)" /> <arg name="log_file" value="$(arg log_file)"/> </include> - <include file="$(find px4)/launch/iris.launch" /> + <include file="$(find px4)/launch/iris.launch"> + <arg name="ns" value="$(arg ns)"/> + </include> </launch> diff --git a/launch/iris.launch b/launch/iris.launch index be33cb85f..bf5b099b6 100644 --- a/launch/iris.launch +++ b/launch/iris.launch @@ -1,8 +1,11 @@ <launch> +<arg name="ns"/> -<include file="$(find px4)/launch/multicopter_w.launch" /> +<include file="$(find px4)/launch/multicopter_w.launch"> + <arg name="ns" value="$(arg ns)"/> +</include> -<group ns="px4_multicopter"> +<group ns="$(arg ns)"> <param name="mixer" type="string" value="i" /> <param name="MPC_XY_P" type="double" value="1.0" /> <param name="MPC_XY_FF" type="double" value="0.0" /> @@ -10,6 +13,8 @@ <param name="MPC_XY_VEL_I" type="double" value="0.0" /> <param name="MPC_XY_VEL_D" type="double" value="0.01" /> <param name="MPC_XY_VEL_MAX" type="double" value="2.0" /> + <param name="MPC_Z_VEL_P" type="double" value="0.3" /> + <param name="MPC_Z_P" type="double" value="2" /> <param name="vehicle_model" type="string" value="iris" /> </group> diff --git a/launch/mavros_sitl.launch b/launch/mavros_sitl.launch index 40010a273..7362fa464 100644 --- a/launch/mavros_sitl.launch +++ b/launch/mavros_sitl.launch @@ -1,7 +1,9 @@ <launch> - <!-- vim: set ft=xml noet : --> - <!-- example launch script for PX4 based FCU's --> +<!-- vim: set ft=xml noet : --> +<!-- example launch script for PX4 based FCU's --> +<arg name="ns" default="/" /> +<group ns="$(arg ns)"> <arg name="fcu_url" default="udp://localhost:14560@localhost:14565" /> <arg name="gcs_url" default="" /> <arg name="tgt_system" default="1" /> @@ -18,4 +20,5 @@ <arg name="tgt_system" value="$(arg tgt_system)" /> <arg name="tgt_component" value="$(arg tgt_component)" /> </include> +</group> </launch> diff --git a/launch/multicopter.launch b/launch/multicopter.launch index bc0e37771..6882f2413 100644 --- a/launch/multicopter.launch +++ b/launch/multicopter.launch @@ -1,6 +1,7 @@ <launch> +<arg name="ns"/> -<group ns="px4_multicopter"> +<group ns="$(arg ns)"> <node pkg="joy" name="joy_node" type="joy_node"/> <node pkg="px4" name="manual_input" type="manual_input"/> <node pkg="px4" name="commander" type="commander"/> diff --git a/launch/multicopter_w.launch b/launch/multicopter_w.launch index f124082aa..66c30d186 100644 --- a/launch/multicopter_w.launch +++ b/launch/multicopter_w.launch @@ -1,8 +1,11 @@ <launch> +<arg name="ns"/> -<include file="$(find px4)/launch/multicopter.launch" /> +<include file="$(find px4)/launch/multicopter.launch"> + <arg name="ns" value="$(arg ns)"/> +</include> -<group ns="px4_multicopter"> +<group ns="$(arg ns)"> <param name="mixer" type="string" value="w" /> </group> diff --git a/launch/multicopter_x.launch b/launch/multicopter_x.launch index 6355b87be..c686eba39 100644 --- a/launch/multicopter_x.launch +++ b/launch/multicopter_x.launch @@ -1,8 +1,11 @@ <launch> +<arg name="ns"/> -<include file="$(find px4)/launch/multicopter.launch" /> +<include file="$(find px4)/launch/multicopter.launch"> + <arg name="ns" value="$(arg ns)"/> +</include> -<group ns="px4_multicopter"> +<group ns="$(arg ns)"> <param name="mixer" type="string" value="x" /> </group> diff --git a/makefiles/config_aerocore_default.mk b/makefiles/config_aerocore_default.mk index 0d3934bd0..9ab63a973 100644 --- a/makefiles/config_aerocore_default.mk +++ b/makefiles/config_aerocore_default.mk @@ -27,7 +27,7 @@ MODULES += modules/sensors # # System commands # -MODULES += systemcmds/boardinfo +MODULES += systemcmds/ver MODULES += systemcmds/mixer MODULES += systemcmds/param MODULES += systemcmds/perf diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index f30733694..e3515a927 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -67,9 +67,12 @@ MODULES += modules/land_detector # # Estimation modules (EKF / other filters) # -MODULES += modules/attitude_estimator_ekf +# Too high RAM usage due to static allocations +# MODULES += modules/attitude_estimator_ekf MODULES += modules/ekf_att_pos_estimator -MODULES += modules/position_estimator_inav +# Since attitude_estimator_ekf is disabled, this app won't be +# worthwhile on its own +# MODULES += modules/position_estimator_inav # # Vehicle Control diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index 86925f9ff..324e12696 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -44,12 +44,12 @@ MODULES += modules/sensors MODULES += drivers/mkblctrl MODULES += drivers/px4flow MODULES += drivers/oreoled +MODULES += drivers/gimbal # # System commands # MODULES += systemcmds/bl_update -MODULES += systemcmds/boardinfo MODULES += systemcmds/mixer MODULES += systemcmds/param MODULES += systemcmds/perf diff --git a/makefiles/config_px4fmu-v2_multiplatform.mk b/makefiles/config_px4fmu-v2_multiplatform.mk index 29eb68096..568f940ee 100644 --- a/makefiles/config_px4fmu-v2_multiplatform.mk +++ b/makefiles/config_px4fmu-v2_multiplatform.mk @@ -48,7 +48,6 @@ MODULES += drivers/px4flow # System commands # MODULES += systemcmds/bl_update -MODULES += systemcmds/boardinfo MODULES += systemcmds/mixer MODULES += systemcmds/param MODULES += systemcmds/perf diff --git a/makefiles/config_px4fmu-v2_test.mk b/makefiles/config_px4fmu-v2_test.mk index 22563838b..0d04a1f19 100644 --- a/makefiles/config_px4fmu-v2_test.mk +++ b/makefiles/config_px4fmu-v2_test.mk @@ -42,7 +42,6 @@ MODULES += modules/sensors # System commands # MODULES += systemcmds/bl_update -MODULES += systemcmds/boardinfo MODULES += systemcmds/mixer MODULES += systemcmds/param MODULES += systemcmds/perf diff --git a/msg/vehicle_attitude.msg b/msg/vehicle_attitude.msg index 98018a1df..63879149a 100644 --- a/msg/vehicle_attitude.msg +++ b/msg/vehicle_attitude.msg @@ -4,15 +4,15 @@ uint64 timestamp # in microseconds since system start float32 roll # Roll angle (rad, Tait-Bryan, NED) float32 pitch # Pitch angle (rad, Tait-Bryan, NED) float32 yaw # Yaw angle (rad, Tait-Bryan, NED) -float32 rollspeed # Roll angular speed (rad/s, Tait-Bryan, NED) -float32 pitchspeed # Pitch angular speed (rad/s, Tait-Bryan, NED) -float32 yawspeed # Yaw angular speed (rad/s, Tait-Bryan, NED) -float32 rollacc # Roll angular accelration (rad/s, Tait-Bryan, NED) -float32 pitchacc # Pitch angular acceleration (rad/s, Tait-Bryan, NED) -float32 yawacc # Yaw angular acceleration (rad/s, Tait-Bryan, NED) +float32 rollspeed # Roll body angular rate (rad/s, x forward/y right/z down) +float32 pitchspeed # Pitch body angular rate (rad/s, x forward/y right/z down) +float32 yawspeed # Yaw body angular rate (rad/s, x forward/y right/z down) +float32 rollacc # Roll angular accelration (rad/s^2, x forward/y right/z down) +float32 pitchacc # Pitch angular acceleration (rad/s^2, x forward/y right/z down) +float32 yawacc # Yaw angular acceleration (rad/s^2, x forward/y right/z down) float32[3] rate_offsets # Offsets of the body angular rates from zero float32[9] R # Rotation matrix, body to world, (Tait-Bryan, NED) float32[4] q # Quaternion (NED) -float32[3] g_comp # Compensated gravity vector +float32[3] g_comp # Compensated gravity vector bool R_valid # Rotation matrix valid bool q_valid # Quaternion valid diff --git a/nuttx-configs/aerocore/include/board.h b/nuttx-configs/aerocore/include/board.h index 8705c1bc2..6dc85735a 100644 --- a/nuttx-configs/aerocore/include/board.h +++ b/nuttx-configs/aerocore/include/board.h @@ -145,7 +145,7 @@ #define STM32_APB2_TIM11_CLKIN (2*STM32_PCLK2_FREQUENCY) /* Timer Frequencies, if APBx is set to 1, frequency is same to APBx - * otherwise frequency is 2xAPBx. + * otherwise frequency is 2xAPBx. * Note: TIM1,8 are on APB2, others on APB1 */ @@ -203,7 +203,7 @@ /* * SPI */ -/* PA[4-7] SPI1 broken out on J12 */ +/* PA[4-7] SPI1 broken out on J12 */ #define GPIO_SPI1_NSS (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN4) /* should be GPIO_SPI1_NSS_2 but use as a GPIO */ #define GPIO_SPI1_SCK (GPIO_SPI1_SCK_1|GPIO_SPEED_50MHz) #define GPIO_SPI1_MISO (GPIO_SPI1_MISO_1|GPIO_SPEED_50MHz) diff --git a/nuttx-configs/aerocore/src/empty.c b/nuttx-configs/aerocore/src/empty.c index ace900866..5de10699f 100644 --- a/nuttx-configs/aerocore/src/empty.c +++ b/nuttx-configs/aerocore/src/empty.c @@ -1,4 +1,4 @@ /* - * There are no source files here, but libboard.a can't be empty, so + * There are no source files here, but libboard.a can't be empty, so * we have this empty source file to keep it company. */ diff --git a/nuttx-configs/px4fmu-v1/include/board.h b/nuttx-configs/px4fmu-v1/include/board.h index ff1c63424..9e6d50a8b 100644 --- a/nuttx-configs/px4fmu-v1/include/board.h +++ b/nuttx-configs/px4fmu-v1/include/board.h @@ -209,7 +209,7 @@ /* UART DMA configuration for USART1/6 */ #define DMAMAP_USART1_RX DMAMAP_USART1_RX_2 #define DMAMAP_USART6_RX DMAMAP_USART6_RX_2 - + /* * CAN * diff --git a/nuttx-configs/px4fmu-v1/src/empty.c b/nuttx-configs/px4fmu-v1/src/empty.c index ace900866..5de10699f 100644 --- a/nuttx-configs/px4fmu-v1/src/empty.c +++ b/nuttx-configs/px4fmu-v1/src/empty.c @@ -1,4 +1,4 @@ /* - * There are no source files here, but libboard.a can't be empty, so + * There are no source files here, but libboard.a can't be empty, so * we have this empty source file to keep it company. */ diff --git a/nuttx-configs/px4fmu-v2/include/board.h b/nuttx-configs/px4fmu-v2/include/board.h index 3b3c6fa70..52668cacd 100755 --- a/nuttx-configs/px4fmu-v2/include/board.h +++ b/nuttx-configs/px4fmu-v2/include/board.h @@ -146,21 +146,21 @@ #define STM32_APB2_TIM11_CLKIN (2*STM32_PCLK2_FREQUENCY) /* Timer Frequencies, if APBx is set to 1, frequency is same to APBx - * otherwise frequency is 2xAPBx. + * otherwise frequency is 2xAPBx. * Note: TIM1,8 are on APB2, others on APB1 */ #define STM32_TIM18_FREQUENCY (2*STM32_PCLK2_FREQUENCY) #define STM32_TIM27_FREQUENCY (2*STM32_PCLK1_FREQUENCY) -/* SDIO dividers. Note that slower clocking is required when DMA is disabled +/* SDIO dividers. Note that slower clocking is required when DMA is disabled * in order to avoid RX overrun/TX underrun errors due to delayed responses * to service FIFOs in interrupt driven mode. These values have not been * tuned!!! * * HCLK=72MHz, SDIOCLK=72MHz, SDIO_CK=HCLK/(178+2)=400 KHz */ - + #define SDIO_INIT_CLKDIV (178 << SDIO_CLKCR_CLKDIV_SHIFT) /* DMA ON: HCLK=72 MHz, SDIOCLK=72MHz, SDIO_CK=HCLK/(2+2)=18 MHz @@ -168,9 +168,9 @@ */ #ifdef CONFIG_SDIO_DMA -# define SDIO_MMCXFR_CLKDIV (2 << SDIO_CLKCR_CLKDIV_SHIFT) +# define SDIO_MMCXFR_CLKDIV (2 << SDIO_CLKCR_CLKDIV_SHIFT) #else -# define SDIO_MMCXFR_CLKDIV (3 << SDIO_CLKCR_CLKDIV_SHIFT) +# define SDIO_MMCXFR_CLKDIV (3 << SDIO_CLKCR_CLKDIV_SHIFT) #endif /* DMA ON: HCLK=72 MHz, SDIOCLK=72MHz, SDIO_CK=HCLK/(1+2)=24 MHz @@ -227,10 +227,10 @@ #define DMAMAP_USART1_RX DMAMAP_USART1_RX_2 #define DMAMAP_USART6_RX DMAMAP_USART6_RX_2 -/* +/* * CAN * - * CAN1 is routed to the onboard transceiver. + * CAN1 is routed to the onboard transceiver. * CAN2 is routed to the expansion connector. */ #define GPIO_CAN1_RX GPIO_CAN1_RX_3 diff --git a/nuttx-configs/px4fmu-v2/scripts/ld.script b/nuttx-configs/px4fmu-v2/scripts/ld.script index bec896d1c..b04ad89a6 100644 --- a/nuttx-configs/px4fmu-v2/scripts/ld.script +++ b/nuttx-configs/px4fmu-v2/scripts/ld.script @@ -66,12 +66,20 @@ EXTERN(_vectors) /* force the vectors to be included in the output */ * code pulled in by libgcc.a requires it (and that code cannot be easily avoided). */ EXTERN(abort) +EXTERN(_bootdelay_signature) SECTIONS { .text : { _stext = ABSOLUTE(.); *(.vectors) + . = ALIGN(32); + /* + This signature provides the bootloader with a way to delay booting + */ + _bootdelay_signature = ABSOLUTE(.); + FILL(0xffecc2925d7d05c5) + . += 8; *(.text .text.*) *(.fixup) *(.gnu.warning) diff --git a/nuttx-configs/px4fmu-v2/src/empty.c b/nuttx-configs/px4fmu-v2/src/empty.c index ace900866..5de10699f 100644 --- a/nuttx-configs/px4fmu-v2/src/empty.c +++ b/nuttx-configs/px4fmu-v2/src/empty.c @@ -1,4 +1,4 @@ /* - * There are no source files here, but libboard.a can't be empty, so + * There are no source files here, but libboard.a can't be empty, so * we have this empty source file to keep it company. */ diff --git a/nuttx-configs/px4io-v1/include/board.h b/nuttx-configs/px4io-v1/include/board.h index 815079266..ee8a9d1ba 100755 --- a/nuttx-configs/px4io-v1/include/board.h +++ b/nuttx-configs/px4io-v1/include/board.h @@ -143,7 +143,7 @@ extern "C" { ************************************************************************************/ EXTERN void stm32_boardinitialize(void); - + #if defined(__cplusplus) } #endif diff --git a/nuttx-configs/px4io-v1/src/empty.c b/nuttx-configs/px4io-v1/src/empty.c index ace900866..5de10699f 100644 --- a/nuttx-configs/px4io-v1/src/empty.c +++ b/nuttx-configs/px4io-v1/src/empty.c @@ -1,4 +1,4 @@ /* - * There are no source files here, but libboard.a can't be empty, so + * There are no source files here, but libboard.a can't be empty, so * we have this empty source file to keep it company. */ diff --git a/nuttx-configs/px4io-v2/include/board.h b/nuttx-configs/px4io-v2/include/board.h index 17e77918b..b70056f82 100755 --- a/nuttx-configs/px4io-v2/include/board.h +++ b/nuttx-configs/px4io-v2/include/board.h @@ -138,7 +138,7 @@ extern "C" { ************************************************************************************/ EXTERN void stm32_boardinitialize(void); - + #if defined(__cplusplus) } #endif diff --git a/nuttx-configs/px4io-v2/src/empty.c b/nuttx-configs/px4io-v2/src/empty.c index ace900866..5de10699f 100644 --- a/nuttx-configs/px4io-v2/src/empty.c +++ b/nuttx-configs/px4io-v2/src/empty.c @@ -1,4 +1,4 @@ /* - * There are no source files here, but libboard.a can't be empty, so + * There are no source files here, but libboard.a can't be empty, so * we have this empty source file to keep it company. */ diff --git a/src/drivers/batt_smbus/batt_smbus.cpp b/src/drivers/batt_smbus/batt_smbus.cpp index a958fc60d..9f72bd56f 100644 --- a/src/drivers/batt_smbus/batt_smbus.cpp +++ b/src/drivers/batt_smbus/batt_smbus.cpp @@ -84,6 +84,7 @@ #define BATT_SMBUS_ADDR 0x0B ///< I2C address #define BATT_SMBUS_TEMP 0x08 ///< temperature register #define BATT_SMBUS_VOLTAGE 0x09 ///< voltage register +#define BATT_SMBUS_REMAINING_CAPACITY 0x0f ///< predicted remaining battery capacity as a percentage #define BATT_SMBUS_DESIGN_CAPACITY 0x18 ///< design capacity register #define BATT_SMBUS_DESIGN_VOLTAGE 0x19 ///< design voltage register #define BATT_SMBUS_SERIALNUM 0x1c ///< serial number register @@ -179,6 +180,7 @@ private: orb_advert_t _batt_topic; ///< uORB battery topic orb_id_t _batt_orb_id; ///< uORB battery topic ID uint64_t _start_time; ///< system time we first attempt to communicate with battery + uint16_t _batt_design_capacity; ///< battery's design capacity in mAh (0 means unknown) }; namespace @@ -197,7 +199,8 @@ BATT_SMBUS::BATT_SMBUS(int bus, uint16_t batt_smbus_addr) : _reports(nullptr), _batt_topic(-1), _batt_orb_id(nullptr), - _start_time(0) + _start_time(0), + _batt_design_capacity(0) { // work_cancel in the dtor will explode if we don't do this... memset(&_work, 0, sizeof(_work)); @@ -263,7 +266,7 @@ BATT_SMBUS::test() if (updated) { if (orb_copy(ORB_ID(battery_status), sub, &status) == OK) { - warnx("V=%4.2f C=%4.2f", status.voltage_v, status.current_a); + warnx("V=%4.2f C=%4.2f DismAh=%4.2f", (float)status.voltage_v, (float)status.current_a, (float)status.discharged_mah); } } @@ -279,6 +282,7 @@ BATT_SMBUS::search() { bool found_slave = false; uint16_t tmp; + int16_t orig_addr = get_address(); // search through all valid SMBus addresses for (uint8_t i = BATT_SMBUS_ADDR_MIN; i <= BATT_SMBUS_ADDR_MAX; i++) { @@ -293,6 +297,9 @@ BATT_SMBUS::search() usleep(1); } + // restore original i2c address + set_address(orig_addr); + // display completion message if (found_slave) { warnx("Done."); @@ -364,13 +371,28 @@ BATT_SMBUS::cycle() new_report.voltage_v = ((float)tmp) / 1000.0f; // read current - usleep(1); uint8_t buff[4]; if (read_block(BATT_SMBUS_CURRENT, buff, 4, false) == 4) { new_report.current_a = -(float)((int32_t)((uint32_t)buff[3] << 24 | (uint32_t)buff[2] << 16 | (uint32_t)buff[1] << 8 | (uint32_t)buff[0])) / 1000.0f; } + // read battery design capacity + if (_batt_design_capacity == 0) { + if (read_reg(BATT_SMBUS_DESIGN_CAPACITY, tmp) == OK) { + _batt_design_capacity = tmp; + } + } + + // read remaining capacity + if (_batt_design_capacity > 0) { + if (read_reg(BATT_SMBUS_REMAINING_CAPACITY, tmp) == OK) { + if (tmp < _batt_design_capacity) { + new_report.discharged_mah = _batt_design_capacity - tmp; + } + } + } + // publish to orb if (_batt_topic != -1) { orb_publish(_batt_orb_id, _batt_topic, &new_report); @@ -430,8 +452,6 @@ BATT_SMBUS::read_block(uint8_t reg, uint8_t *data, uint8_t max_len, bool append_ { uint8_t buff[max_len + 2]; // buffer to hold results - usleep(1); - // read bytes including PEC int ret = transfer(®, 1, buff, max_len + 2); diff --git a/src/drivers/boards/aerocore/board_config.h b/src/drivers/boards/aerocore/board_config.h index 776a2071e..19518d73d 100644 --- a/src/drivers/boards/aerocore/board_config.h +++ b/src/drivers/boards/aerocore/board_config.h @@ -54,7 +54,7 @@ __BEGIN_DECLS #include <arch/board/board.h> #define UDID_START 0x1FFF7A10 - + /**************************************************************************************************** * Definitions ****************************************************************************************************/ diff --git a/src/drivers/boards/px4fmu-v1/board_config.h b/src/drivers/boards/px4fmu-v1/board_config.h index 882ec6aa2..17fa9c542 100644 --- a/src/drivers/boards/px4fmu-v1/board_config.h +++ b/src/drivers/boards/px4fmu-v1/board_config.h @@ -52,7 +52,7 @@ __BEGIN_DECLS /* these headers are not C++ safe */ #include <stm32.h> #include <arch/board/board.h> - + /**************************************************************************************************** * Definitions ****************************************************************************************************/ diff --git a/src/drivers/boards/px4fmu-v2/board_config.h b/src/drivers/boards/px4fmu-v2/board_config.h index 273af1023..6188e29ae 100644 --- a/src/drivers/boards/px4fmu-v2/board_config.h +++ b/src/drivers/boards/px4fmu-v2/board_config.h @@ -54,7 +54,7 @@ __BEGIN_DECLS #include <arch/board/board.h> #define UDID_START 0x1FFF7A10 - + /**************************************************************************************************** * Definitions ****************************************************************************************************/ diff --git a/src/drivers/boards/px4io-v1/px4io_init.c b/src/drivers/boards/px4io-v1/px4io_init.c index 8292da9e1..5a02a9716 100644 --- a/src/drivers/boards/px4io-v1/px4io_init.c +++ b/src/drivers/boards/px4io-v1/px4io_init.c @@ -87,7 +87,7 @@ __EXPORT void stm32_boardinitialize(void) stm32_configgpio(GPIO_RELAY1_EN); stm32_configgpio(GPIO_RELAY2_EN); - /* turn off - all leds are active low */ + /* turn off - all leds are active low */ stm32_gpiowrite(GPIO_LED1, true); stm32_gpiowrite(GPIO_LED2, true); stm32_gpiowrite(GPIO_LED3, true); diff --git a/src/drivers/boards/px4io-v2/px4iov2_init.c b/src/drivers/boards/px4io-v2/px4iov2_init.c index 5c3343ccc..fd7eb33c3 100644 --- a/src/drivers/boards/px4io-v2/px4iov2_init.c +++ b/src/drivers/boards/px4io-v2/px4iov2_init.c @@ -126,7 +126,7 @@ __EXPORT void stm32_boardinitialize(void) stm32_configgpio(GPIO_SBUS_INPUT); /* xxx alternate function */ stm32_configgpio(GPIO_SBUS_OUTPUT); - + /* sbus output enable is active low - disable it by default */ stm32_gpiowrite(GPIO_SBUS_OENABLE, true); stm32_configgpio(GPIO_SBUS_OENABLE); diff --git a/src/drivers/drv_airspeed.h b/src/drivers/drv_airspeed.h index 2ff91d5d0..fddef3626 100644 --- a/src/drivers/drv_airspeed.h +++ b/src/drivers/drv_airspeed.h @@ -35,7 +35,7 @@ * @file drv_airspeed.h * * Airspeed driver interface. - * + * * @author Simon Wilks */ diff --git a/src/drivers/drv_blinkm.h b/src/drivers/drv_blinkm.h index 7258c9e84..8568436d3 100644 --- a/src/drivers/drv_blinkm.h +++ b/src/drivers/drv_blinkm.h @@ -60,7 +60,7 @@ /** play the numbered script in (arg), repeating forever */ #define BLINKM_PLAY_SCRIPT _BLINKMIOC(2) -/** +/** * Set the user script; (arg) is a pointer to an array of script lines, * where each line is an array of four bytes giving <duration>, <command>, arg[0-2] * diff --git a/src/drivers/drv_orb_dev.h b/src/drivers/drv_orb_dev.h index e0b16fa5c..c1db6b534 100644 --- a/src/drivers/drv_orb_dev.h +++ b/src/drivers/drv_orb_dev.h @@ -36,7 +36,7 @@ /** * @file drv_orb_dev.h - * + * * uORB published object driver. */ diff --git a/src/drivers/drv_oreoled.h b/src/drivers/drv_oreoled.h index 0dcb10a7b..00e368318 100644 --- a/src/drivers/drv_oreoled.h +++ b/src/drivers/drv_oreoled.h @@ -117,7 +117,7 @@ enum oreoled_macro { OREOLED_PARAM_MACRO_ENUM_COUNT }; -/* +/* structure passed to OREOLED_SET_RGB ioctl() Note that the driver scales the brightness to 0 to 255, regardless of the hardware scaling diff --git a/src/drivers/drv_pwm_input.h b/src/drivers/drv_pwm_input.h index 778d9fcf5..47b84e6e1 100644 --- a/src/drivers/drv_pwm_input.h +++ b/src/drivers/drv_pwm_input.h @@ -46,6 +46,6 @@ __BEGIN_DECLS /* * Initialise the timer */ -__EXPORT extern int pwm_input_main(int argc, char * argv[]); +__EXPORT extern int pwm_input_main(int argc, char *argv[]); __END_DECLS diff --git a/src/drivers/gimbal/gimbal.cpp b/src/drivers/gimbal/gimbal.cpp new file mode 100644 index 000000000..1e27309d8 --- /dev/null +++ b/src/drivers/gimbal/gimbal.cpp @@ -0,0 +1,586 @@ +/**************************************************************************** + * + * Copyright (c) 2014, 2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file gimbal.cpp + * @author Lorenz Meier <lm@inf.ethz.ch> + * @author Anton Matosov <anton.matosov@gmail.com> + * + * Driver to control a gimbal - relies on input via telemetry or RC + * and output via the standardized control group #2 and a mixer. + */ + +#include <nuttx/config.h> + +#include <sys/types.h> +#include <stdint.h> +#include <stdlib.h> +#include <stdbool.h> +#include <semaphore.h> +#include <string.h> +#include <fcntl.h> +#include <poll.h> +#include <errno.h> +#include <stdio.h> +#include <math.h> +#include <unistd.h> +#include <termios.h> + +#include <nuttx/arch.h> +#include <nuttx/wqueue.h> +#include <nuttx/clock.h> + +#include <systemlib/perf_counter.h> +#include <systemlib/err.h> + +#include <drivers/drv_hrt.h> +#include <drivers/drv_range_finder.h> +#include <drivers/device/device.h> +#include <drivers/device/ringbuffer.h> + +#include <uORB/uORB.h> +#include <uORB/topics/vehicle_command.h> +#include <uORB/topics/actuator_controls.h> +#include <uORB/topics/vehicle_attitude.h> + +#include <board_config.h> +#include <mathlib/math/test/test.hpp> +#include <mathlib/math/Quaternion.hpp> + +/* Configuration Constants */ + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +#ifndef CONFIG_SCHED_WORKQUEUE +# error This requires CONFIG_SCHED_WORKQUEUE. +#endif + +#define GIMBAL_DEVICE_PATH "/dev/gimbal" + +#define GIMBAL_UPDATE_INTERVAL (5 * 1000) + +#define GIMBALIOCATTCOMPENSATE 1 + +class Gimbal : public device::CDev +{ +public: + Gimbal(); + virtual ~Gimbal(); + + virtual int init(); + + virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + + /** + * Diagnostics - print some basic information about the driver. + */ + void print_info(); + +protected: + virtual int probe(); + +private: + float _min_distance; + float _max_distance; + work_s _work; + int _vehicle_command_sub; + int _att_sub; + + bool _attitude_compensation_roll; + bool _attitude_compensation_pitch; + bool _attitude_compensation_yaw; + bool _initialized; + bool _control_cmd_set; + bool _config_cmd_set; + + orb_advert_t _actuator_controls_2_topic; + + perf_counter_t _sample_perf; + perf_counter_t _comms_errors; + perf_counter_t _buffer_overflows; + + struct vehicle_command_s _control_cmd; + struct vehicle_command_s _config_cmd; + + /** + * Initialise the automatic measurement state machine and start it. + * + * @note This function is called at open and error time. It might make sense + * to make it more aggressive about resetting the bus in case of errors. + */ + void start(); + + /** + * Stop the automatic measurement state machine. + */ + void stop(); + + /** + * Perform a poll cycle; collect from the previous measurement + * and start a new one. + */ + void cycle(); + + /** + * Static trampoline from the workq context; because we don't have a + * generic workq wrapper yet. + * + * @param arg Instance pointer for the driver that is polling. + */ + static void cycle_trampoline(void *arg); + + +}; + +/* + * Driver 'main' command. + */ +extern "C" __EXPORT int gimbal_main(int argc, char *argv[]); + +Gimbal::Gimbal() : + CDev("Gimbal", GIMBAL_DEVICE_PATH), + _vehicle_command_sub(-1), + _att_sub(-1), + _attitude_compensation_roll(true), + _attitude_compensation_pitch(true), + _attitude_compensation_yaw(true), + _initialized(false), + _actuator_controls_2_topic(-1), + _sample_perf(perf_alloc(PC_ELAPSED, "gimbal_read")), + _comms_errors(perf_alloc(PC_COUNT, "gimbal_comms_errors")), + _buffer_overflows(perf_alloc(PC_COUNT, "gimbal_buffer_overflows")) +{ + // disable debug() calls + _debug_enabled = false; + + // work_cancel in the dtor will explode if we don't do this... + memset(&_work, 0, sizeof(_work)); +} + +Gimbal::~Gimbal() +{ + /* make sure we are truly inactive */ + stop(); + + ::close(_actuator_controls_2_topic); + ::close(_vehicle_command_sub); +} + +int +Gimbal::init() +{ + int ret = ERROR; + + /* do regular cdev init */ + if (CDev::init() != OK) { + goto out; + } + + start(); + ret = OK; + +out: + return ret; +} + +int +Gimbal::probe() +{ + return OK; +} + +int +Gimbal::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + switch (cmd) { + + case GIMBALIOCATTCOMPENSATE: + _attitude_compensation_roll = (arg != 0); + _attitude_compensation_pitch = (arg != 0); + _attitude_compensation_yaw = (arg != 0); + return OK; + + default: + /* give it to the superclass */ + return CDev::ioctl(filp, cmd, arg); + } +} + +ssize_t +Gimbal::read(struct file *filp, char *buffer, size_t buflen) +{ + return 0; +} + +void +Gimbal::start() +{ + /* schedule a cycle to start things */ + work_queue(LPWORK, &_work, (worker_t)&Gimbal::cycle_trampoline, this, 1); +} + +void +Gimbal::stop() +{ + work_cancel(LPWORK, &_work); +} + +void +Gimbal::cycle_trampoline(void *arg) +{ + Gimbal *dev = static_cast<Gimbal *>(arg); + + dev->cycle(); +} + +void +Gimbal::cycle() +{ + if (!_initialized) { + /* get a subscription handle on the vehicle command topic */ + _vehicle_command_sub = orb_subscribe(ORB_ID(vehicle_command)); + + /* get a publication handle on actuator output topic */ + struct actuator_controls_s zero_report; + memset(&zero_report, 0, sizeof(zero_report)); + zero_report.timestamp = hrt_absolute_time(); + _actuator_controls_2_topic = orb_advertise(ORB_ID(actuator_controls_2), &zero_report); + + if (_actuator_controls_2_topic < 0) { + warnx("advert err"); + } + + _initialized = true; + } + + bool updated = false; + + perf_begin(_sample_perf); + + float roll = 0.0f; + float pitch = 0.0f; + float yaw = 0.0f; + + + if (_att_sub < 0) { + _att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + } + + vehicle_attitude_s att; + + orb_copy(ORB_ID(vehicle_attitude), _att_sub, &att); + + if (_attitude_compensation_roll) { + roll = -att.roll; + updated = true; + } + + if (_attitude_compensation_pitch) { + pitch = -att.pitch; + updated = true; + } + + if (_attitude_compensation_yaw) { + yaw = att.yaw; + updated = true; + } + + + struct vehicle_command_s cmd; + + bool cmd_updated; + + orb_check(_vehicle_command_sub, &cmd_updated); + + if (cmd_updated) { + + orb_copy(ORB_ID(vehicle_command), _vehicle_command_sub, &cmd); + + if (cmd.command == VEHICLE_CMD_DO_MOUNT_CONTROL + || cmd.command == VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT) { + + _control_cmd = cmd; + _control_cmd_set = true; + + } else if (cmd.command == VEHICLE_CMD_DO_MOUNT_CONFIGURE) { + + _config_cmd = cmd; + _config_cmd_set = true; + + } + + } + + if (_config_cmd_set) { + + _config_cmd_set = false; + + _attitude_compensation_roll = (int)_config_cmd.param2 == 1; + _attitude_compensation_pitch = (int)_config_cmd.param3 == 1; + _attitude_compensation_yaw = (int)_config_cmd.param4 == 1; + + } + + if (_control_cmd_set) { + + VEHICLE_MOUNT_MODE mountMode = (VEHICLE_MOUNT_MODE)_control_cmd.param7; + debug("control_cmd: %d, mountMode %d | param1: %8.4f param2: %8.4f", _control_cmd.command, mountMode, (double)_control_cmd.param1, (double)_control_cmd.param2); + + if (_control_cmd.command == VEHICLE_CMD_DO_MOUNT_CONTROL && mountMode == VEHICLE_MOUNT_MODE_MAVLINK_TARGETING) { + /* Convert to range -1 ... 1, which corresponds to -180deg ... 180deg */ + roll += 1.0f / M_PI_F * M_DEG_TO_RAD_F * _control_cmd.param1; + pitch += 1.0f / M_PI_F * M_DEG_TO_RAD_F * _control_cmd.param2; + yaw += 1.0f / M_PI_F * M_DEG_TO_RAD_F * _control_cmd.param3; + + updated = true; + } + + if (_control_cmd.command == VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT && mountMode == VEHICLE_MOUNT_MODE_MAVLINK_TARGETING) { + float gimbalDirectionQuat[] = {_control_cmd.param1, _control_cmd.param2, _control_cmd.param3, _control_cmd.param4}; + math::Vector<3> gimablDirectionEuler = math::Quaternion(gimbalDirectionQuat).to_dcm().to_euler(); + + roll += gimablDirectionEuler(0); + pitch += gimablDirectionEuler(1); + yaw += gimablDirectionEuler(2); + + updated = true; + } + + } + + if (updated) { + + struct actuator_controls_s controls; + + // debug("publishing | roll: %8.4f pitch: %8.4f yaw: %8.4f", (double)roll, (double)pitch, (double)yaw); + + /* fill in the final control values */ + controls.timestamp = hrt_absolute_time(); + controls.control[0] = roll; + controls.control[1] = pitch; + controls.control[2] = yaw; + + /* publish it */ + orb_publish(ORB_ID(actuator_controls_2), _actuator_controls_2_topic, &controls); + + } + + /* notify anyone waiting for data */ + poll_notify(POLLIN); + + perf_end(_sample_perf); + + /* schedule a fresh cycle call when the measurement is done */ + work_queue(LPWORK, + &_work, + (worker_t)&Gimbal::cycle_trampoline, + this, + USEC2TICK(GIMBAL_UPDATE_INTERVAL)); +} + +void +Gimbal::print_info() +{ + perf_print_counter(_sample_perf); + perf_print_counter(_comms_errors); + perf_print_counter(_buffer_overflows); +} + +/** + * Local functions in support of the shell command. + */ +namespace gimbal +{ + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +const int ERROR = -1; + +Gimbal *g_dev; + +void start(); +void stop(); +void test(); +void reset(); +void info(); + +/** + * Start the driver. + */ +void +start() +{ + if (g_dev != nullptr) { + errx(1, "already started"); + } + + /* create the driver */ + g_dev = new Gimbal(); + + if (g_dev == nullptr) { + goto fail; + } + + if (OK != g_dev->init()) { + goto fail; + } + + exit(0); + +fail: + + if (g_dev != nullptr) { + delete g_dev; + g_dev = nullptr; + } + + errx(1, "driver start failed"); +} + +/** + * Stop the driver + */ +void stop() +{ + if (g_dev != nullptr) { + delete g_dev; + g_dev = nullptr; + + } else { + errx(1, "driver not running"); + } + + exit(0); +} + +/** + * Perform some basic functional tests on the driver; + * make sure we can collect data from the sensor in polled + * and automatic modes. + */ +void +test() +{ + int fd = open(GIMBAL_DEVICE_PATH, O_RDONLY); + + if (ioctl(fd, GIMBALIOCATTCOMPENSATE, 1) < 0) { + err(1, "failed enabling compensation"); + } + + errx(0, "PASS"); +} + +/** + * Reset the driver. + */ +void +reset() +{ + int fd = open(GIMBAL_DEVICE_PATH, O_RDONLY); + + if (fd < 0) { + err(1, "failed "); + } + + // if (ioctl(fd, GIMBALIOCATTCOMPENSATE, 1) < 0) { + // err(1, "failed enabling compensation"); + // } + + exit(0); +} + +/** + * Print a little info about the driver. + */ +void +info() +{ + if (g_dev == nullptr) { + errx(1, "driver not running"); + } + + printf("state @ %p\n", g_dev); + g_dev->print_info(); + + exit(0); +} + +} // namespace + +int +gimbal_main(int argc, char *argv[]) +{ + /* + * Start/load the driver. + */ + if (!strcmp(argv[1], "start")) { + gimbal::start(); + } + + /* + * Stop the driver + */ + if (!strcmp(argv[1], "stop")) { + gimbal::stop(); + } + + /* + * Test the driver/device. + */ + if (!strcmp(argv[1], "test")) { + gimbal::test(); + } + + /* + * Reset the driver. + */ + if (!strcmp(argv[1], "reset")) { + gimbal::reset(); + } + + /* + * Print driver information. + */ + if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) { + gimbal::info(); + } + + errx(1, "unrecognized command, try 'start', 'stop', 'reset', 'test' or 'info'"); +} diff --git a/src/systemcmds/boardinfo/module.mk b/src/drivers/gimbal/module.mk index 6851d229b..cc0a10c6e 100644 --- a/src/systemcmds/boardinfo/module.mk +++ b/src/drivers/gimbal/module.mk @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. +# Copyright (c) 2014, 2015 PX4 Development Team. All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions @@ -32,10 +32,11 @@ ############################################################################ # -# Information about FMU and IO boards connected +# Makefile to build the gimbal high-level controller # -MODULE_COMMAND = boardinfo -SRCS = boardinfo.c +MODULE_COMMAND = gimbal -MAXOPTIMIZATION = -Os +SRCS = gimbal.cpp + +MAXOPTIMIZATION = -Os diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 7560ef20b..2b3520fc8 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -848,6 +848,10 @@ HMC5883::collect() struct mag_report new_report; bool sensor_is_onboard = false; + float xraw_f; + float yraw_f; + float zraw_f; + /* this should be fairly close to the end of the measurement, so the best approximation of the time */ new_report.timestamp = hrt_absolute_time(); new_report.error_count = perf_event_count(_comms_errors); @@ -907,17 +911,21 @@ HMC5883::collect() report.x = -report.x; } - /* the standard external mag by 3DR has x pointing to the + /* the standard external mag by 3DR has x pointing to the * right, y pointing backwards, and z down, therefore switch x * and y and invert y */ - new_report.x = ((-report.y * _range_scale) - _scale.x_offset) * _scale.x_scale; - /* flip axes and negate value for y */ - new_report.y = ((report.x * _range_scale) - _scale.y_offset) * _scale.y_scale; - /* z remains z */ - new_report.z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale; + xraw_f = -report.y; + yraw_f = report.x; + zraw_f = report.z; // apply user specified rotation - rotate_3f(_rotation, new_report.x, new_report.y, new_report.z); + rotate_3f(_rotation, xraw_f, yraw_f, zraw_f); + + new_report.x = ((xraw_f * _range_scale) - _scale.x_offset) * _scale.x_scale; + /* flip axes and negate value for y */ + new_report.y = ((yraw_f * _range_scale) - _scale.y_offset) * _scale.y_scale; + /* z remains z */ + new_report.z = ((zraw_f * _range_scale) - _scale.z_offset) * _scale.z_scale; if (!(_pub_blocked)) { diff --git a/src/drivers/hmc5883/hmc5883.h b/src/drivers/hmc5883/hmc5883.h index e91e91fc0..9607fe614 100644 --- a/src/drivers/hmc5883/hmc5883.h +++ b/src/drivers/hmc5883/hmc5883.h @@ -50,4 +50,4 @@ /* interface factories */ extern device::Device *HMC5883_SPI_interface(int bus) weak_function; extern device::Device *HMC5883_I2C_interface(int bus) weak_function; -typedef device::Device* (*HMC5883_constructor)(int); +typedef device::Device *(*HMC5883_constructor)(int); diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 4c41491a8..768723640 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -1029,9 +1029,16 @@ L3GD20::measure() report.temperature_raw = raw_report.temp; - report.x = ((report.x_raw * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale; - report.y = ((report.y_raw * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale; - report.z = ((report.z_raw * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale; + float xraw_f = report.x_raw; + float yraw_f = report.y_raw; + float zraw_f = report.z_raw; + + // apply user specified rotation + rotate_3f(_rotation, xraw_f, yraw_f, zraw_f); + + report.x = ((xraw_f * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale; + report.y = ((yraw_f * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale; + report.z = ((zraw_f * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale; report.x = _gyro_filter_x.apply(report.x); report.y = _gyro_filter_y.apply(report.y); @@ -1039,9 +1046,6 @@ L3GD20::measure() report.temperature = L3GD20_TEMP_OFFSET_CELSIUS - raw_report.temp; - // apply user specified rotation - rotate_3f(_rotation, report.x, report.y, report.z); - report.scaling = _gyro_range_scale; report.range_rad_s = _gyro_range_rad_s; diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index b5f01b942..84f7fb5c8 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -1517,9 +1517,16 @@ LSM303D::measure() accel_report.y_raw = raw_accel_report.y; accel_report.z_raw = raw_accel_report.z; - float x_in_new = ((accel_report.x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; - float y_in_new = ((accel_report.y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; - float z_in_new = ((accel_report.z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; + float xraw_f = raw_accel_report.x; + float yraw_f = raw_accel_report.y; + float zraw_f = raw_accel_report.z; + + // apply user specified rotation + rotate_3f(_rotation, xraw_f, yraw_f, zraw_f); + + float x_in_new = ((xraw_f * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; + float y_in_new = ((yraw_f * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; + float z_in_new = ((zraw_f * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; /* we have logs where the accelerometers get stuck at a fixed @@ -1555,9 +1562,6 @@ LSM303D::measure() accel_report.y = _accel_filter_y.apply(y_in_new); accel_report.z = _accel_filter_z.apply(z_in_new); - // apply user specified rotation - rotate_3f(_rotation, accel_report.x, accel_report.y, accel_report.z); - accel_report.scaling = _accel_range_scale; accel_report.range_m_s2 = _accel_range_m_s2; @@ -1623,16 +1627,21 @@ LSM303D::mag_measure() mag_report.x_raw = raw_mag_report.x; mag_report.y_raw = raw_mag_report.y; mag_report.z_raw = raw_mag_report.z; - mag_report.x = ((mag_report.x_raw * _mag_range_scale) - _mag_scale.x_offset) * _mag_scale.x_scale; - mag_report.y = ((mag_report.y_raw * _mag_range_scale) - _mag_scale.y_offset) * _mag_scale.y_scale; - mag_report.z = ((mag_report.z_raw * _mag_range_scale) - _mag_scale.z_offset) * _mag_scale.z_scale; + + float xraw_f = mag_report.x_raw; + float yraw_f = mag_report.y_raw; + float zraw_f = mag_report.z_raw; + + // apply user specified rotation + rotate_3f(_rotation, xraw_f, yraw_f, zraw_f); + + mag_report.x = ((xraw_f * _mag_range_scale) - _mag_scale.x_offset) * _mag_scale.x_scale; + mag_report.y = ((yraw_f * _mag_range_scale) - _mag_scale.y_offset) * _mag_scale.y_scale; + mag_report.z = ((zraw_f * _mag_range_scale) - _mag_scale.z_offset) * _mag_scale.z_scale; mag_report.scaling = _mag_range_scale; mag_report.range_ga = (float)_mag_range_ga; mag_report.error_count = perf_event_count(_bad_registers) + perf_event_count(_bad_values); - // apply user specified rotation - rotate_3f(_rotation, mag_report.x, mag_report.y, mag_report.z); - _mag_reports->force(&mag_report); /* XXX please check this poll_notify, is it the right one? */ diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index d3b8d4e9d..cb463eb59 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -96,6 +96,9 @@ #define ESC_UORB_PUBLISH_DELAY 500000 #define CONTROL_INPUT_DROP_LIMIT_MS 20 +#define RC_MIN_VALUE 1010 +#define RC_MAX_VALUE 2100 + struct MotorData_t { unsigned int Version; // the version of the BL (0 = old) @@ -141,6 +144,8 @@ public: void set_px4mode(int px4mode); void set_frametype(int frametype); unsigned int mk_check_for_blctrl(unsigned int count, bool showOutput, bool initI2C); + void set_rc_min_value(unsigned value); + void set_rc_max_value(unsigned value); private: static const unsigned _max_actuators = MAX_MOTORS; @@ -164,7 +169,9 @@ private: bool _armed; unsigned long debugCounter; MixerGroup *_mixers; - bool _indicate_esc; + bool _indicate_esc; + unsigned _rc_min_value; + unsigned _rc_max_value; param_t _param_indicate_esc; actuator_controls_s _controls; MotorData_t Motor[MAX_MOTORS]; @@ -227,7 +234,9 @@ MK::MK(int bus, const char *_device_path) : _task_should_exit(false), _armed(false), _mixers(nullptr), - _indicate_esc(false) + _indicate_esc(false), + _rc_min_value(RC_MIN_VALUE), + _rc_max_value(RC_MAX_VALUE) { strncpy(_device, _device_path, sizeof(_device)); /* enforce null termination */ @@ -327,6 +336,19 @@ MK::set_frametype(int frametype) _frametype = frametype; } +void +MK::set_rc_min_value(unsigned value) +{ + _rc_min_value = value; + fprintf(stderr, "[mkblctrl] rc_min = %i\n", _rc_min_value); +} + +void +MK::set_rc_max_value(unsigned value) +{ + _rc_max_value = value; + fprintf(stderr, "[mkblctrl] rc_max = %i\n", _rc_max_value); +} int MK::set_motor_count(unsigned count) @@ -984,7 +1006,7 @@ MK::pwm_ioctl(file *filp, int cmd, unsigned long arg) case PWM_SERVO_SET(0) ... PWM_SERVO_SET(_max_actuators - 1): if (arg < 2150) { Motor[cmd - PWM_SERVO_SET(0)].RawPwmValue = (unsigned short)arg; - mk_servo_set(cmd - PWM_SERVO_SET(0), scaling(arg, 1010, 2100, 0, 2047)); + mk_servo_set(cmd - PWM_SERVO_SET(0), scaling(arg, _rc_min_value, _rc_max_value, 0, 2047)); } else { ret = -EINVAL; @@ -1064,6 +1086,37 @@ MK::pwm_ioctl(file *filp, int cmd, unsigned long arg) break; } + case PWM_SERVO_SET_MIN_PWM: { + struct pwm_output_values* pwm = (struct pwm_output_values*)arg; + if (pwm->channel_count > _max_actuators) + /* fail with error */ + return -E2BIG; + + set_rc_min_value((unsigned)pwm->values[0]); + ret = OK; + break; + } + + case PWM_SERVO_GET_MIN_PWM: + ret = OK; + break; + + case PWM_SERVO_SET_MAX_PWM: { + struct pwm_output_values* pwm = (struct pwm_output_values*)arg; + if (pwm->channel_count > _max_actuators) + /* fail with error */ + return -E2BIG; + + set_rc_max_value((unsigned)pwm->values[0]); + ret = OK; + break; + } + + case PWM_SERVO_GET_MAX_PWM: + ret = OK; + break; + + default: ret = -ENOTTY; break; @@ -1094,7 +1147,7 @@ MK::write(file *filp, const char *buffer, size_t len) for (uint8_t i = 0; i < count; i++) { Motor[i].RawPwmValue = (unsigned short)values[i]; - mk_servo_set(i, scaling(values[i], 1010, 2100, 0, 2047)); + mk_servo_set(i, scaling(values[i], _rc_min_value, _rc_max_value, 0, 2047)); } return count * 2; @@ -1116,10 +1169,16 @@ enum FrameType { int -mk_new_mode(int motorcount, bool motortest, int px4mode, int frametype, bool overrideSecurityChecks) +mk_new_mode(int motorcount, bool motortest, int px4mode, int frametype, bool overrideSecurityChecks, unsigned rcmin, unsigned rcmax) { int shouldStop = 0; + /* set rc min pulse value */ + g_mk->set_rc_min_value(rcmin); + + /* set rc max pulse value */ + g_mk->set_rc_max_value(rcmax); + /* native PX4 addressing) */ g_mk->set_px4mode(px4mode); @@ -1209,6 +1268,9 @@ mkblctrl_main(int argc, char *argv[]) bool showHelp = false; bool newMode = true; const char *devicepath = ""; + unsigned rc_min_value = RC_MIN_VALUE; + unsigned rc_max_value = RC_MAX_VALUE; + char *ep; /* * optional parameters @@ -1268,6 +1330,35 @@ mkblctrl_main(int argc, char *argv[]) } } + /* look for the optional -rc_min parameter */ + if (strcmp(argv[i], "-rc_min") == 0 ) { + if (argc > i + 1) { + rc_min_value = strtoul(argv[i + 1], &ep, 0); + if (*ep != '\0') { + errx(1, "bad pwm val (-rc_min)"); + return 1; + } + } else { + errx(1, "missing value (-rc_min)"); + return 1; + } + } + + /* look for the optional -rc_max parameter */ + if (strcmp(argv[i], "-rc_max") == 0 ) { + if (argc > i + 1) { + rc_max_value = strtoul(argv[i + 1], &ep, 0); + if (*ep != '\0') { + errx(1, "bad pwm val (-rc_max)"); + return 1; + } + } else { + errx(1, "missing value (-rc_max)"); + return 1; + } + } + + } if (showHelp) { @@ -1276,6 +1367,8 @@ mkblctrl_main(int argc, char *argv[]) fprintf(stderr, "\t -mkmode {+/x} \t\t Type of frame, if Mikrokopter motor order is used.\n"); fprintf(stderr, "\t -d {devicepath & name}\t\t Create alternate pwm device.\n"); fprintf(stderr, "\t --override-security-checks \t\t Disable all security checks (arming and number of ESCs). Used to test single Motors etc. (DANGER !!!)\n"); + fprintf(stderr, "\t -rcmin {pwn-value}\t\t Set RC_MIN Value.\n"); + fprintf(stderr, "\t -rcmax {pwn-value}\t\t Set RC_MAX Value.\n"); fprintf(stderr, "\n"); fprintf(stderr, "Motortest:\n"); fprintf(stderr, "First you have to start mkblctrl, the you can enter Motortest Mode with:\n"); @@ -1294,7 +1387,7 @@ mkblctrl_main(int argc, char *argv[]) /* parameter set ? */ if (newMode) { /* switch parameter */ - return mk_new_mode(motorcount, motortest, px4mode, frametype, overrideSecurityChecks); + return mk_new_mode(motorcount, motortest, px4mode, frametype, overrideSecurityChecks, rc_min_value, rc_max_value); } exit(0); diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index b48ea8577..eaf25a9f3 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -1711,17 +1711,21 @@ MPU6000::measure() arb.y_raw = report.accel_y; arb.z_raw = report.accel_z; - float x_in_new = ((report.accel_x * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; - float y_in_new = ((report.accel_y * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; - float z_in_new = ((report.accel_z * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; + float xraw_f = report.accel_x; + float yraw_f = report.accel_y; + float zraw_f = report.accel_z; + + // apply user specified rotation + rotate_3f(_rotation, xraw_f, yraw_f, zraw_f); + + float x_in_new = ((xraw_f * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; + float y_in_new = ((yraw_f * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; + float z_in_new = ((zraw_f * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; arb.x = _accel_filter_x.apply(x_in_new); arb.y = _accel_filter_y.apply(y_in_new); arb.z = _accel_filter_z.apply(z_in_new); - // apply user specified rotation - rotate_3f(_rotation, arb.x, arb.y, arb.z); - arb.scaling = _accel_range_scale; arb.range_m_s2 = _accel_range_m_s2; @@ -1732,17 +1736,21 @@ MPU6000::measure() grb.y_raw = report.gyro_y; grb.z_raw = report.gyro_z; - float x_gyro_in_new = ((report.gyro_x * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale; - float y_gyro_in_new = ((report.gyro_y * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale; - float z_gyro_in_new = ((report.gyro_z * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale; + xraw_f = report.gyro_x; + yraw_f = report.gyro_y; + zraw_f = report.gyro_z; + + // apply user specified rotation + rotate_3f(_rotation, xraw_f, yraw_f, zraw_f); + + float x_gyro_in_new = ((xraw_f * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale; + float y_gyro_in_new = ((yraw_f * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale; + float z_gyro_in_new = ((zraw_f * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale; grb.x = _gyro_filter_x.apply(x_gyro_in_new); grb.y = _gyro_filter_y.apply(y_gyro_in_new); grb.z = _gyro_filter_z.apply(z_gyro_in_new); - // apply user specified rotation - rotate_3f(_rotation, grb.x, grb.y, grb.z); - grb.scaling = _gyro_range_scale; grb.range_rad_s = _gyro_range_rad_s; diff --git a/src/drivers/ms5611/ms5611.h b/src/drivers/ms5611/ms5611.h index c8211b8dd..c946e38cb 100644 --- a/src/drivers/ms5611/ms5611.h +++ b/src/drivers/ms5611/ms5611.h @@ -82,4 +82,4 @@ extern bool crc4(uint16_t *n_prom); /* interface factories */ extern device::Device *MS5611_spi_interface(ms5611::prom_u &prom_buf, uint8_t busnum) weak_function; extern device::Device *MS5611_i2c_interface(ms5611::prom_u &prom_buf, uint8_t busnum) weak_function; -typedef device::Device* (*MS5611_constructor)(ms5611::prom_u &prom_buf, uint8_t busnum); +typedef device::Device *(*MS5611_constructor)(ms5611::prom_u &prom_buf, uint8_t busnum); diff --git a/src/examples/fixedwing_control/main.c b/src/examples/fixedwing_control/main.c index d8b777b91..17b27290c 100644 --- a/src/examples/fixedwing_control/main.c +++ b/src/examples/fixedwing_control/main.c @@ -112,8 +112,9 @@ static void usage(const char *reason); * @param att The current attitude. The controller should make the attitude match the setpoint * @param rates_sp The angular rate setpoint. This is the output of the controller. */ -void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, - struct actuator_controls_s *actuators); +void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att, + struct vehicle_rates_setpoint_s *rates_sp, + struct actuator_controls_s *actuators); /** * Control heading. @@ -128,7 +129,7 @@ void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const st * @param att_sp The attitude setpoint. This is the output of the controller */ void control_heading(const struct vehicle_global_position_s *pos, const struct position_setpoint_s *sp, - const struct vehicle_attitude_s *att, struct vehicle_attitude_setpoint_s *att_sp); + const struct vehicle_attitude_s *att, struct vehicle_attitude_setpoint_s *att_sp); /* Variables */ static bool thread_should_exit = false; /**< Daemon exit flag */ @@ -137,8 +138,9 @@ static int deamon_task; /**< Handle of deamon task / thread */ static struct params p; static struct param_handles ph; -void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, - struct actuator_controls_s *actuators) +void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att, + struct vehicle_rates_setpoint_s *rates_sp, + struct actuator_controls_s *actuators) { /* @@ -175,7 +177,7 @@ void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const st } void control_heading(const struct vehicle_global_position_s *pos, const struct position_setpoint_s *sp, - const struct vehicle_attitude_s *att, struct vehicle_attitude_setpoint_s *att_sp) + const struct vehicle_attitude_s *att, struct vehicle_attitude_setpoint_s *att_sp) { /* @@ -192,6 +194,7 @@ void control_heading(const struct vehicle_global_position_s *pos, const struct p /* limit output, this commonly is a tuning parameter, too */ if (att_sp->roll_body < -0.6f) { att_sp->roll_body = -0.6f; + } else if (att_sp->roll_body > 0.6f) { att_sp->roll_body = 0.6f; } @@ -285,7 +288,8 @@ int fixedwing_control_thread_main(int argc, char *argv[]) /* Setup of loop */ struct pollfd fds[2] = {{ .fd = param_sub, .events = POLLIN }, - { .fd = att_sub, .events = POLLIN }}; + { .fd = att_sub, .events = POLLIN } + }; while (!thread_should_exit) { @@ -345,12 +349,14 @@ int fixedwing_control_thread_main(int argc, char *argv[]) if (manual_sp_updated) /* get the RC (or otherwise user based) input */ + { orb_copy(ORB_ID(manual_control_setpoint), manual_sp_sub, &manual_sp); + } /* check if the throttle was ever more than 50% - go later only to failsafe if yes */ if (isfinite(manual_sp.z) && - (manual_sp.z >= 0.6f) && - (manual_sp.z <= 1.0f)) { + (manual_sp.z >= 0.6f) && + (manual_sp.z <= 1.0f)) { } /* get the system status and the flight mode we're in */ @@ -378,8 +384,9 @@ int fixedwing_control_thread_main(int argc, char *argv[]) thread_running = false; /* kill all outputs */ - for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) + for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) { actuators.control[i] = 0.0f; + } orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); @@ -393,8 +400,9 @@ int fixedwing_control_thread_main(int argc, char *argv[]) static void usage(const char *reason) { - if (reason) + if (reason) { fprintf(stderr, "%s\n", reason); + } fprintf(stderr, "usage: ex_fixedwing_control {start|stop|status}\n\n"); exit(1); @@ -410,8 +418,9 @@ usage(const char *reason) */ int ex_fixedwing_control_main(int argc, char *argv[]) { - if (argc < 1) + if (argc < 1) { usage("missing command"); + } if (!strcmp(argv[1], "start")) { @@ -423,11 +432,11 @@ int ex_fixedwing_control_main(int argc, char *argv[]) thread_should_exit = false; deamon_task = task_spawn_cmd("ex_fixedwing_control", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 20, - 2048, - fixedwing_control_thread_main, - (argv) ? (char * const *)&argv[2] : (char * const *)NULL); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 20, + 2048, + fixedwing_control_thread_main, + (argv) ? (char * const *)&argv[2] : (char * const *)NULL); thread_running = true; exit(0); } diff --git a/src/examples/fixedwing_control/params.c b/src/examples/fixedwing_control/params.c index c2e94ff3d..fbbd30201 100644 --- a/src/examples/fixedwing_control/params.c +++ b/src/examples/fixedwing_control/params.c @@ -34,7 +34,7 @@ /* * @file params.c - * + * * Parameters for fixedwing demo */ diff --git a/src/examples/fixedwing_control/params.h b/src/examples/fixedwing_control/params.h index 4ae8e90ec..49ffff446 100644 --- a/src/examples/fixedwing_control/params.h +++ b/src/examples/fixedwing_control/params.h @@ -34,7 +34,7 @@ /* * @file params.h - * + * * Definition of parameters for fixedwing example */ diff --git a/src/examples/flow_position_estimator/flow_position_estimator_main.c b/src/examples/flow_position_estimator/flow_position_estimator_main.c index da4ea825b..f8e3f2dc5 100644 --- a/src/examples/flow_position_estimator/flow_position_estimator_main.c +++ b/src/examples/flow_position_estimator/flow_position_estimator_main.c @@ -81,8 +81,10 @@ static void usage(const char *reason); static void usage(const char *reason) { - if (reason) + if (reason) { fprintf(stderr, "%s\n", reason); + } + fprintf(stderr, "usage: daemon {start|stop|status} [-p <additional params>]\n\n"); exit(1); } @@ -97,13 +99,12 @@ static void usage(const char *reason) */ int flow_position_estimator_main(int argc, char *argv[]) { - if (argc < 1) + if (argc < 1) { usage("missing command"); + } - if (!strcmp(argv[1], "start")) - { - if (thread_running) - { + if (!strcmp(argv[1], "start")) { + if (thread_running) { printf("flow position estimator already running\n"); /* this is not an error */ exit(0); @@ -111,26 +112,26 @@ int flow_position_estimator_main(int argc, char *argv[]) thread_should_exit = false; daemon_task = task_spawn_cmd("flow_position_estimator", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 4000, - flow_position_estimator_thread_main, - (argv) ? (char * const *)&argv[2] : (char * const *)NULL); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 4000, + flow_position_estimator_thread_main, + (argv) ? (char * const *)&argv[2] : (char * const *)NULL); exit(0); } - if (!strcmp(argv[1], "stop")) - { + if (!strcmp(argv[1], "stop")) { thread_should_exit = true; exit(0); } - if (!strcmp(argv[1], "status")) - { - if (thread_running) + if (!strcmp(argv[1], "status")) { + if (thread_running) { printf("\tflow position estimator is running\n"); - else + + } else { printf("\tflow position estimator not started\n"); + } exit(0); } @@ -147,9 +148,10 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) /* rotation matrix for transformation of optical flow speed vectors */ static const int8_t rotM_flow_sensor[3][3] = {{ 0, -1, 0 }, - { 1, 0, 0 }, - { 0, 0, 1 }}; // 90deg rotated - const float time_scale = powf(10.0f,-6.0f); + { 1, 0, 0 }, + { 0, 0, 1 } + }; // 90deg rotated + const float time_scale = powf(10.0f, -6.0f); static float speed[3] = {0.0f, 0.0f, 0.0f}; static float flow_speed[3] = {0.0f, 0.0f, 0.0f}; static float global_speed[3] = {0.0f, 0.0f, 0.0f}; @@ -192,18 +194,18 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) /* init local position and filtered flow struct */ struct vehicle_local_position_s local_pos = { - .x = 0.0f, - .y = 0.0f, - .z = 0.0f, - .vx = 0.0f, - .vy = 0.0f, - .vz = 0.0f + .x = 0.0f, + .y = 0.0f, + .z = 0.0f, + .vx = 0.0f, + .vy = 0.0f, + .vz = 0.0f }; struct filtered_bottom_flow_s filtered_flow = { - .sumx = 0.0f, - .sumy = 0.0f, - .vx = 0.0f, - .vy = 0.0f + .sumx = 0.0f, + .sumy = 0.0f, + .vx = 0.0f, + .vy = 0.0f }; /* advert pub messages */ @@ -224,37 +226,29 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) perf_counter_t mc_interval_perf = perf_alloc(PC_INTERVAL, "flow_position_estimator_interval"); perf_counter_t mc_err_perf = perf_alloc(PC_COUNT, "flow_position_estimator_err"); - while (!thread_should_exit) - { + while (!thread_should_exit) { - if (sensors_ready) - { + if (sensors_ready) { /*This runs at the rate of the sensors */ struct pollfd fds[2] = { - { .fd = optical_flow_sub, .events = POLLIN }, - { .fd = parameter_update_sub, .events = POLLIN } + { .fd = optical_flow_sub, .events = POLLIN }, + { .fd = parameter_update_sub, .events = POLLIN } }; /* wait for a sensor update, check for exit condition every 500 ms */ int ret = poll(fds, 2, 500); - if (ret < 0) - { + if (ret < 0) { /* poll error, count it in perf */ perf_count(mc_err_perf); - } - else if (ret == 0) - { + } else if (ret == 0) { /* no return value, ignore */ // printf("[flow position estimator] no bottom flow.\n"); - } - else - { + } else { /* parameter update available? */ - if (fds[1].revents & POLLIN) - { + if (fds[1].revents & POLLIN) { /* read from param to clear updated flag */ struct parameter_update_s update; orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update); @@ -264,8 +258,7 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) } /* only if flow data changed */ - if (fds[0].revents & POLLIN) - { + if (fds[0].revents & POLLIN) { perf_begin(mc_loop_perf); orb_copy(ORB_ID(optical_flow), optical_flow_sub, &flow); @@ -284,46 +277,48 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) * -> minimum sonar value 0.3m */ - if (!vehicle_liftoff) - { - if (armed.armed && att_sp.thrust > params.minimum_liftoff_thrust && sonar_new > 0.3f && sonar_new < 1.0f) + if (!vehicle_liftoff) { + if (armed.armed && att_sp.thrust > params.minimum_liftoff_thrust && sonar_new > 0.3f && sonar_new < 1.0f) { vehicle_liftoff = true; - } - else - { - if (!armed.armed || (att_sp.thrust < params.minimum_liftoff_thrust && sonar_new <= 0.3f)) + } + + } else { + if (!armed.armed || (att_sp.thrust < params.minimum_liftoff_thrust && sonar_new <= 0.3f)) { vehicle_liftoff = false; + } } /* calc dt between flow timestamps */ /* ignore first flow msg */ - if(time_last_flow == 0) - { + if (time_last_flow == 0) { time_last_flow = flow.timestamp; continue; } + dt = (float)(flow.timestamp - time_last_flow) * time_scale ; time_last_flow = flow.timestamp; /* only make position update if vehicle is lift off or DEBUG is activated*/ - if (vehicle_liftoff || params.debug) - { + if (vehicle_liftoff || params.debug) { /* copy flow */ if (flow.integration_timespan > 0) { flow_speed[0] = flow.pixel_flow_x_integral / (flow.integration_timespan / 1e6f) * flow.ground_distance_m; flow_speed[1] = flow.pixel_flow_y_integral / (flow.integration_timespan / 1e6f) * flow.ground_distance_m; + } else { flow_speed[0] = 0; flow_speed[1] = 0; } + flow_speed[2] = 0.0f; /* convert to bodyframe velocity */ - for(uint8_t i = 0; i < 3; i++) - { + for (uint8_t i = 0; i < 3; i++) { float sum = 0.0f; - for(uint8_t j = 0; j < 3; j++) + + for (uint8_t j = 0; j < 3; j++) { sum = sum + flow_speed[j] * rotM_flow_sensor[j][i]; + } speed[i] = sum; } @@ -339,11 +334,12 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) /* convert to globalframe velocity * -> local position is currently not used for position control */ - for(uint8_t i = 0; i < 3; i++) - { + for (uint8_t i = 0; i < 3; i++) { float sum = 0.0f; - for(uint8_t j = 0; j < 3; j++) + + for (uint8_t j = 0; j < 3; j++) { sum = sum + speed[j] * PX4_R(att.R, i, j); + } global_speed[i] = sum; } @@ -354,9 +350,8 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) local_pos.vy = global_speed[1]; local_pos.xy_valid = true; local_pos.v_xy_valid = true; - } - else - { + + } else { /* set speed to zero and let position as it is */ filtered_flow.vx = 0; filtered_flow.vy = 0; @@ -367,24 +362,20 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) } /* filtering ground distance */ - if (!vehicle_liftoff || !armed.armed) - { + if (!vehicle_liftoff || !armed.armed) { /* not possible to fly */ sonar_valid = false; local_pos.z = 0.0f; local_pos.z_valid = false; - } - else - { + + } else { sonar_valid = true; } - if (sonar_valid || params.debug) - { + if (sonar_valid || params.debug) { /* simple lowpass sonar filtering */ /* if new value or with sonar update frequency */ - if (sonar_new != sonar_last || counter % 10 == 0) - { + if (sonar_new != sonar_last || counter % 10 == 0) { sonar_lp = 0.05f * sonar_new + 0.95f * sonar_lp; sonar_last = sonar_new; } @@ -392,12 +383,10 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) float height_diff = sonar_new - sonar_lp; /* if over 1/2m spike follow lowpass */ - if (height_diff < -params.sonar_lower_lp_threshold || height_diff > params.sonar_upper_lp_threshold) - { + if (height_diff < -params.sonar_lower_lp_threshold || height_diff > params.sonar_upper_lp_threshold) { local_pos.z = -sonar_lp; - } - else - { + + } else { local_pos.z = -sonar_new; } @@ -408,15 +397,14 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) local_pos.timestamp = hrt_absolute_time(); /* publish local position */ - if(isfinite(local_pos.x) && isfinite(local_pos.y) && isfinite(local_pos.z) - && isfinite(local_pos.vx) && isfinite(local_pos.vy)) - { + if (isfinite(local_pos.x) && isfinite(local_pos.y) && isfinite(local_pos.z) + && isfinite(local_pos.vx) && isfinite(local_pos.vy)) { orb_publish(ORB_ID(vehicle_local_position), local_pos_pub, &local_pos); } /* publish filtered flow */ - if(isfinite(filtered_flow.sumx) && isfinite(filtered_flow.sumy) && isfinite(filtered_flow.vx) && isfinite(filtered_flow.vy)) - { + if (isfinite(filtered_flow.sumx) && isfinite(filtered_flow.sumy) && isfinite(filtered_flow.vx) + && isfinite(filtered_flow.vy)) { orb_publish(ORB_ID(filtered_bottom_flow), filtered_flow_pub, &filtered_flow); } @@ -427,9 +415,7 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) } } - } - else - { + } else { /* sensors not ready waiting for first attitude msg */ /* polling */ @@ -440,19 +426,16 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) /* wait for a attitude message, check for exit condition every 5 s */ int ret = poll(fds, 1, 5000); - if (ret < 0) - { + if (ret < 0) { /* poll error, count it in perf */ perf_count(mc_err_perf); - } - else if (ret == 0) - { + + } else if (ret == 0) { /* no return value, ignore */ printf("[flow position estimator] no attitude received.\n"); - } - else - { - if (fds[0].revents & POLLIN){ + + } else { + if (fds[0].revents & POLLIN) { sensors_ready = true; printf("[flow position estimator] initialized.\n"); } diff --git a/src/examples/flow_position_estimator/flow_position_estimator_params.c b/src/examples/flow_position_estimator/flow_position_estimator_params.c index ec3c3352d..b56579787 100644 --- a/src/examples/flow_position_estimator/flow_position_estimator_params.c +++ b/src/examples/flow_position_estimator/flow_position_estimator_params.c @@ -35,7 +35,7 @@ /* * @file flow_position_estimator_params.c - * + * * Parameters for position estimator */ diff --git a/src/examples/flow_position_estimator/flow_position_estimator_params.h b/src/examples/flow_position_estimator/flow_position_estimator_params.h index f9a9bb303..3ab4e560f 100644 --- a/src/examples/flow_position_estimator/flow_position_estimator_params.h +++ b/src/examples/flow_position_estimator/flow_position_estimator_params.h @@ -35,7 +35,7 @@ /* * @file flow_position_estimator_params.h - * + * * Parameters for position estimator */ diff --git a/src/examples/matlab_csv_serial/matlab_csv_serial.c b/src/examples/matlab_csv_serial/matlab_csv_serial.c index 145cf99cc..8e439bdac 100644 --- a/src/examples/matlab_csv_serial/matlab_csv_serial.c +++ b/src/examples/matlab_csv_serial/matlab_csv_serial.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2014-2015 PX4 Development Team. All rights reserved. + * Copyright (c) 2014-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -73,8 +73,10 @@ static void usage(const char *reason); static void usage(const char *reason) { - if (reason) + if (reason) { fprintf(stderr, "%s\n", reason); + } + fprintf(stderr, "usage: daemon {start|stop|status} [-p <additional params>]\n\n"); exit(1); } @@ -89,13 +91,12 @@ static void usage(const char *reason) */ int matlab_csv_serial_main(int argc, char *argv[]) { - if (argc < 1) + if (argc < 1) { usage("missing command"); + } - if (!strcmp(argv[1], "start")) - { - if (thread_running) - { + if (!strcmp(argv[1], "start")) { + if (thread_running) { warnx("already running\n"); /* this is not an error */ exit(0); @@ -103,24 +104,23 @@ int matlab_csv_serial_main(int argc, char *argv[]) thread_should_exit = false; daemon_task = task_spawn_cmd("matlab_csv_serial", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 2000, - matlab_csv_serial_thread_main, - (argv) ? (char * const *)&argv[2] : (char * const *)NULL); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 2000, + matlab_csv_serial_thread_main, + (argv) ? (char * const *)&argv[2] : (char * const *)NULL); exit(0); } - if (!strcmp(argv[1], "stop")) - { + if (!strcmp(argv[1], "stop")) { thread_should_exit = true; exit(0); } - if (!strcmp(argv[1], "status")) - { + if (!strcmp(argv[1], "status")) { if (thread_running) { warnx("running"); + } else { warnx("stopped"); } @@ -139,7 +139,7 @@ int matlab_csv_serial_thread_main(int argc, char *argv[]) errx(1, "need a serial port name as argument"); } - const char* uart_name = argv[1]; + const char *uart_name = argv[1]; warnx("opening port %s", uart_name); @@ -197,40 +197,35 @@ int matlab_csv_serial_thread_main(int argc, char *argv[]) thread_running = true; - while (!thread_should_exit) - { + while (!thread_should_exit) { /*This runs at the rate of the sensors */ struct pollfd fds[] = { - { .fd = accel0_sub, .events = POLLIN } + { .fd = accel0_sub, .events = POLLIN } }; /* wait for a sensor update, check for exit condition every 500 ms */ int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), 500); - if (ret < 0) - { + if (ret < 0) { /* poll error, ignore */ - } - else if (ret == 0) - { + } else if (ret == 0) { /* no return value, ignore */ warnx("no sensor data"); - } - else - { + + } else { /* accel0 update available? */ - if (fds[0].revents & POLLIN) - { + if (fds[0].revents & POLLIN) { orb_copy(ORB_ID(sensor_accel), accel0_sub, &accel0); orb_copy(ORB_ID(sensor_accel), accel1_sub, &accel1); orb_copy(ORB_ID(sensor_gyro), gyro0_sub, &gyro0); orb_copy(ORB_ID(sensor_gyro), gyro1_sub, &gyro1); // write out on accel 0, but collect for all other sensors as they have updates - dprintf(serial_fd, "%llu,%d,%d,%d,%d,%d,%d\n", accel0.timestamp, (int)accel0.x_raw, (int)accel0.y_raw, (int)accel0.z_raw, + dprintf(serial_fd, "%llu,%d,%d,%d,%d,%d,%d\n", accel0.timestamp, (int)accel0.x_raw, (int)accel0.y_raw, + (int)accel0.z_raw, (int)accel1.x_raw, (int)accel1.y_raw, (int)accel1.z_raw); } diff --git a/src/examples/publisher/publisher_example.h b/src/examples/publisher/publisher_example.h index 8165b0ffc..0a66d3edc 100644 --- a/src/examples/publisher/publisher_example.h +++ b/src/examples/publisher/publisher_example.h @@ -40,7 +40,8 @@ #pragma once #include <px4.h> -class PublisherExample { +class PublisherExample +{ public: PublisherExample(); @@ -49,7 +50,7 @@ public: int main(); protected: px4::NodeHandle _n; - px4::Publisher<px4::px4_rc_channels> * _rc_channels_pub; - px4::Publisher<px4::px4_vehicle_attitude> * _v_att_pub; - px4::Publisher<px4::px4_parameter_update> * _parameter_update_pub; + px4::Publisher<px4::px4_rc_channels> *_rc_channels_pub; + px4::Publisher<px4::px4_vehicle_attitude> *_v_att_pub; + px4::Publisher<px4::px4_parameter_update> *_parameter_update_pub; }; diff --git a/src/examples/publisher/publisher_start_nuttx.cpp b/src/examples/publisher/publisher_start_nuttx.cpp index db9d85269..405b3c987 100644 --- a/src/examples/publisher/publisher_start_nuttx.cpp +++ b/src/examples/publisher/publisher_start_nuttx.cpp @@ -69,11 +69,11 @@ int publisher_main(int argc, char *argv[]) task_should_exit = false; daemon_task = task_spawn_cmd("publisher", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 2000, - main, - (argv) ? (char* const*)&argv[2] : (char* const*)NULL); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 2000, + main, + (argv) ? (char * const *)&argv[2] : (char * const *)NULL); exit(0); } diff --git a/src/examples/px4_daemon_app/px4_daemon_app.c b/src/examples/px4_daemon_app/px4_daemon_app.c index 45d541137..860b1af78 100644 --- a/src/examples/px4_daemon_app/px4_daemon_app.c +++ b/src/examples/px4_daemon_app/px4_daemon_app.c @@ -34,7 +34,7 @@ /** * @file px4_daemon_app.c * daemon application example for PX4 autopilot - * + * * @author Example User <mail@example.com> */ @@ -71,8 +71,10 @@ static void usage(const char *reason); static void usage(const char *reason) { - if (reason) + if (reason) { warnx("%s\n", reason); + } + errx(1, "usage: daemon {start|stop|status} [-p <additional params>]\n\n"); } @@ -80,14 +82,15 @@ usage(const char *reason) * The daemon app only briefly exists to start * the background job. The stack size assigned in the * Makefile does only apply to this management task. - * + * * The actual stack size should be set in the call * to task_create(). */ int px4_daemon_app_main(int argc, char *argv[]) { - if (argc < 1) + if (argc < 1) { usage("missing command"); + } if (!strcmp(argv[1], "start")) { @@ -99,11 +102,11 @@ int px4_daemon_app_main(int argc, char *argv[]) thread_should_exit = false; daemon_task = task_spawn_cmd("daemon", - SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT, - 2000, - px4_daemon_thread_main, - (argv) ? (char * const *)&argv[2] : (char * const *)NULL); + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, + 2000, + px4_daemon_thread_main, + (argv) ? (char * const *)&argv[2] : (char * const *)NULL); exit(0); } @@ -115,9 +118,11 @@ int px4_daemon_app_main(int argc, char *argv[]) if (!strcmp(argv[1], "status")) { if (thread_running) { warnx("\trunning\n"); + } else { warnx("\tnot started\n"); } + exit(0); } @@ -125,7 +130,8 @@ int px4_daemon_app_main(int argc, char *argv[]) exit(1); } -int px4_daemon_thread_main(int argc, char *argv[]) { +int px4_daemon_thread_main(int argc, char *argv[]) +{ warnx("[daemon] starting\n"); diff --git a/src/examples/px4_simple_app/px4_simple_app.c b/src/examples/px4_simple_app/px4_simple_app.c index 4e9f099ed..e22c59fa2 100644 --- a/src/examples/px4_simple_app/px4_simple_app.c +++ b/src/examples/px4_simple_app/px4_simple_app.c @@ -75,30 +75,33 @@ int px4_simple_app_main(int argc, char *argv[]) for (int i = 0; i < 5; i++) { /* wait for sensor update of 1 file descriptor for 1000 ms (1 second) */ int poll_ret = poll(fds, 1, 1000); - + /* handle the poll result */ if (poll_ret == 0) { /* this means none of our providers is giving us data */ printf("[px4_simple_app] Got no data within a second\n"); + } else if (poll_ret < 0) { /* this is seriously bad - should be an emergency */ if (error_counter < 10 || error_counter % 50 == 0) { /* use a counter to prevent flooding (and slowing us down) */ printf("[px4_simple_app] ERROR return value from poll(): %d\n" - , poll_ret); + , poll_ret); } + error_counter++; + } else { - + if (fds[0].revents & POLLIN) { /* obtained data for the first file descriptor */ struct sensor_combined_s raw; /* copy sensors raw data into local buffer */ orb_copy(ORB_ID(sensor_combined), sensor_sub_fd, &raw); printf("[px4_simple_app] Accelerometer:\t%8.4f\t%8.4f\t%8.4f\n", - (double)raw.accelerometer_m_s2[0], - (double)raw.accelerometer_m_s2[1], - (double)raw.accelerometer_m_s2[2]); + (double)raw.accelerometer_m_s2[0], + (double)raw.accelerometer_m_s2[1], + (double)raw.accelerometer_m_s2[2]); /* set att and publish this information for other apps */ att.roll = raw.accelerometer_m_s2[0]; @@ -106,6 +109,7 @@ int px4_simple_app_main(int argc, char *argv[]) att.yaw = raw.accelerometer_m_s2[2]; orb_publish(ORB_ID(vehicle_attitude), att_pub, &att); } + /* there could be more file descriptors here, in the form like: * if (fds[1..n].revents & POLLIN) {} */ diff --git a/src/examples/rover_steering_control/main.cpp b/src/examples/rover_steering_control/main.cpp index edde5627d..96b40b23f 100644 --- a/src/examples/rover_steering_control/main.cpp +++ b/src/examples/rover_steering_control/main.cpp @@ -266,18 +266,27 @@ int rover_steering_control_thread_main(int argc, char *argv[]) /* subscribe to topics. */ int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + int global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); + int manual_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); + int vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); + int att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); + int param_sub = orb_subscribe(ORB_ID(parameter_update)); /* Setup of loop */ struct pollfd fds[2]; + fds[0].fd = param_sub; + fds[0].events = POLLIN; + fds[1].fd = att_sub; + fds[1].events = POLLIN; while (!thread_should_exit) { @@ -370,6 +379,7 @@ int rover_steering_control_thread_main(int argc, char *argv[]) for (unsigned i = 0; i < (sizeof(actuators.control) / sizeof(actuators.control[0])); i++) { actuators.control[i] = 0.0f; } + actuators.timestamp = hrt_absolute_time(); orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); @@ -420,7 +430,7 @@ int rover_steering_control_main(int argc, char *argv[]) SCHED_PRIORITY_MAX - 20, 2048, rover_steering_control_thread_main, - (argv) ? (char *const *)&argv[2] : (char *const *)NULL); + (argv) ? (char * const *)&argv[2] : (char * const *)NULL); thread_running = true; exit(0); } diff --git a/src/examples/subscriber/subscriber_example.cpp b/src/examples/subscriber/subscriber_example.cpp index 496e1dcd8..5a23dc80b 100644 --- a/src/examples/subscriber/subscriber_example.cpp +++ b/src/examples/subscriber/subscriber_example.cpp @@ -43,7 +43,8 @@ using namespace px4; -void rc_channels_callback_function(const px4_rc_channels &msg) { +void rc_channels_callback_function(const px4_rc_channels &msg) +{ PX4_INFO("I heard: [%llu]", msg.data().timestamp_last_valid); } @@ -81,21 +82,24 @@ SubscriberExample::SubscriberExample() : * This tutorial demonstrates simple receipt of messages over the PX4 middleware system. * Also the current value of the _sub_rc_chan subscription is printed */ -void SubscriberExample::rc_channels_callback(const px4_rc_channels &msg) { +void SubscriberExample::rc_channels_callback(const px4_rc_channels &msg) +{ PX4_INFO("rc_channels_callback (method): [%llu]", - msg.data().timestamp_last_valid); + msg.data().timestamp_last_valid); PX4_INFO("rc_channels_callback (method): value of _sub_rc_chan: [%llu]", - _sub_rc_chan->data().timestamp_last_valid); + _sub_rc_chan->data().timestamp_last_valid); } -void SubscriberExample::vehicle_attitude_callback(const px4_vehicle_attitude &msg) { +void SubscriberExample::vehicle_attitude_callback(const px4_vehicle_attitude &msg) +{ PX4_INFO("vehicle_attitude_callback (method): [%llu]", - msg.data().timestamp); + msg.data().timestamp); } -void SubscriberExample::parameter_update_callback(const px4_parameter_update &msg) { +void SubscriberExample::parameter_update_callback(const px4_parameter_update &msg) +{ PX4_INFO("parameter_update_callback (method): [%llu]", - msg.data().timestamp); + msg.data().timestamp); _p_sub_interv.update(); PX4_INFO("Param SUB_INTERV = %d", _p_sub_interv.get()); _p_test_float.update(); diff --git a/src/examples/subscriber/subscriber_example.h b/src/examples/subscriber/subscriber_example.h index 686fed023..9eb5dd2a0 100644 --- a/src/examples/subscriber/subscriber_example.h +++ b/src/examples/subscriber/subscriber_example.h @@ -43,7 +43,8 @@ using namespace px4; void rc_channels_callback_function(const px4_rc_channels &msg); -class SubscriberExample { +class SubscriberExample +{ public: SubscriberExample(); @@ -54,7 +55,7 @@ protected: px4::NodeHandle _n; px4::ParameterInt _p_sub_interv; px4::ParameterFloat _p_test_float; - px4::Subscriber<px4_rc_channels> * _sub_rc_chan; + px4::Subscriber<px4_rc_channels> *_sub_rc_chan; void rc_channels_callback(const px4_rc_channels &msg); diff --git a/src/examples/subscriber/subscriber_start_nuttx.cpp b/src/examples/subscriber/subscriber_start_nuttx.cpp index 6129b19ac..b450230c1 100644 --- a/src/examples/subscriber/subscriber_start_nuttx.cpp +++ b/src/examples/subscriber/subscriber_start_nuttx.cpp @@ -69,11 +69,11 @@ int subscriber_main(int argc, char *argv[]) task_should_exit = false; daemon_task = task_spawn_cmd("subscriber", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 2000, - main, - (argv) ? (char* const*)&argv[2] : (char* const*)NULL); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 2000, + main, + (argv) ? (char * const *)&argv[2] : (char * const *)NULL); exit(0); } diff --git a/src/lib/conversion/rotation.h b/src/lib/conversion/rotation.h index 917c7f830..13fe701df 100644 --- a/src/lib/conversion/rotation.h +++ b/src/lib/conversion/rotation.h @@ -118,7 +118,7 @@ const rot_lookup_t rot_lookup[] = { * Get the rotation matrix */ __EXPORT void -get_rot_matrix(enum Rotation rot, math::Matrix<3,3> *rot_matrix); +get_rot_matrix(enum Rotation rot, math::Matrix<3, 3> *rot_matrix); /** diff --git a/src/lib/ecl/attitude_fw/ecl_controller.cpp b/src/lib/ecl/attitude_fw/ecl_controller.cpp index 46140fbfd..9cd08a50d 100644 --- a/src/lib/ecl/attitude_fw/ecl_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_controller.cpp @@ -49,6 +49,7 @@ #include "ecl_controller.h" #include <stdio.h> +#include <mathlib/mathlib.h> ECL_Controller::ECL_Controller(const char *name) : _last_run(0), @@ -126,3 +127,14 @@ float ECL_Controller::get_desired_bodyrate() { return _bodyrate_setpoint; } + +float ECL_Controller::constrain_airspeed(float airspeed, float minspeed, float maxspeed) { + float airspeed_result = airspeed; + if (!isfinite(airspeed)) { + /* airspeed is NaN, +- INF or not available, pick center of band */ + airspeed = 0.5f * (minspeed + maxspeed); + } else if (airspeed < minspeed) { + airspeed = minspeed; + } + return airspeed_result; +} diff --git a/src/lib/ecl/attitude_fw/ecl_controller.h b/src/lib/ecl/attitude_fw/ecl_controller.h index 254052964..ac1ac88d0 100644 --- a/src/lib/ecl/attitude_fw/ecl_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_controller.h @@ -119,4 +119,5 @@ protected: perf_counter_t _nonfinite_input_perf; static const uint8_t _perf_name_max = 40; char _perf_name[_perf_name_max]; + float constrain_airspeed(float airspeed, float minspeed, float maxspeed); }; diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp index ca454fa62..c80eb357c 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp @@ -94,9 +94,11 @@ float ECL_PitchController::control_attitude(const struct ECL_ControlData &ctl_da } } + /* input conditioning */ + float airspeed = constrain_airspeed(ctl_data.airspeed, ctl_data.airspeed_min, ctl_data.airspeed_max); /* calculate the offset in the rate resulting from rolling */ //xxx needs explanation and conversion to body angular rates or should be removed - float turn_offset = fabsf((CONSTANTS_ONE_G / ctl_data.airspeed) * + float turn_offset = fabsf((CONSTANTS_ONE_G / airspeed) * tanf(roll) * sinf(roll)) * _roll_ff; if (inverted) { @@ -154,17 +156,6 @@ float ECL_PitchController::control_bodyrate(const struct ECL_ControlData &ctl_da lock_integrator = true; } - /* input conditioning */ - float airspeed = ctl_data.airspeed; - - if (!isfinite(airspeed)) { - /* airspeed is NaN, +- INF or not available, pick center of band */ - airspeed = 0.5f * (ctl_data.airspeed_min + ctl_data.airspeed_max); - - } else if (airspeed < ctl_data.airspeed_min) { - airspeed = ctl_data.airspeed_min; - } - /* Transform setpoint to body angular rates (jacobian) */ _bodyrate_setpoint = cosf(ctl_data.roll) * _rate_setpoint + cosf(ctl_data.pitch) * sinf(ctl_data.roll) * ctl_data.yaw_rate_setpoint; @@ -175,7 +166,7 @@ float ECL_PitchController::control_bodyrate(const struct ECL_ControlData &ctl_da _rate_error = _bodyrate_setpoint - pitch_bodyrate; - if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * ctl_data.airspeed_min) { + if (!lock_integrator && _k_i > 0.0f) { float id = _rate_error * dt * ctl_data.scaler; diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp index 5d0846ac3..160dc5cad 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp @@ -106,17 +106,6 @@ float ECL_RollController::control_bodyrate(const struct ECL_ControlData &ctl_dat lock_integrator = true; } - /* input conditioning */ - float airspeed = ctl_data.airspeed; - - if (!isfinite(airspeed)) { - /* airspeed is NaN, +- INF or not available, pick center of band */ - airspeed = 0.5f * (ctl_data.airspeed_min + ctl_data.airspeed_max); - - } else if (airspeed < ctl_data.airspeed_min) { - airspeed = ctl_data.airspeed_min; - } - /* Transform setpoint to body angular rates (jacobian) */ _bodyrate_setpoint = _rate_setpoint - sinf(ctl_data.pitch) * ctl_data.yaw_rate_setpoint; @@ -126,7 +115,7 @@ float ECL_RollController::control_bodyrate(const struct ECL_ControlData &ctl_dat /* Calculate body angular rate error */ _rate_error = _bodyrate_setpoint - roll_bodyrate; //body angular rate error - if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * ctl_data.airspeed_min) { + if (!lock_integrator && _k_i > 0.0f) { float id = _rate_error * dt * ctl_data.scaler; @@ -157,3 +146,4 @@ float ECL_RollController::control_bodyrate(const struct ECL_ControlData &ctl_dat return math::constrain(_last_output, -1.0f, 1.0f); } + diff --git a/src/lib/eigen b/src/lib/eigen new file mode 160000 +Subproject e7850ed81f9c469e02df496ef09ae32ec0379b7 diff --git a/src/lib/launchdetection/LaunchDetector.h b/src/lib/launchdetection/LaunchDetector.h index 4215b49d2..3b276c556 100644 --- a/src/lib/launchdetection/LaunchDetector.h +++ b/src/lib/launchdetection/LaunchDetector.h @@ -76,7 +76,7 @@ private: method is checked for further adavancing in the state machine (e.g. when to power up the motors) */ - LaunchMethod* launchMethods[1]; + LaunchMethod *launchMethods[1]; control::BlockParamInt launchdetection_on; control::BlockParamFloat throttlePreTakeoff; diff --git a/src/lib/px4_eigen.h b/src/lib/px4_eigen.h new file mode 100644 index 000000000..2cd98e59a --- /dev/null +++ b/src/lib/px4_eigen.h @@ -0,0 +1,51 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file px4_eigen.h + * + * Compatability header to make Eigen compile on the PX4 stack + * @author Johan Jansen <jnsn.johan@gmail.com> + */ + +#pragma once + +#pragma GCC diagnostic push +#define RAND_MAX __RAND_MAX +#pragma GCC diagnostic ignored "-Wshadow" +#pragma GCC diagnostic ignored "-Wfloat-equal" +#define _GLIBCXX_USE_C99_FP_MACROS_DYNAMIC 1 + +#include <eigen/Eigen/Core> +#include <eigen/Eigen/Geometry> +#pragma GCC diagnostic pop diff --git a/src/lib/rc/module.mk b/src/lib/rc/module.mk index e089c6965..20f89c847 100644 --- a/src/lib/rc/module.mk +++ b/src/lib/rc/module.mk @@ -33,8 +33,10 @@ # # Yuntec ST24 transmitter protocol decoder +# Graupnet HoTT transmitter protocol decoder # -SRCS = st24.c +SRCS = st24.c \ + sumd.c MAXOPTIMIZATION = -Os diff --git a/src/lib/rc/st24.h b/src/lib/rc/st24.h index 454234601..0cf732824 100644 --- a/src/lib/rc/st24.h +++ b/src/lib/rc/st24.h @@ -158,6 +158,6 @@ uint8_t st24_common_crc8(uint8_t *ptr, uint8_t len); * @return 0 for success (a decoded packet), 1 for no packet yet (accumulating), 2 for unknown packet, 3 for out of sync, 4 for checksum error */ __EXPORT int st24_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channel_count, - uint16_t *channels, uint16_t max_chan_count); + uint16_t *channels, uint16_t max_chan_count); __END_DECLS diff --git a/src/lib/rc/sumd.c b/src/lib/rc/sumd.c new file mode 100644 index 000000000..a98c986bb --- /dev/null +++ b/src/lib/rc/sumd.c @@ -0,0 +1,319 @@ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* + * @file sumd.h + * + * RC protocol definition for Graupner HoTT transmitter (SUMD/SUMH Protocol) + * + * @author Marco Bauer <marco@wtns.de> + */ + +#include <stdbool.h> +#include <stdio.h> +#include "sumd.h" + + +enum SUMD_DECODE_STATE { + SUMD_DECODE_STATE_UNSYNCED = 0, + SUMD_DECODE_STATE_GOT_HEADER, + SUMD_DECODE_STATE_GOT_STATE, + SUMD_DECODE_STATE_GOT_LEN, + SUMD_DECODE_STATE_GOT_DATA, + SUMD_DECODE_STATE_GOT_CRC, + SUMD_DECODE_STATE_GOT_CRC16_BYTE_1, + SUMD_DECODE_STATE_GOT_CRC16_BYTE_2 +}; + +/* +const char *decode_states[] = {"UNSYNCED", + "GOT_HEADER", + "GOT_STATE", + "GOT_LEN", + "GOT_DATA", + "GOT_CRC", + "GOT_CRC16_BYTE_1", + "GOT_CRC16_BYTE_2" + }; +*/ + +uint8_t _crc8 = 0x00; +uint16_t _crc16 = 0x0000; +bool _sumd = true; +bool _crcOK = false; +bool _debug = false; + + +/* define range mapping here, -+100% -> 1000..2000 */ +#define SUMD_RANGE_MIN 0.0f +#define SUMD_RANGE_MAX 4096.0f + +#define SUMD_TARGET_MIN 1000.0f +#define SUMD_TARGET_MAX 2000.0f + +/* pre-calculate the floating point stuff as far as possible at compile time */ +#define SUMD_SCALE_FACTOR ((SUMD_TARGET_MAX - SUMD_TARGET_MIN) / (SUMD_RANGE_MAX - SUMD_RANGE_MIN)) +#define SUMD_SCALE_OFFSET (int)(SUMD_TARGET_MIN - (SUMD_SCALE_FACTOR * SUMD_RANGE_MIN + 0.5f)) + +static enum SUMD_DECODE_STATE _decode_state = SUMD_DECODE_STATE_UNSYNCED; +static uint8_t _rxlen; + +static ReceiverFcPacketHoTT _rxpacket; + + +uint16_t sumd_crc16(uint16_t crc, uint8_t value) +{ + int i; + crc ^= (uint16_t)value << 8; + for (i = 0; i < 8; i++) { + crc = (crc & 0x8000) ? (crc << 1) ^ 0x1021 : (crc << 1); + } + return crc; +} + +uint8_t sumd_crc8(uint8_t crc, uint8_t value) +{ + crc += value; + return crc; +} + +int sumd_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channel_count, uint16_t *channels, + uint16_t max_chan_count) +{ + + int ret = 1; + switch (_decode_state) { + case SUMD_DECODE_STATE_UNSYNCED: + if(_debug) + printf( " SUMD_DECODE_STATE_UNSYNCED \n") ; + + if (byte == SUMD_HEADER_ID) { + _rxpacket.header = byte; + _sumd = true; + _rxlen = 0; + _crc16 = 0x0000; + _crc8 = 0x00; + _crcOK = false; + _crc16 = sumd_crc16(_crc16, byte); + _crc8 = sumd_crc8(_crc8, byte); + _decode_state = SUMD_DECODE_STATE_GOT_HEADER; + if(_debug) + printf( " SUMD_DECODE_STATE_GOT_HEADER: %x \n", byte) ; + + } else { + ret = 3; + } + + break; + + case SUMD_DECODE_STATE_GOT_HEADER: + if (byte == SUMD_ID_SUMD || byte == SUMD_ID_SUMH) { + _rxpacket.status = byte; + if (byte == SUMD_ID_SUMH) { + _sumd = false; + } + if (_sumd) { + _crc16 = sumd_crc16(_crc16, byte); + } else { + _crc8 = sumd_crc8(_crc8, byte); + } + _decode_state = SUMD_DECODE_STATE_GOT_STATE; + if(_debug) + printf( " SUMD_DECODE_STATE_GOT_STATE: %x \n", byte) ; + + } else { + _decode_state = SUMD_DECODE_STATE_UNSYNCED; + } + + break; + + case SUMD_DECODE_STATE_GOT_STATE: + if (byte >= 2 && byte <= SUMD_MAX_CHANNELS) { + _rxpacket.length = byte; + if (_sumd) { + _crc16 = sumd_crc16(_crc16, byte); + } else { + _crc8 = sumd_crc8(_crc8, byte); + } + _rxlen++; + _decode_state = SUMD_DECODE_STATE_GOT_LEN; + if(_debug) + printf( " SUMD_DECODE_STATE_GOT_LEN: %x (%d) \n", byte, byte) ; + + } else { + _decode_state = SUMD_DECODE_STATE_UNSYNCED; + } + break; + + case SUMD_DECODE_STATE_GOT_LEN: + _rxpacket.sumd_data[_rxlen] = byte; + if (_sumd) { + _crc16 = sumd_crc16(_crc16, byte); + } else { + _crc8 = sumd_crc8(_crc8, byte); + } + _rxlen++; + + if (_rxlen <= ((_rxpacket.length*2) )) { + if(_debug) + printf( " SUMD_DECODE_STATE_GOT_DATA[%d]: %x\n", _rxlen-2, byte) ; + + } else { + _decode_state = SUMD_DECODE_STATE_GOT_DATA; + if(_debug) + printf( " SUMD_DECODE_STATE_GOT_DATA -- finish --\n") ; + + } + + break; + + case SUMD_DECODE_STATE_GOT_DATA: + _rxpacket.crc16_high = byte; + if(_debug) + printf( " SUMD_DECODE_STATE_GOT_CRC16[1]: %x [%x]\n", byte, ((_crc16 >> 8) & 0xff)) ; + + if (_sumd) { + _decode_state = SUMD_DECODE_STATE_GOT_CRC; + } else { + _decode_state = SUMD_DECODE_STATE_GOT_CRC16_BYTE_1; + } + + break; + + case SUMD_DECODE_STATE_GOT_CRC16_BYTE_1: + _rxpacket.crc16_low = byte; + if(_debug) + printf( " SUMD_DECODE_STATE_GOT_CRC16[2]: %x [%x]\n", byte, (_crc16 & 0xff)) ; + + _decode_state = SUMD_DECODE_STATE_GOT_CRC16_BYTE_2; + + break; + + case SUMD_DECODE_STATE_GOT_CRC16_BYTE_2: + _rxpacket.telemetry = byte; + if(_debug) + printf( " SUMD_DECODE_STATE_GOT_SUMH_TELEMETRY: %x\n", byte) ; + + _decode_state = SUMD_DECODE_STATE_GOT_CRC; + + break; + + case SUMD_DECODE_STATE_GOT_CRC: + if (_sumd) { + _rxpacket.crc16_low = byte; + if(_debug) + printf( " SUMD_DECODE_STATE_GOT_CRC[2]: %x [%x]\n\n", byte, (_crc16 & 0xff)) ; + + if (_crc16 == (uint16_t)(_rxpacket.crc16_high<<8)+_rxpacket.crc16_low) { + _crcOK = true; + } + } else { + _rxpacket.crc8 = byte; + if(_debug) + printf( " SUMD_DECODE_STATE_GOT_CRC8_SUMH: %x [%x]\n\n", byte, _crc8) ; + + if (_crc8 == _rxpacket.crc8) { + _crcOK = true; + } + } + + if (_crcOK) { + if(_debug) + printf( " CRC - OK \n") ; + + if (_sumd) { + if(_debug) + printf( " Got valid SUMD Packet\n") ; + + } else { + if(_debug) + printf( " Got valid SUMH Packet\n") ; + + } + + if(_debug) + printf( " RXLEN: %d [Chans: %d] \n\n", _rxlen-1, (_rxlen-1)/2) ; + + ret = 0; + unsigned i; + uint8_t _cnt = *rx_count + 1; + *rx_count = _cnt; + + *rssi = 255; + + /* received Channels */ + if ((uint16_t)_rxpacket.length > max_chan_count) { + _rxpacket.length = (uint8_t) max_chan_count; + } + *channel_count = (uint16_t)_rxpacket.length; + + /* decode the actual packet */ + /* reorder first 4 channels */ + + /* ch1 = roll -> sumd = ch2 */ + channels[0] = (uint16_t)((_rxpacket.sumd_data[1*2+1]<<8) | _rxpacket.sumd_data[1*2+2])>>3; + /* ch2 = pitch -> sumd = ch2 */ + channels[1] = (uint16_t)((_rxpacket.sumd_data[2*2+1]<<8) | _rxpacket.sumd_data[2*2+2])>>3; + /* ch3 = throttle -> sumd = ch2 */ + channels[2] = (uint16_t)((_rxpacket.sumd_data[0*2+1]<<8) | _rxpacket.sumd_data[0*2+2])>>3; + /* ch4 = yaw -> sumd = ch2 */ + channels[3] = (uint16_t)((_rxpacket.sumd_data[3*2+1]<<8) | _rxpacket.sumd_data[3*2+2])>>3; + + /* we start at channel 5(index 4) */ + unsigned chan_index = 4; + + for (i = 4; i < _rxpacket.length; i++) { + if(_debug) + printf( "ch[%d] : %x %x [ %x %d ]\n", i+1, _rxpacket.sumd_data[i*2+1], _rxpacket.sumd_data[i*2+2], ((_rxpacket.sumd_data[i*2+1]<<8) | _rxpacket.sumd_data[i*2+2])>>3, ((_rxpacket.sumd_data[i*2+1]<<8) | _rxpacket.sumd_data[i*2+2])>>3); + + channels[chan_index] = (uint16_t)((_rxpacket.sumd_data[i*2+1]<<8) | _rxpacket.sumd_data[i*2+2])>>3; + /* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */ + //channels[chan_index] = (uint16_t)(channels[chan_index] * SUMD_SCALE_FACTOR + .5f) + SUMD_SCALE_OFFSET; + + chan_index++; + } + + } else { + /* decoding failed */ + ret = 4; + if(_debug) + printf( " CRC - fail \n") ; + + } + + _decode_state = SUMD_DECODE_STATE_UNSYNCED; + break; + } + + return ret; +} diff --git a/src/lib/rc/sumd.h b/src/lib/rc/sumd.h new file mode 100644 index 000000000..f4793edfc --- /dev/null +++ b/src/lib/rc/sumd.h @@ -0,0 +1,108 @@ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file sumd.h + * + * RC protocol definition for Graupner HoTT transmitter + * + * @author Marco Bauer <marco@wtns.de> + */ + +#pragma once + +#include <stdint.h> + +__BEGIN_DECLS + +#define SUMD_MAX_CHANNELS 32 +#define SUMD_HEADER_LENGTH 3 +#define SUMD_HEADER_ID 0xA8 +#define SUMD_ID_SUMH 0x00 +#define SUMD_ID_SUMD 0x01 +#define SUMD_ID_FAILSAFE 0x81 + + + + +#pragma pack(push, 1) +typedef struct { + uint8_t header; ///< 0xA8 for a valid packet + uint8_t status; ///< 0x01 valid and live SUMD data frame / 0x00 = SUMH / 0x81 = Failsafe + uint8_t length; ///< Channels + uint8_t sumd_data[SUMD_MAX_CHANNELS*2]; ///< ChannelData (High Byte/ Low Byte) + uint8_t crc16_high; ///< High Byte of 16 Bit CRC + uint8_t crc16_low; ///< Low Byte of 16 Bit CRC + uint8_t telemetry; ///< Telemetry request + uint8_t crc8; ///< SUMH CRC8 +} ReceiverFcPacketHoTT; +#pragma pack(pop) + + +/** + * CRC16 implementation for SUMD protocol + * + * @param crc Initial CRC Value + * @param value to accumulate in the checksum + * @return the checksum + */ +uint16_t sumd_crc16(uint16_t crc, uint8_t value); + +/** + * CRC8 implementation for SUMH protocol + * + * @param crc Initial CRC Value + * @param value to accumulate in the checksum + * @return the checksum + */ +uint8_t sumd_crc8(uint8_t crc, uint8_t value); + +/** + * Decoder for SUMD/SUMH protocol + * + * @param byte current char to read + * @param rssi pointer to a byte where the RSSI value is written back to + * @param rx_count pointer to a byte where the receive count of packets signce last wireless frame is written back to + * @param channels pointer to a datastructure of size max_chan_count where channel values (12 bit) are written back to + * @param max_chan_count maximum channels to decode - if more channels are decoded, the last n are skipped and success (0) is returned + * @return 0 for success (a decoded packet), 1 for no packet yet (accumulating), 2 for unknown packet, 3 for out of sync, 4 for checksum error + */ +/* +__EXPORT int sumd_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channel_count, + uint16_t *channels, uint16_t max_chan_count); +*/ +int sumd_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channel_count, + uint16_t *channels, uint16_t max_chan_count); + + +__END_DECLS diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c index 5637ec102..e143f37b9 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c @@ -177,6 +177,7 @@ int parameters_update(const struct attitude_estimator_ekf_param_handles *h, stru for (int i = 0; i < 3; i++) { param_get(h->moment_inertia_J[i], &(p->moment_inertia_J[3 * i + i])); } + param_get(h->use_moment_inertia, &(p->use_moment_inertia)); return OK; diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index d70e05000..526b135d8 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -164,11 +164,6 @@ int do_accel_calibration(int mavlink_fd) mavlink_and_console_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name); - mavlink_and_console_log_info(mavlink_fd, "You need to put the system on all six sides"); - sleep(3); - mavlink_and_console_log_info(mavlink_fd, "Follow the instructions on the screen"); - sleep(5); - struct accel_scale accel_scale = { 0.0f, 1.0f, @@ -352,7 +347,7 @@ int do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_se (!data_collected[4]) ? "up " : ""); /* allow user enough time to read the message */ - sleep(3); + sleep(1); int orient = detect_orientation(mavlink_fd, subs); @@ -365,7 +360,7 @@ int do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_se /* inform user about already handled side */ if (data_collected[orient]) { mavlink_and_console_log_info(mavlink_fd, "%s side done, rotate to a different side", orientation_strs[orient]); - sleep(3); + sleep(1); continue; } @@ -374,7 +369,7 @@ int do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_se read_accelerometer_avg(subs, accel_ref, orient, samples_num); mavlink_and_console_log_info(mavlink_fd, "%s side done, rotate to a different side", orientation_strs[orient]); usleep(100000); - mavlink_and_console_log_info(mavlink_fd, "result for %s side: [ %.2f %.2f %.2f ]", orientation_strs[orient], + mavlink_and_console_log_info(mavlink_fd, "result for %s side: [ %8.4f %8.4f %8.4f ]", orientation_strs[orient], (double)accel_ref[0][orient][0], (double)accel_ref[0][orient][1], (double)accel_ref[0][orient][2]); @@ -399,13 +394,6 @@ int do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_se for (unsigned i = 0; i < (*active_sensors); i++) { res = calculate_calibration_values(i, accel_ref, accel_T, accel_offs, CONSTANTS_ONE_G); - /* verbose output on the console */ - printf("accel_T transformation matrix:\n"); - for (unsigned j = 0; j < 3; j++) { - printf(" %8.4f %8.4f %8.4f\n", (double)accel_T[i][j][0], (double)accel_T[i][j][1], (double)accel_T[i][j][2]); - } - printf("\n"); - if (res != OK) { mavlink_and_console_log_critical(mavlink_fd, "ERROR: calibration values calculation error"); break; @@ -635,7 +623,6 @@ int read_accelerometer_avg(int (&subs)[max_sens], float (&accel_avg)[max_sens][6 for (unsigned s = 0; s < max_sens; s++) { for (unsigned i = 0; i < 3; i++) { accel_avg[s][orient][i] = accel_sum[s][i] / counts[s]; - warnx("input: s:%u, axis: %u, orient: %u cnt: %u -> %8.4f", s, i, orient, counts[s], (double)accel_avg[s][orient][i]); } } diff --git a/src/modules/commander/calibration_routines.h b/src/modules/commander/calibration_routines.h index 3c8e49ee9..ba4ca07cc 100644 --- a/src/modules/commander/calibration_routines.h +++ b/src/modules/commander/calibration_routines.h @@ -57,4 +57,5 @@ * @return 0 on success, 1 on failure */ int sphere_fit_least_squares(const float x[], const float y[], const float z[], - unsigned int size, unsigned int max_iterations, float delta, float *sphere_x, float *sphere_y, float *sphere_z, float *sphere_radius);
\ No newline at end of file + unsigned int size, unsigned int max_iterations, float delta, float *sphere_x, float *sphere_y, float *sphere_z, + float *sphere_radius);
\ No newline at end of file diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index f832f761e..b8638c6a1 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -133,7 +133,7 @@ extern struct system_load_s system_load; #define STICK_ON_OFF_HYSTERESIS_TIME_MS 1000 #define STICK_ON_OFF_COUNTER_LIMIT (STICK_ON_OFF_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) -#define POSITION_TIMEOUT (2 * 1000 * 1000) /**< consider the local or global position estimate invalid after 600ms */ +#define POSITION_TIMEOUT (1 * 1000 * 1000) /**< consider the local or global position estimate invalid after 1000ms */ #define FAILSAFE_DEFAULT_TIMEOUT (3 * 1000 * 1000) /**< hysteresis time - the failsafe will trigger after 3 seconds in this state */ #define OFFBOARD_TIMEOUT 500000 #define DIFFPRESS_TIMEOUT 2000000 @@ -278,7 +278,7 @@ int commander_main(int argc, char *argv[]) daemon_task = task_spawn_cmd("commander", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 40, - 3200, + 3400, commander_thread_main, (argv) ? (char * const *)&argv[2] : (char * const *)NULL); @@ -727,6 +727,9 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s case VEHICLE_CMD_CUSTOM_2: case VEHICLE_CMD_PAYLOAD_PREPARE_DEPLOY: case VEHICLE_CMD_PAYLOAD_CONTROL_DEPLOY: + case VEHICLE_CMD_DO_MOUNT_CONTROL: + case VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT: + case VEHICLE_CMD_DO_MOUNT_CONFIGURE: /* ignore commands that handled in low prio loop */ break; @@ -964,19 +967,6 @@ int commander_thread_main(int argc, char *argv[]) int ret; - pthread_attr_t commander_low_prio_attr; - pthread_attr_init(&commander_low_prio_attr); - pthread_attr_setstacksize(&commander_low_prio_attr, 2400); - - struct sched_param param; - (void)pthread_attr_getschedparam(&commander_low_prio_attr, ¶m); - - /* low priority */ - param.sched_priority = SCHED_PRIORITY_DEFAULT - 50; - (void)pthread_attr_setschedparam(&commander_low_prio_attr, ¶m); - pthread_create(&commander_low_prio_thread, &commander_low_prio_attr, commander_low_prio_loop, NULL); - pthread_attr_destroy(&commander_low_prio_attr); - /* Start monitoring loop */ unsigned counter = 0; unsigned stick_off_counter = 0; @@ -1141,6 +1131,20 @@ int commander_thread_main(int argc, char *argv[]) bool main_state_changed = false; bool failsafe_old = false; + /* initialize low priority thread */ + pthread_attr_t commander_low_prio_attr; + pthread_attr_init(&commander_low_prio_attr); + pthread_attr_setstacksize(&commander_low_prio_attr, 2100); + + struct sched_param param; + (void)pthread_attr_getschedparam(&commander_low_prio_attr, ¶m); + + /* low priority */ + param.sched_priority = SCHED_PRIORITY_DEFAULT - 50; + (void)pthread_attr_setschedparam(&commander_low_prio_attr, ¶m); + pthread_create(&commander_low_prio_thread, &commander_low_prio_attr, commander_low_prio_loop, NULL); + pthread_attr_destroy(&commander_low_prio_attr); + while (!thread_should_exit) { if (mavlink_fd < 0 && counter % (1000000 / MAVLINK_OPEN_INTERVAL) == 0) { diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index b5ec6c4e6..a2e827a15 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -231,7 +231,7 @@ int led_init() /* then try RGB LEDs, this can fail on FMUv1*/ rgbleds = open(RGBLED0_DEVICE_PATH, 0); - if (rgbleds == -1) { + if (rgbleds < 0) { warnx("No RGB LED found at " RGBLED0_DEVICE_PATH); } @@ -240,50 +240,64 @@ int led_init() void led_deinit() { - close(leds); + if (leds >= 0) { + close(leds); + } - if (rgbleds != -1) { + if (rgbleds >= 0) { close(rgbleds); } } int led_toggle(int led) { + if (leds < 0) { + return leds; + } return ioctl(leds, LED_TOGGLE, led); } int led_on(int led) { + if (leds < 0) { + return leds; + } return ioctl(leds, LED_ON, led); } int led_off(int led) { + if (leds < 0) { + return leds; + } return ioctl(leds, LED_OFF, led); } void rgbled_set_color(rgbled_color_t color) { - if (rgbleds != -1) { - ioctl(rgbleds, RGBLED_SET_COLOR, (unsigned long)color); + if (rgbleds < 0) { + return; } + ioctl(rgbleds, RGBLED_SET_COLOR, (unsigned long)color); } void rgbled_set_mode(rgbled_mode_t mode) { - if (rgbleds != -1) { - ioctl(rgbleds, RGBLED_SET_MODE, (unsigned long)mode); + if (rgbleds < 0) { + return; } + ioctl(rgbleds, RGBLED_SET_MODE, (unsigned long)mode); } void rgbled_set_pattern(rgbled_pattern_t *pattern) { - if (rgbleds != -1) { - ioctl(rgbleds, RGBLED_SET_PATTERN, (unsigned long)pattern); + if (rgbleds < 0) { + return; } + ioctl(rgbleds, RGBLED_SET_PATTERN, (unsigned long)pattern); } float battery_remaining_estimate_voltage(float voltage, float discharged, float throttle_normalized) diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 1b0c4258b..94eeb6f71 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -55,6 +55,7 @@ PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f); * Defines the voltage where a single cell of the battery is considered empty. * * @group Battery Calibration + * @unit V */ PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.4f); @@ -64,6 +65,7 @@ PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.4f); * Defines the voltage where a single cell of the battery is considered full. * * @group Battery Calibration + * @unit V */ PARAM_DEFINE_FLOAT(BAT_V_CHARGED, 4.2f); @@ -74,6 +76,7 @@ PARAM_DEFINE_FLOAT(BAT_V_CHARGED, 4.2f); * to maximum current ratio and assumes linearity. * * @group Battery Calibration + * @unit V */ PARAM_DEFINE_FLOAT(BAT_V_LOAD_DROP, 0.07f); @@ -83,6 +86,7 @@ PARAM_DEFINE_FLOAT(BAT_V_LOAD_DROP, 0.07f); * Defines the number of cells the attached battery consists of. * * @group Battery Calibration + * @unit S */ PARAM_DEFINE_INT32(BAT_N_CELLS, 3); @@ -92,6 +96,7 @@ PARAM_DEFINE_INT32(BAT_N_CELLS, 3); * Defines the capacity of the attached battery. * * @group Battery Calibration + * @unit mA */ PARAM_DEFINE_FLOAT(BAT_CAPACITY, -1.0f); diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index dfd1657c5..8a70cf66e 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -73,16 +73,17 @@ int do_gyro_calibration(int mavlink_fd) /* wait for the user to respond */ sleep(2); - struct gyro_scale gyro_scale[max_gyros] = { { + struct gyro_scale gyro_scale_zero = { 0.0f, 1.0f, 0.0f, 1.0f, 0.0f, 1.0f, - } }; + struct gyro_scale gyro_scale[max_gyros]; + int res = OK; /* store board ID */ @@ -97,7 +98,7 @@ int do_gyro_calibration(int mavlink_fd) for (unsigned s = 0; s < max_gyros; s++) { /* ensure all scale fields are initialized tha same as the first struct */ - (void)memcpy(&gyro_scale[s], &gyro_scale[0], sizeof(gyro_scale[0])); + (void)memcpy(&gyro_scale[s], &gyro_scale_zero, sizeof(gyro_scale[0])); sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s); /* reset all offsets to zero and all scales to one */ @@ -109,7 +110,7 @@ int do_gyro_calibration(int mavlink_fd) device_id[s] = ioctl(fd, DEVIOCGDEVICEID, 0); - res = ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale); + res = ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale_zero); close(fd); if (res != OK) { @@ -120,6 +121,8 @@ int do_gyro_calibration(int mavlink_fd) unsigned calibration_counter[max_gyros] = { 0 }; const unsigned calibration_count = 5000; + struct gyro_report gyro_report_0 = {}; + if (res == OK) { /* determine gyro mean values */ unsigned poll_errcount = 0; @@ -140,7 +143,7 @@ int do_gyro_calibration(int mavlink_fd) while (calibration_counter[0] < calibration_count) { /* wait blocking for new data */ - int poll_ret = poll(fds, 1, 1000); + int poll_ret = poll(&fds[0], max_gyros, 1000); if (poll_ret > 0) { @@ -150,6 +153,11 @@ int do_gyro_calibration(int mavlink_fd) if (changed) { orb_copy(ORB_ID(sensor_gyro), sub_sensor_gyro[s], &gyro_report); + + if (s == 0) { + orb_copy(ORB_ID(sensor_gyro), sub_sensor_gyro[s], &gyro_report_0); + } + gyro_scale[s].x_offset += gyro_report.x; gyro_scale[s].y_offset += gyro_report.y; gyro_scale[s].z_offset += gyro_report.z; @@ -183,8 +191,20 @@ int do_gyro_calibration(int mavlink_fd) if (res == OK) { /* check offsets */ - if (!isfinite(gyro_scale[0].x_offset) || !isfinite(gyro_scale[0].y_offset) || !isfinite(gyro_scale[0].z_offset)) { - mavlink_log_critical(mavlink_fd, "ERROR: offset is NaN"); + float xdiff = gyro_report_0.x - gyro_scale[0].x_offset; + float ydiff = gyro_report_0.y - gyro_scale[0].y_offset; + float zdiff = gyro_report_0.z - gyro_scale[0].z_offset; + + /* maximum allowable calibration error in radians */ + const float maxoff = 0.01f; + + if (!isfinite(gyro_scale[0].x_offset) || + !isfinite(gyro_scale[0].y_offset) || + !isfinite(gyro_scale[0].z_offset) || + fabsf(xdiff) > maxoff || + fabsf(ydiff) > maxoff || + fabsf(zdiff) > maxoff) { + mavlink_log_critical(mavlink_fd, "ERROR: Calibration failed"); res = ERROR; } } @@ -214,6 +234,7 @@ int do_gyro_calibration(int mavlink_fd) int fd = open(str, 0); if (fd < 0) { + failed = true; continue; } @@ -226,7 +247,7 @@ int do_gyro_calibration(int mavlink_fd) } if (failed) { - mavlink_log_critical(mavlink_fd, "ERROR: failed to set offset params"); + mavlink_and_console_log_critical(mavlink_fd, "ERROR: failed to set offset params"); res = ERROR; } } diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index e0786db79..a0aadab39 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -164,9 +164,9 @@ int calibrate_instance(int mavlink_fd, unsigned s, unsigned device_id) const unsigned int calibration_maxcount = 240; unsigned int calibration_counter; - float *x = NULL; - float *y = NULL; - float *z = NULL; + float *x = new float[calibration_maxcount]; + float *y = new float[calibration_maxcount]; + float *z = new float[calibration_maxcount]; char str[30]; int res = OK; @@ -174,24 +174,20 @@ int calibrate_instance(int mavlink_fd, unsigned s, unsigned device_id) /* allocate memory */ mavlink_and_console_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 20); - x = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_maxcount)); - y = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_maxcount)); - z = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_maxcount)); - - if (x == NULL || y == NULL || z == NULL) { + if (x == nullptr || y == nullptr || z == nullptr) { mavlink_and_console_log_critical(mavlink_fd, "ERROR: out of memory"); /* clean up */ - if (x != NULL) { - free(x); + if (x != nullptr) { + delete x; } - if (y != NULL) { - free(y); + if (y != nullptr) { + delete y; } - if (z != NULL) { - free(z); + if (z != nullptr) { + delete z; } res = ERROR; @@ -274,16 +270,16 @@ int calibrate_instance(int mavlink_fd, unsigned s, unsigned device_id) } } - if (x != NULL) { - free(x); + if (x != nullptr) { + delete x; } - if (y != NULL) { - free(y); + if (y != nullptr) { + delete y; } - if (z != NULL) { - free(z); + if (z != nullptr) { + delete z; } if (res == OK) { diff --git a/src/modules/commander/module.mk b/src/modules/commander/module.mk index 4a1369aba..4ee8732fc 100644 --- a/src/modules/commander/module.mk +++ b/src/modules/commander/module.mk @@ -52,5 +52,5 @@ MODULE_STACKSIZE = 5000 MAXOPTIMIZATION = -Os -EXTRACXXFLAGS = -Wframe-larger-than=2000 +EXTRACXXFLAGS = -Wframe-larger-than=2200 diff --git a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h index 228ffa853..4d5e56a96 100644 --- a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h +++ b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h @@ -143,6 +143,7 @@ private: int _mission_sub; int _home_sub; /**< home position as defined by commander / user */ int _landDetectorSub; + int _armedSub; orb_advert_t _att_pub; /**< vehicle attitude */ orb_advert_t _global_pos_pub; /**< global position */ @@ -163,6 +164,7 @@ private: struct wind_estimate_s _wind; /**< wind estimate */ struct range_finder_report _distance; /**< distance estimate */ struct vehicle_land_detected_s _landDetector; + struct actuator_armed_s _armed; struct gyro_scale _gyro_offsets[3]; struct accel_scale _accel_offsets[3]; @@ -172,7 +174,6 @@ private: struct map_projection_reference_s _pos_ref; - float _baro_ref; /**< barometer reference altitude */ float _baro_ref_offset; /**< offset between initial baro reference and GPS init baro altitude */ float _baro_gps_offset; /**< offset between baro altitude (at GPS init time) and GPS altitude */ hrt_abstime _last_debug_print = 0; @@ -192,6 +193,7 @@ private: bool _gpsIsGood; ///< True if the current GPS fix is good enough for us to use uint64_t _previousGPSTimestamp; ///< Timestamp of last good GPS fix we have received bool _baro_init; + float _baroAltRef; bool _gps_initialized; hrt_abstime _filter_start_time; hrt_abstime _last_sensor_timestamp; @@ -206,7 +208,6 @@ private: bool _ekf_logging; ///< log EKF state unsigned _debug; ///< debug level - default 0 - bool _newDataGps; bool _newHgtData; bool _newAdsData; bool _newDataMag; diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 903158129..c16e72ea0 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -96,13 +96,13 @@ uint64_t getMicros() namespace estimator { - /* oddly, ERROR is not defined for c++ */ - #ifdef ERROR - # undef ERROR - #endif - static const int ERROR = -1; +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; - AttitudePositionEstimatorEKF *g_estimator = nullptr; +AttitudePositionEstimatorEKF *g_estimator = nullptr; } AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() : @@ -110,7 +110,7 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() : _task_running(false), _estimator_task(-1), - /* subscriptions */ +/* subscriptions */ _sensor_combined_sub(-1), _distance_sub(-1), _airspeed_sub(-1), @@ -122,8 +122,9 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() : _mission_sub(-1), _home_sub(-1), _landDetectorSub(-1), + _armedSub(-1), - /* publications */ +/* publications */ _att_pub(-1), _global_pos_pub(-1), _local_pos_pub(-1), @@ -131,71 +132,71 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() : _wind_pub(-1), _att({}), - _gyro({}), - _accel({}), - _mag({}), - _airspeed({}), - _baro({}), - _vstatus({}), - _global_pos({}), - _local_pos({}), - _gps({}), - _wind({}), - _distance{}, - _landDetector{}, - - _gyro_offsets({}), - _accel_offsets({}), - _mag_offsets({}), - - _sensor_combined{}, - - _pos_ref{}, - _baro_ref(0.0f), - _baro_ref_offset(0.0f), - _baro_gps_offset(0.0f), - - /* performance counters */ - _loop_perf(perf_alloc(PC_ELAPSED, "ekf_att_pos_estimator")), - _loop_intvl(perf_alloc(PC_INTERVAL, "ekf_att_pos_est_interval")), - _perf_gyro(perf_alloc(PC_INTERVAL, "ekf_att_pos_gyro_upd")), - _perf_mag(perf_alloc(PC_INTERVAL, "ekf_att_pos_mag_upd")), - _perf_gps(perf_alloc(PC_INTERVAL, "ekf_att_pos_gps_upd")), - _perf_baro(perf_alloc(PC_INTERVAL, "ekf_att_pos_baro_upd")), - _perf_airspeed(perf_alloc(PC_INTERVAL, "ekf_att_pos_aspd_upd")), - _perf_reset(perf_alloc(PC_COUNT, "ekf_att_pos_reset")), - - /* states */ - _gps_alt_filt(0.0f), - _baro_alt_filt(0.0f), - _covariancePredictionDt(0.0f), - _gpsIsGood(false), - _previousGPSTimestamp(0), - _baro_init(false), - _gps_initialized(false), - _filter_start_time(0), - _last_sensor_timestamp(0), - _last_run(0), - _distance_last_valid(0), - _gyro_valid(false), - _accel_valid(false), - _mag_valid(false), - _gyro_main(0), - _accel_main(0), - _mag_main(0), - _ekf_logging(true), - _debug(0), - - _newDataGps(false), - _newHgtData(false), - _newAdsData(false), - _newDataMag(false), - _newRangeData(false), - - _mavlink_fd(-1), - _parameters{}, - _parameter_handles{}, - _ekf(nullptr) + _gyro({}), + _accel({}), + _mag({}), + _airspeed({}), + _baro({}), + _vstatus({}), + _global_pos({}), + _local_pos({}), + _gps({}), + _wind({}), + _distance {}, + _landDetector {}, + _armed {}, + + _gyro_offsets({}), + _accel_offsets({}), + _mag_offsets({}), + + _sensor_combined {}, + + _pos_ref{}, + _baro_ref_offset(0.0f), + _baro_gps_offset(0.0f), + + /* performance counters */ + _loop_perf(perf_alloc(PC_ELAPSED, "ekf_att_pos_estimator")), + _loop_intvl(perf_alloc(PC_INTERVAL, "ekf_att_pos_est_interval")), + _perf_gyro(perf_alloc(PC_INTERVAL, "ekf_att_pos_gyro_upd")), + _perf_mag(perf_alloc(PC_INTERVAL, "ekf_att_pos_mag_upd")), + _perf_gps(perf_alloc(PC_INTERVAL, "ekf_att_pos_gps_upd")), + _perf_baro(perf_alloc(PC_INTERVAL, "ekf_att_pos_baro_upd")), + _perf_airspeed(perf_alloc(PC_INTERVAL, "ekf_att_pos_aspd_upd")), + _perf_reset(perf_alloc(PC_COUNT, "ekf_att_pos_reset")), + + /* states */ + _gps_alt_filt(0.0f), + _baro_alt_filt(0.0f), + _covariancePredictionDt(0.0f), + _gpsIsGood(false), + _previousGPSTimestamp(0), + _baro_init(false), + _baroAltRef(0.0f), + _gps_initialized(false), + _filter_start_time(0), + _last_sensor_timestamp(0), + _last_run(0), + _distance_last_valid(0), + _gyro_valid(false), + _accel_valid(false), + _mag_valid(false), + _gyro_main(0), + _accel_main(0), + _mag_main(0), + _ekf_logging(true), + _debug(0), + + _newHgtData(false), + _newAdsData(false), + _newDataMag(false), + _newRangeData(false), + + _mavlink_fd(-1), + _parameters {}, + _parameter_handles {}, + _ekf(nullptr) { _last_run = hrt_absolute_time(); @@ -334,7 +335,7 @@ int AttitudePositionEstimatorEKF::parameters_update() _ekf->posDSigma = _parameters.posd_noise; _ekf->magMeasurementSigma = _parameters.mag_noise; _ekf->gyroProcessNoise = _parameters.gyro_pnoise; - _ekf->accelProcessNoise = _parameters.acc_pnoise; + _ekf->accelProcessNoise = _parameters.acc_pnoise; _ekf->airspeedMeasurementSigma = _parameters.eas_noise; _ekf->rngFinderPitch = 0.0f; // XXX base on SENS_BOARD_Y_OFF } @@ -352,6 +353,9 @@ void AttitudePositionEstimatorEKF::vehicle_status_poll() if (vstatus_updated) { orb_copy(ORB_ID(vehicle_status), _vstatus_sub, &_vstatus); + + //Tell EKF that the vehicle is a fixed wing or multi-rotor + _ekf->setIsFixedWing(!_vstatus.is_rotary_wing); } } @@ -365,14 +369,15 @@ int AttitudePositionEstimatorEKF::check_filter_state() int check = _ekf->CheckAndBound(&ekf_report); - const char* const feedback[] = { 0, - "NaN in states, resetting", - "stale sensor data, resetting", - "got initial position lock", - "excessive gyro offsets", - "velocity diverted, check accel config", - "excessive covariances", - "unknown condition, resetting"}; + const char *const feedback[] = { 0, + "NaN in states, resetting", + "stale sensor data, resetting", + "got initial position lock", + "excessive gyro offsets", + "velocity diverted, check accel config", + "excessive covariances", + "unknown condition, resetting" + }; // Print out error condition if (check) { @@ -391,6 +396,7 @@ int AttitudePositionEstimatorEKF::check_filter_state() } struct estimator_status_report rep; + memset(&rep, 0, sizeof(rep)); // If error flag is set, we got a filter reset @@ -433,18 +439,18 @@ int AttitudePositionEstimatorEKF::check_filter_state() if (rep.health_flags < ((1 << 0) | (1 << 1) | (1 << 2) | (1 << 3))) { warnx("health: VEL:%s POS:%s HGT:%s OFFS:%s", - ((rep.health_flags & (1 << 0)) ? "OK" : "ERR"), - ((rep.health_flags & (1 << 1)) ? "OK" : "ERR"), - ((rep.health_flags & (1 << 2)) ? "OK" : "ERR"), - ((rep.health_flags & (1 << 3)) ? "OK" : "ERR")); + ((rep.health_flags & (1 << 0)) ? "OK" : "ERR"), + ((rep.health_flags & (1 << 1)) ? "OK" : "ERR"), + ((rep.health_flags & (1 << 2)) ? "OK" : "ERR"), + ((rep.health_flags & (1 << 3)) ? "OK" : "ERR")); } if (rep.timeout_flags) { warnx("timeout: %s%s%s%s", - ((rep.timeout_flags & (1 << 0)) ? "VEL " : ""), - ((rep.timeout_flags & (1 << 1)) ? "POS " : ""), - ((rep.timeout_flags & (1 << 2)) ? "HGT " : ""), - ((rep.timeout_flags & (1 << 3)) ? "IMU " : "")); + ((rep.timeout_flags & (1 << 0)) ? "VEL " : ""), + ((rep.timeout_flags & (1 << 1)) ? "POS " : ""), + ((rep.timeout_flags & (1 << 2)) ? "HGT " : ""), + ((rep.timeout_flags & (1 << 3)) ? "IMU " : "")); } } @@ -461,6 +467,7 @@ int AttitudePositionEstimatorEKF::check_filter_state() if (_estimator_status_pub > 0) { orb_publish(ORB_ID(estimator_status), _estimator_status_pub, &rep); + } else { _estimator_status_pub = orb_advertise(ORB_ID(estimator_status), &rep); } @@ -497,6 +504,7 @@ void AttitudePositionEstimatorEKF::task_main() _params_sub = orb_subscribe(ORB_ID(parameter_update)); _home_sub = orb_subscribe(ORB_ID(home_position)); _landDetectorSub = orb_subscribe(ORB_ID(vehicle_land_detected)); + _armedSub = orb_subscribe(ORB_ID(actuator_armed)); /* rate limit vehicle status updates to 5Hz */ orb_set_interval(_vstatus_sub, 200); @@ -530,8 +538,9 @@ void AttitudePositionEstimatorEKF::task_main() int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); /* timed out - periodic check for _task_should_exit, etc. */ - if (pret == 0) + if (pret == 0) { continue; + } /* this is undesirable but not much we can do - might want to flag unhappy status */ if (pret < 0) { @@ -590,7 +599,7 @@ void AttitudePositionEstimatorEKF::task_main() /** * PART ONE: COLLECT ALL DATA **/ - pollData(); + pollData(); /* * CHECK IF ITS THE RIGHT TIME TO RUN THINGS ALREADY @@ -626,25 +635,26 @@ void AttitudePositionEstimatorEKF::task_main() // } /* Initialize the filter first */ - if (!_gps_initialized && _gpsIsGood) { - initializeGPS(); - } - else if (!_ekf->statesInitialised) { + if (!_ekf->statesInitialised) { // North, East Down position (m) - float initVelNED[3] = {0.0f, 0.0f, 0.0f}; + float initVelNED[3] = {0.0f, 0.0f, 0.0f}; _ekf->posNE[0] = 0.0f; _ekf->posNE[1] = 0.0f; - _local_pos.ref_alt = _baro_ref; + _local_pos.ref_alt = 0.0f; _baro_ref_offset = 0.0f; _baro_gps_offset = 0.0f; _baro_alt_filt = _baro.altitude; _ekf->InitialiseFilter(initVelNED, 0.0, 0.0, 0.0f, 0.0f); - } - else if (_ekf->statesInitialised) { + } else { + + if (!_gps_initialized && _gpsIsGood) { + initializeGPS(); + continue; + } // Check if on ground - status is used by covariance prediction _ekf->setOnGround(_landDetector.landed); @@ -652,13 +662,14 @@ void AttitudePositionEstimatorEKF::task_main() // We're apparently initialized in this case now // check (and reset the filter as needed) int check = check_filter_state(); + if (check) { // Let the system re-initialize itself continue; } //Run EKF data fusion steps - updateSensorFusion(_newDataGps, _newDataMag, _newRangeData, _newHgtData, _newAdsData); + updateSensorFusion(_gpsIsGood, _newDataMag, _newRangeData, _newHgtData, _newAdsData); //Publish attitude estimations publishAttitude(); @@ -667,8 +678,7 @@ void AttitudePositionEstimatorEKF::task_main() publishLocalPosition(); //Publish Global Position, but only if it's any good - if(_gps_initialized && _gpsIsGood) - { + if (_gps_initialized && (_gpsIsGood || _global_pos.dead_reckoning)) { publishGlobalPosition(); } @@ -708,7 +718,7 @@ void AttitudePositionEstimatorEKF::initializeGPS() _baro_alt_filt = _baro.altitude; _ekf->baroHgt = _baro.altitude; - _ekf->hgtMea = 1.0f * (_ekf->baroHgt - (_baro_ref)); + _ekf->hgtMea = _ekf->baroHgt; // Set up position variables correctly _ekf->GPSstatus = _gps.fix_type; @@ -736,12 +746,14 @@ void AttitudePositionEstimatorEKF::initializeGPS() map_projection_init(&_pos_ref, lat, lon); mavlink_log_info(_mavlink_fd, "[ekf] ref: LA %.4f,LO %.4f,ALT %.2f", lat, lon, (double)gps_alt); - #if 0 +#if 0 warnx("HOME/REF: LA %8.4f,LO %8.4f,ALT %8.2f V: %8.4f %8.4f %8.4f", lat, lon, (double)gps_alt, - (double)_ekf->velNED[0], (double)_ekf->velNED[1], (double)_ekf->velNED[2]); - warnx("BARO: %8.4f m / ref: %8.4f m / gps offs: %8.4f m", (double)_ekf->baroHgt, (double)_baro_ref, (double)_baro_ref_offset); - warnx("GPS: eph: %8.4f, epv: %8.4f, declination: %8.4f", (double)_gps.eph, (double)_gps.epv, (double)math::degrees(declination)); - #endif + (double)_ekf->velNED[0], (double)_ekf->velNED[1], (double)_ekf->velNED[2]); + warnx("BARO: %8.4f m / ref: %8.4f m / gps offs: %8.4f m", (double)_ekf->baroHgt, (double)_baro_ref, + (double)_baro_ref_offset); + warnx("GPS: eph: %8.4f, epv: %8.4f, declination: %8.4f", (double)_gps.eph, (double)_gps.epv, + (double)math::degrees(declination)); +#endif _gps_initialized = true; } @@ -772,14 +784,14 @@ void AttitudePositionEstimatorEKF::publishAttitude() _att.pitch = euler(1); _att.yaw = euler(2); - _att.rollspeed = _ekf->angRate.x - _ekf->states[10] / _ekf->dtIMU; - _att.pitchspeed = _ekf->angRate.y - _ekf->states[11] / _ekf->dtIMU; - _att.yawspeed = _ekf->angRate.z - _ekf->states[12] / _ekf->dtIMU; + _att.rollspeed = _ekf->angRate.x - _ekf->states[10] / _ekf->dtIMUfilt; + _att.pitchspeed = _ekf->angRate.y - _ekf->states[11] / _ekf->dtIMUfilt; + _att.yawspeed = _ekf->angRate.z - _ekf->states[12] / _ekf->dtIMUfilt; // gyro offsets - _att.rate_offsets[0] = _ekf->states[10] / _ekf->dtIMU; - _att.rate_offsets[1] = _ekf->states[11] / _ekf->dtIMU; - _att.rate_offsets[2] = _ekf->states[12] / _ekf->dtIMU; + _att.rate_offsets[0] = _ekf->states[10] / _ekf->dtIMUfilt; + _att.rate_offsets[1] = _ekf->states[11] / _ekf->dtIMUfilt; + _att.rate_offsets[2] = _ekf->states[12] / _ekf->dtIMUfilt; /* lazily publish the attitude only once available */ if (_att_pub > 0) { @@ -799,7 +811,7 @@ void AttitudePositionEstimatorEKF::publishLocalPosition() _local_pos.y = _ekf->states[8]; // XXX need to announce change of Z reference somehow elegantly - _local_pos.z = _ekf->states[9] - _baro_ref_offset; + _local_pos.z = _ekf->states[9] - _baro_ref_offset - _baroAltRef; _local_pos.vx = _ekf->states[4]; _local_pos.vy = _ekf->states[5]; @@ -840,13 +852,14 @@ void AttitudePositionEstimatorEKF::publishGlobalPosition() if (_local_pos.v_xy_valid) { _global_pos.vel_n = _local_pos.vx; _global_pos.vel_e = _local_pos.vy; + } else { _global_pos.vel_n = 0.0f; _global_pos.vel_e = 0.0f; } /* local pos alt is negative, change sign and add alt offsets */ - _global_pos.alt = _baro_ref + (-_local_pos.z) - _baro_gps_offset; + _global_pos.alt = (-_local_pos.z) - _baro_gps_offset; if (_local_pos.v_z_valid) { _global_pos.vel_d = _local_pos.vz; @@ -855,7 +868,7 @@ void AttitudePositionEstimatorEKF::publishGlobalPosition() /* terrain altitude */ _global_pos.terrain_alt = _ekf->hgtRef - _ekf->flowStates[1]; _global_pos.terrain_alt_valid = (_distance_last_valid > 0) && - (hrt_elapsed_time(&_distance_last_valid) < 20 * 1000 * 1000); + (hrt_elapsed_time(&_distance_last_valid) < 20 * 1000 * 1000); _global_pos.yaw = _local_pos.yaw; _global_pos.eph = _gps.eph; @@ -895,8 +908,8 @@ void AttitudePositionEstimatorEKF::publishWindEstimate() } -void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const bool fuseMag, const bool fuseRangeSensor, - const bool fuseBaro, const bool fuseAirSpeed) +void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const bool fuseMag, + const bool fuseRangeSensor, const bool fuseBaro, const bool fuseAirSpeed) { // Run the strapdown INS equations every IMU update _ekf->UpdateStrapdownEquationsNED(); @@ -911,8 +924,8 @@ void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const // perform a covariance prediction if the total delta angle has exceeded the limit // or the time limit will be exceeded at the next IMU update - if ((_covariancePredictionDt >= (_ekf->covTimeStepMax - _ekf->dtIMU)) - || (_ekf->summedDelAng.length() > _ekf->covDelAngMax)) { + if ((_covariancePredictionDt >= (_ekf->covTimeStepMax - _ekf->dtIMU)) + || (_ekf->summedDelAng.length() > _ekf->covDelAngMax)) { _ekf->CovariancePrediction(_covariancePredictionDt); _ekf->summedDelAng.zero(); _ekf->summedDelVel.zero(); @@ -934,8 +947,7 @@ void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const // run the fusion step _ekf->FuseVelposNED(); - } - else if (!_gps_initialized) { + } else if (!_gps_initialized) { // force static mode _ekf->staticMode = true; @@ -959,15 +971,14 @@ void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const // run the fusion step _ekf->FuseVelposNED(); - } - else { + } else { _ekf->fuseVelData = false; _ekf->fusePosData = false; } if (fuseBaro) { // Could use a blend of GPS and baro alt data if desired - _ekf->hgtMea = 1.0f * (_ekf->baroHgt - _baro_ref); + _ekf->hgtMea = _ekf->baroHgt; _ekf->fuseHgtData = true; // recall states stored at time of measurement after adjusting for delays @@ -976,34 +987,33 @@ void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const // run the fusion step _ekf->FuseVelposNED(); - } - else { + } else { _ekf->fuseHgtData = false; } // Fuse Magnetometer Measurements if (fuseMag) { _ekf->fuseMagData = true; - _ekf->RecallStates(_ekf->statesAtMagMeasTime, (IMUmsec - _parameters.mag_delay_ms)); // Assume 50 msec avg delay for magnetometer data + _ekf->RecallStates(_ekf->statesAtMagMeasTime, + (IMUmsec - _parameters.mag_delay_ms)); // Assume 50 msec avg delay for magnetometer data _ekf->magstate.obsIndex = 0; _ekf->FuseMagnetometer(); _ekf->FuseMagnetometer(); _ekf->FuseMagnetometer(); - } - else { + } else { _ekf->fuseMagData = false; } // Fuse Airspeed Measurements if (fuseAirSpeed && _ekf->VtasMeas > 7.0f) { _ekf->fuseVtasData = true; - _ekf->RecallStates(_ekf->statesAtVtasMeasTime, (IMUmsec - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data + _ekf->RecallStates(_ekf->statesAtVtasMeasTime, + (IMUmsec - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data _ekf->FuseAirspeed(); - } - else { + } else { _ekf->fuseVtasData = false; } @@ -1059,48 +1069,69 @@ void AttitudePositionEstimatorEKF::print_status() // 16-18: Earth Magnetic Field Vector - gauss (North, East, Down) // 19-21: Body Magnetic Field Vector - gauss (X,Y,Z) - printf("dtIMU: %8.6f IMUmsec: %d\n", (double)_ekf->dtIMU, (int)IMUmsec); + printf("dtIMU: %8.6f filt: %8.6f IMUmsec: %d\n", (double)_ekf->dtIMU, (double)_ekf->dtIMUfilt, (int)IMUmsec); printf("baro alt: %8.4f GPS alt: %8.4f\n", (double)_baro.altitude, (double)(_gps.alt / 1e3f)); - printf("ref alt: %8.4f baro ref offset: %8.4f baro GPS offset: %8.4f\n", (double)_baro_ref, (double)_baro_ref_offset, (double)_baro_gps_offset); - printf("dvel: %8.6f %8.6f %8.6f accel: %8.6f %8.6f %8.6f\n", (double)_ekf->dVelIMU.x, (double)_ekf->dVelIMU.y, (double)_ekf->dVelIMU.z, (double)_ekf->accel.x, (double)_ekf->accel.y, (double)_ekf->accel.z); - printf("dang: %8.4f %8.4f %8.4f dang corr: %8.4f %8.4f %8.4f\n" , (double)_ekf->dAngIMU.x, (double)_ekf->dAngIMU.y, (double)_ekf->dAngIMU.z, (double)_ekf->correctedDelAng.x, (double)_ekf->correctedDelAng.y, (double)_ekf->correctedDelAng.z); - printf("states (quat) [0-3]: %8.4f, %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[0], (double)_ekf->states[1], (double)_ekf->states[2], (double)_ekf->states[3]); - printf("states (vel m/s) [4-6]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[4], (double)_ekf->states[5], (double)_ekf->states[6]); - printf("states (pos m) [7-9]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[7], (double)_ekf->states[8], (double)_ekf->states[9]); - printf("states (delta ang) [10-12]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[10], (double)_ekf->states[11], (double)_ekf->states[12]); + printf("baro ref offset: %8.4f baro GPS offset: %8.4f\n", (double)_baro_ref_offset, + (double)_baro_gps_offset); + printf("dvel: %8.6f %8.6f %8.6f accel: %8.6f %8.6f %8.6f\n", (double)_ekf->dVelIMU.x, (double)_ekf->dVelIMU.y, + (double)_ekf->dVelIMU.z, (double)_ekf->accel.x, (double)_ekf->accel.y, (double)_ekf->accel.z); + printf("dang: %8.4f %8.4f %8.4f dang corr: %8.4f %8.4f %8.4f\n" , (double)_ekf->dAngIMU.x, (double)_ekf->dAngIMU.y, + (double)_ekf->dAngIMU.z, (double)_ekf->correctedDelAng.x, (double)_ekf->correctedDelAng.y, + (double)_ekf->correctedDelAng.z); + printf("states (quat) [0-3]: %8.4f, %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[0], (double)_ekf->states[1], + (double)_ekf->states[2], (double)_ekf->states[3]); + printf("states (vel m/s) [4-6]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[4], (double)_ekf->states[5], + (double)_ekf->states[6]); + printf("states (pos m) [7-9]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[7], (double)_ekf->states[8], + (double)_ekf->states[9]); + printf("states (delta ang) [10-12]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[10], (double)_ekf->states[11], + (double)_ekf->states[12]); if (EKF_STATE_ESTIMATES == 23) { printf("states (accel offs) [13]: %8.4f\n", (double)_ekf->states[13]); printf("states (wind) [14-15]: %8.4f, %8.4f\n", (double)_ekf->states[14], (double)_ekf->states[15]); - printf("states (earth mag) [16-18]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[16], (double)_ekf->states[17], (double)_ekf->states[18]); - printf("states (body mag) [19-21]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[19], (double)_ekf->states[20], (double)_ekf->states[21]); + printf("states (earth mag) [16-18]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[16], (double)_ekf->states[17], + (double)_ekf->states[18]); + printf("states (body mag) [19-21]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[19], (double)_ekf->states[20], + (double)_ekf->states[21]); printf("states (terrain) [22]: %8.4f\n", (double)_ekf->states[22]); } else { printf("states (wind) [13-14]: %8.4f, %8.4f\n", (double)_ekf->states[13], (double)_ekf->states[14]); - printf("states (earth mag) [15-17]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[15], (double)_ekf->states[16], (double)_ekf->states[17]); - printf("states (body mag) [18-20]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[18], (double)_ekf->states[19], (double)_ekf->states[20]); + printf("states (earth mag) [15-17]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[15], (double)_ekf->states[16], + (double)_ekf->states[17]); + printf("states (body mag) [18-20]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[18], (double)_ekf->states[19], + (double)_ekf->states[20]); } + printf("states: %s %s %s %s %s %s %s %s %s %s\n", - (_ekf->statesInitialised) ? "INITIALIZED" : "NON_INIT", - (_landDetector.landed) ? "ON_GROUND" : "AIRBORNE", - (_ekf->fuseVelData) ? "FUSE_VEL" : "INH_VEL", - (_ekf->fusePosData) ? "FUSE_POS" : "INH_POS", - (_ekf->fuseHgtData) ? "FUSE_HGT" : "INH_HGT", - (_ekf->fuseMagData) ? "FUSE_MAG" : "INH_MAG", - (_ekf->fuseVtasData) ? "FUSE_VTAS" : "INH_VTAS", - (_ekf->useAirspeed) ? "USE_AIRSPD" : "IGN_AIRSPD", - (_ekf->useCompass) ? "USE_COMPASS" : "IGN_COMPASS", - (_ekf->staticMode) ? "STATIC_MODE" : "DYNAMIC_MODE"); + (_ekf->statesInitialised) ? "INITIALIZED" : "NON_INIT", + (_landDetector.landed) ? "ON_GROUND" : "AIRBORNE", + (_ekf->fuseVelData) ? "FUSE_VEL" : "INH_VEL", + (_ekf->fusePosData) ? "FUSE_POS" : "INH_POS", + (_ekf->fuseHgtData) ? "FUSE_HGT" : "INH_HGT", + (_ekf->fuseMagData) ? "FUSE_MAG" : "INH_MAG", + (_ekf->fuseVtasData) ? "FUSE_VTAS" : "INH_VTAS", + (_ekf->useAirspeed) ? "USE_AIRSPD" : "IGN_AIRSPD", + (_ekf->useCompass) ? "USE_COMPASS" : "IGN_COMPASS", + (_ekf->staticMode) ? "STATIC_MODE" : "DYNAMIC_MODE"); } void AttitudePositionEstimatorEKF::pollData() { + //Update arming status + bool armedUpdate; + orb_check(_armedSub, &armedUpdate); + + if (armedUpdate) { + orb_copy(ORB_ID(actuator_armed), _armedSub, &_armed); + } + + //Update Gyro and Accelerometer static Vector3f lastAngRate; static Vector3f lastAccel; bool accel_updated = false; - //Update Gyro and Accelerometer orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined); static hrt_abstime last_accel = 0; @@ -1108,6 +1139,7 @@ void AttitudePositionEstimatorEKF::pollData() if (last_accel != _sensor_combined.accelerometer_timestamp) { accel_updated = true; + } else { accel_updated = false; } @@ -1136,9 +1168,9 @@ void AttitudePositionEstimatorEKF::pollData() int last_gyro_main = _gyro_main; if (isfinite(_sensor_combined.gyro_rad_s[0]) && - isfinite(_sensor_combined.gyro_rad_s[1]) && - isfinite(_sensor_combined.gyro_rad_s[2]) && - (_sensor_combined.gyro_errcount <= _sensor_combined.gyro1_errcount)) { + isfinite(_sensor_combined.gyro_rad_s[1]) && + isfinite(_sensor_combined.gyro_rad_s[2]) && + (_sensor_combined.gyro_errcount <= _sensor_combined.gyro1_errcount)) { _ekf->angRate.x = _sensor_combined.gyro_rad_s[0]; _ekf->angRate.y = _sensor_combined.gyro_rad_s[1]; @@ -1147,8 +1179,8 @@ void AttitudePositionEstimatorEKF::pollData() _gyro_valid = true; } else if (isfinite(_sensor_combined.gyro1_rad_s[0]) && - isfinite(_sensor_combined.gyro1_rad_s[1]) && - isfinite(_sensor_combined.gyro1_rad_s[2])) { + isfinite(_sensor_combined.gyro1_rad_s[1]) && + isfinite(_sensor_combined.gyro1_rad_s[2])) { _ekf->angRate.x = _sensor_combined.gyro1_rad_s[0]; _ekf->angRate.y = _sensor_combined.gyro1_rad_s[1]; @@ -1167,6 +1199,7 @@ void AttitudePositionEstimatorEKF::pollData() if (!_gyro_valid) { /* keep last value if he hit an out of band value */ lastAngRate = _ekf->angRate; + } else { perf_count(_perf_gyro); } @@ -1181,6 +1214,7 @@ void AttitudePositionEstimatorEKF::pollData() _ekf->accel.y = _sensor_combined.accelerometer_m_s2[1]; _ekf->accel.z = _sensor_combined.accelerometer_m_s2[2]; _accel_main = 0; + } else { _ekf->accel.x = _sensor_combined.accelerometer1_m_s2[0]; _ekf->accel.y = _sensor_combined.accelerometer1_m_s2[1]; @@ -1218,12 +1252,14 @@ void AttitudePositionEstimatorEKF::pollData() //Update Land Detector bool newLandData; orb_check(_landDetectorSub, &newLandData); - if(newLandData) { + + if (newLandData) { orb_copy(ORB_ID(vehicle_land_detected), _landDetectorSub, &_landDetector); } //Update AirSpeed orb_check(_airspeed_sub, &_newAdsData); + if (_newAdsData) { orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed); perf_count(_perf_airspeed); @@ -1232,65 +1268,70 @@ void AttitudePositionEstimatorEKF::pollData() } - orb_check(_gps_sub, &_newDataGps); - if (_newDataGps) { + bool gps_update; + orb_check(_gps_sub, &gps_update); + if (gps_update) { orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &_gps); perf_count(_perf_gps); //We are more strict for our first fix float requiredAccuracy = _parameters.pos_stddev_threshold; - if(_gpsIsGood) { + + if (_gpsIsGood) { requiredAccuracy = _parameters.pos_stddev_threshold * 2.0f; } //Check if the GPS fix is good enough for us to use - if(_gps.fix_type >= 3 && _gps.eph < requiredAccuracy && _gps.epv < requiredAccuracy) { + if (_gps.fix_type >= 3 && _gps.eph < requiredAccuracy && _gps.epv < requiredAccuracy) { _gpsIsGood = true; - } - else { + + } else { _gpsIsGood = false; } if (_gpsIsGood) { //Calculate time since last good GPS fix - const float dtGoodGPS = static_cast<float>(_gps.timestamp_position - _previousGPSTimestamp) / 1e6f; + const float dtLastGoodGPS = static_cast<float>(_gps.timestamp_position - _previousGPSTimestamp) / 1e6f; - _ekf->updateDtGpsFilt(math::constrain(dtGoodGPS, 0.01f, POS_RESET_THRESHOLD)); + //Stop dead-reckoning mode + if (_global_pos.dead_reckoning) { + mavlink_log_info(_mavlink_fd, "[ekf] stop dead-reckoning"); + } - /* fuse GPS updates */ + _global_pos.dead_reckoning = false; - //_gps.timestamp / 1e3; + //Fetch new GPS data _ekf->GPSstatus = _gps.fix_type; _ekf->velNED[0] = _gps.vel_n_m_s; _ekf->velNED[1] = _gps.vel_e_m_s; _ekf->velNED[2] = _gps.vel_d_m_s; - // warnx("GPS updated: status: %d, vel: %8.4f %8.4f %8.4f", (int)GPSstatus, velNED[0], velNED[1], velNED[2]); - _ekf->gpsLat = math::radians(_gps.lat / (double)1e7); _ekf->gpsLon = math::radians(_gps.lon / (double)1e7) - M_PI; _ekf->gpsHgt = _gps.alt / 1e3f; + if (_previousGPSTimestamp != 0) { + //Calculate average time between GPS updates + _ekf->updateDtGpsFilt(math::constrain(dtLastGoodGPS, 0.01f, POS_RESET_THRESHOLD)); + + // update LPF + _gps_alt_filt += (dtLastGoodGPS / (rc + dtLastGoodGPS)) * (_ekf->gpsHgt - _gps_alt_filt); + } + //check if we had a GPS outage for a long time - if(_gps_initialized) { + if (_gps_initialized) { //Convert from global frame to local frame - float posNED[3] = {0.0f, 0.0f, 0.0f}; - _ekf->calcposNED(posNED, _ekf->gpsLat, _ekf->gpsLon, _ekf->gpsHgt, _ekf->latRef, _ekf->lonRef, _ekf->hgtRef); - _ekf->posNE[0] = posNED[0]; - _ekf->posNE[1] = posNED[1]; + map_projection_project(&_pos_ref, (_gps.lat / 1.0e7), (_gps.lon / 1.0e7), &_ekf->posNE[0], &_ekf->posNE[1]); - if (dtGoodGPS > POS_RESET_THRESHOLD) { + if (dtLastGoodGPS > POS_RESET_THRESHOLD) { _ekf->ResetPosition(); _ekf->ResetVelocity(); } } - // update LPF - _gps_alt_filt += (dtGoodGPS / (rc + dtGoodGPS)) * (_ekf->gpsHgt - _gps_alt_filt); - //warnx("gps alt: %6.1f, interval: %6.3f", (double)_ekf->gpsHgt, (double)dtGoodGPS); // if (_gps.s_variance_m_s > 0.25f && _gps.s_variance_m_s < 100.0f * 100.0f) { @@ -1308,17 +1349,36 @@ void AttitudePositionEstimatorEKF::pollData() // warnx("vel: %8.4f pos: %8.4f", _gps.s_variance_m_s, _gps.p_variance_m); _previousGPSTimestamp = _gps.timestamp_position; - } - else { - //Too poor GPS fix to use - _newDataGps = false; + } } // If it has gone more than POS_RESET_THRESHOLD amount of seconds since we received a GPS update, // then something is very wrong with the GPS (possibly a hardware failure or comlink error) - else if( (static_cast<float>(hrt_elapsed_time(&_gps.timestamp_position)) / 1e6f) > POS_RESET_THRESHOLD) { + const float dtLastGoodGPS = static_cast<float>(_gps.timestamp_position - _previousGPSTimestamp) / 1e6f; + + if (dtLastGoodGPS >= POS_RESET_THRESHOLD) { + + if (_global_pos.dead_reckoning) { + mavlink_log_info(_mavlink_fd, "[ekf] gave up dead-reckoning after long timeout"); + } + _gpsIsGood = false; + _global_pos.dead_reckoning = false; + } + + //If we have no good GPS fix for half a second, then enable dead-reckoning mode while armed (for up to POS_RESET_THRESHOLD seconds) + else if (dtLastGoodGPS >= 0.5f) { + if (_armed.armed) { + if (!_global_pos.dead_reckoning) { + mavlink_log_info(_mavlink_fd, "[ekf] dead-reckoning enabled"); + } + + _global_pos.dead_reckoning = true; + + } else { + _global_pos.dead_reckoning = false; + } } //Update barometer @@ -1331,28 +1391,26 @@ void AttitudePositionEstimatorEKF::pollData() // init lowpass filters for baro and gps altitude float baro_elapsed; - if(baro_last == 0) { + + if (baro_last == 0) { baro_elapsed = 0.0f; - } - else { + + } else { baro_elapsed = (_baro.timestamp - baro_last) / 1e6f; } + baro_last = _baro.timestamp; + if(!_baro_init) { + _baro_init = true; + _baroAltRef = _baro.altitude; + } _ekf->updateDtHgtFilt(math::constrain(baro_elapsed, 0.001f, 0.1f)); _ekf->baroHgt = _baro.altitude; - _baro_alt_filt += (baro_elapsed/(rc + baro_elapsed)) * (_baro.altitude - _baro_alt_filt); - - if (!_baro_init) { - _baro_ref = _baro.altitude; - _baro_init = true; - warnx("ALT REF INIT"); - } + _baro_alt_filt += (baro_elapsed / (rc + baro_elapsed)) * (_baro.altitude - _baro_alt_filt); perf_count(_perf_baro); - - _newHgtData = true; } //Update Magnetometer @@ -1377,6 +1435,7 @@ void AttitudePositionEstimatorEKF::pollData() _ekf->magData.z = _sensor_combined.magnetometer_ga[2]; _ekf->magBias.z = 0.000001f; // _mag_offsets.y_offset _mag_main = 0; + } else { _ekf->magData.x = _sensor_combined.magnetometer1_ga[0]; _ekf->magBias.x = 0.000001f; // _mag_offsets.x_offset @@ -1396,31 +1455,30 @@ void AttitudePositionEstimatorEKF::pollData() //Update range data orb_check(_distance_sub, &_newRangeData); + if (_newRangeData) { orb_copy(ORB_ID(sensor_range_finder), _distance_sub, &_distance); if (_distance.valid) { _ekf->rngMea = _distance.distance; _distance_last_valid = _distance.timestamp; + } else { _newRangeData = false; } } } -int AttitudePositionEstimatorEKF::trip_nan() { +int AttitudePositionEstimatorEKF::trip_nan() +{ int ret = 0; // If system is not armed, inject a NaN value into the filter - int armed_sub = orb_subscribe(ORB_ID(actuator_armed)); - - struct actuator_armed_s armed; - orb_copy(ORB_ID(actuator_armed), armed_sub, &armed); - - if (armed.armed) { + if (_armed.armed) { warnx("ACTUATORS ARMED! NOT TRIPPING SYSTEM"); ret = 1; + } else { float nan_val = 0.0f / 0.0f; @@ -1453,24 +1511,26 @@ int AttitudePositionEstimatorEKF::trip_nan() { print_status(); } - close(armed_sub); return ret; } int ekf_att_pos_estimator_main(int argc, char *argv[]) { - if (argc < 1) + if (argc < 1) { errx(1, "usage: ekf_att_pos_estimator {start|stop|status|logging}"); + } if (!strcmp(argv[1], "start")) { - if (estimator::g_estimator != nullptr) + if (estimator::g_estimator != nullptr) { errx(1, "already running"); + } estimator::g_estimator = new AttitudePositionEstimatorEKF(); - if (estimator::g_estimator == nullptr) + if (estimator::g_estimator == nullptr) { errx(1, "alloc failed"); + } if (OK != estimator::g_estimator->start()) { delete estimator::g_estimator; @@ -1484,14 +1544,16 @@ int ekf_att_pos_estimator_main(int argc, char *argv[]) printf("."); fflush(stdout); } + printf("\n"); exit(0); } if (!strcmp(argv[1], "stop")) { - if (estimator::g_estimator == nullptr) + if (estimator::g_estimator == nullptr) { errx(1, "not running"); + } delete estimator::g_estimator; estimator::g_estimator = nullptr; diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_params.c b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_params.c index 8c82dd6c1..f8cca6c0d 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_params.c +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_params.c @@ -218,7 +218,7 @@ PARAM_DEFINE_FLOAT(PE_ACC_PNOISE, 0.25f); * @max 0.00001 * @group Position Estimator */ -PARAM_DEFINE_FLOAT(PE_GBIAS_PNOISE, 1e-07f); +PARAM_DEFINE_FLOAT(PE_GBIAS_PNOISE, 1e-06f); /** * Accelerometer bias estimate process noise @@ -230,7 +230,7 @@ PARAM_DEFINE_FLOAT(PE_GBIAS_PNOISE, 1e-07f); * @max 0.001 * @group Position Estimator */ -PARAM_DEFINE_FLOAT(PE_ABIAS_PNOISE, 0.00005f); +PARAM_DEFINE_FLOAT(PE_ABIAS_PNOISE, 0.0002f); /** * Magnetometer earth frame offsets process noise diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp index d8e3116b9..817590bab 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp @@ -185,7 +185,9 @@ AttPosEKF::AttPosEKF() : minFlowRng(0.0f), moCompR_LOS(0.0f), - _onGround(true) + _isFixedWing(false), + _onGround(true), + _accNavMagHorizontal(0.0f) { memset(&last_ekf_error, 0, sizeof(last_ekf_error)); @@ -196,6 +198,7 @@ AttPosEKF::AttPosEKF() : AttPosEKF::~AttPosEKF() { + //dtor } void AttPosEKF::InitialiseParameters() @@ -207,10 +210,10 @@ void AttPosEKF::InitialiseParameters() yawVarScale = 1.0f; windVelSigma = 0.1f; - dAngBiasSigma = 5.0e-7f; - dVelBiasSigma = 1e-4f; - magEarthSigma = 3.0e-4f; - magBodySigma = 3.0e-4f; + dAngBiasSigma = 1.0e-6; + dVelBiasSigma = 0.0002f; + magEarthSigma = 0.0003f; + magBodySigma = 0.0003f; vneSigma = 0.2f; vdSigma = 0.3f; @@ -348,6 +351,11 @@ void AttPosEKF::UpdateStrapdownEquationsNED() // variance estimation) accNavMag = delVelNav.length()/dtIMU; + //First order low-pass filtered magnitude of horizontal nav acceleration + Vector3f derivativeNav = (delVelNav / dtIMU); + float derivativeVelNavMagnitude = sqrtf(sq(derivativeNav.x) + sq(derivativeNav.y)); + _accNavMagHorizontal = _accNavMagHorizontal * 0.95f + derivativeVelNavMagnitude * 0.05f; + // If calculating position save previous velocity float lastVelocity[3]; lastVelocity[0] = states[4]; @@ -406,9 +414,10 @@ void AttPosEKF::CovariancePrediction(float dt) // calculate covariance prediction process noise for (uint8_t i= 0; i<4; i++) processNoise[i] = 1.0e-9f; for (uint8_t i= 4; i<10; i++) processNoise[i] = 1.0e-9f; - for (uint8_t i=10; i<=12; i++) processNoise[i] = dt * dAngBiasSigma; // scale gyro bias noise when on ground to allow for faster bias estimation - for (uint8_t i=10; i<=12; i++) processNoise[i] = dt * dAngBiasSigma; + float gyroBiasScale = (_onGround) ? 2.0f : 1.0f; + + for (uint8_t i=10; i<=12; i++) processNoise[i] = dt * dAngBiasSigma * gyroBiasScale; processNoise[13] = dVelBiasSigma; if (!inhibitWindStates) { for (uint8_t i=14; i<=15; i++) processNoise[i] = dt * windVelSigma; @@ -2003,12 +2012,12 @@ void AttPosEKF::FuseOptFlow() K_LOS[1][15] = 0.0f; } if (inhibitMagStates) { - K_LOS[1][16] = SK_LOS[0]*(P[16][0]*tempVar[1] + P[16][1]*tempVar[2] - P[16][2]*tempVar[3] + P[16][3]*tempVar[4] + P[16][5]*tempVar[5] - P[16][6]*tempVar[6] - P[16][9]*tempVar[7] + P[16][4]*tempVar[8]); - K_LOS[1][17] = SK_LOS[0]*(P[17][0]*tempVar[1] + P[17][1]*tempVar[2] - P[17][2]*tempVar[3] + P[17][3]*tempVar[4] + P[17][5]*tempVar[5] - P[17][6]*tempVar[6] - P[17][9]*tempVar[7] + P[17][4]*tempVar[8]); - K_LOS[1][18] = SK_LOS[0]*(P[18][0]*tempVar[1] + P[18][1]*tempVar[2] - P[18][2]*tempVar[3] + P[18][3]*tempVar[4] + P[18][5]*tempVar[5] - P[18][6]*tempVar[6] - P[18][9]*tempVar[7] + P[18][4]*tempVar[8]); - K_LOS[1][19] = SK_LOS[0]*(P[19][0]*tempVar[1] + P[19][1]*tempVar[2] - P[19][2]*tempVar[3] + P[19][3]*tempVar[4] + P[19][5]*tempVar[5] - P[19][6]*tempVar[6] - P[19][9]*tempVar[7] + P[19][4]*tempVar[8]); - K_LOS[1][20] = SK_LOS[0]*(P[20][0]*tempVar[1] + P[20][1]*tempVar[2] - P[20][2]*tempVar[3] + P[20][3]*tempVar[4] + P[20][5]*tempVar[5] - P[20][6]*tempVar[6] - P[20][9]*tempVar[7] + P[20][4]*tempVar[8]); - K_LOS[1][21] = SK_LOS[0]*(P[21][0]*tempVar[1] + P[21][1]*tempVar[2] - P[21][2]*tempVar[3] + P[21][3]*tempVar[4] + P[21][5]*tempVar[5] - P[21][6]*tempVar[6] - P[21][9]*tempVar[7] + P[21][4]*tempVar[8]); + K_LOS[1][16] = SK_LOS[0]*(P[16][0]*tempVar[1] + P[16][1]*tempVar[2] - P[16][2]*tempVar[3] + P[16][3]*tempVar[4] + P[16][5]*tempVar[5] - P[16][6]*tempVar[6] - P[16][9]*tempVar[7] + P[16][4]*tempVar[8]); + K_LOS[1][17] = SK_LOS[0]*(P[17][0]*tempVar[1] + P[17][1]*tempVar[2] - P[17][2]*tempVar[3] + P[17][3]*tempVar[4] + P[17][5]*tempVar[5] - P[17][6]*tempVar[6] - P[17][9]*tempVar[7] + P[17][4]*tempVar[8]); + K_LOS[1][18] = SK_LOS[0]*(P[18][0]*tempVar[1] + P[18][1]*tempVar[2] - P[18][2]*tempVar[3] + P[18][3]*tempVar[4] + P[18][5]*tempVar[5] - P[18][6]*tempVar[6] - P[18][9]*tempVar[7] + P[18][4]*tempVar[8]); + K_LOS[1][19] = SK_LOS[0]*(P[19][0]*tempVar[1] + P[19][1]*tempVar[2] - P[19][2]*tempVar[3] + P[19][3]*tempVar[4] + P[19][5]*tempVar[5] - P[19][6]*tempVar[6] - P[19][9]*tempVar[7] + P[19][4]*tempVar[8]); + K_LOS[1][20] = SK_LOS[0]*(P[20][0]*tempVar[1] + P[20][1]*tempVar[2] - P[20][2]*tempVar[3] + P[20][3]*tempVar[4] + P[20][5]*tempVar[5] - P[20][6]*tempVar[6] - P[20][9]*tempVar[7] + P[20][4]*tempVar[8]); + K_LOS[1][21] = SK_LOS[0]*(P[21][0]*tempVar[1] + P[21][1]*tempVar[2] - P[21][2]*tempVar[3] + P[21][3]*tempVar[4] + P[21][5]*tempVar[5] - P[21][6]*tempVar[6] - P[21][9]*tempVar[7] + P[21][4]*tempVar[8]); } else { for (uint8_t i = 16; i < EKF_STATE_ESTIMATES; i++) { K_LOS[1][i] = 0.0f; @@ -2321,15 +2330,20 @@ void AttPosEKF::zeroCols(float (&covMat)[EKF_STATE_ESTIMATES][EKF_STATE_ESTIMATE // Store states in a history array along with time stamp void AttPosEKF::StoreStates(uint64_t timestamp_ms) { - for (size_t i=0; i<EKF_STATE_ESTIMATES; i++) + for (size_t i = 0; i < EKF_STATE_ESTIMATES; i++) { storedStates[i][storeIndex] = states[i]; + } + storedOmega[0][storeIndex] = angRate.x; storedOmega[1][storeIndex] = angRate.y; storedOmega[2][storeIndex] = angRate.z; statetimeStamp[storeIndex] = timestamp_ms; + + // increment to next storage index storeIndex++; - if (storeIndex == EKF_DATA_BUFFER_SIZE) + if (storeIndex >= EKF_DATA_BUFFER_SIZE) { storeIndex = 0; + } } void AttPosEKF::ResetStoredStates() @@ -2342,10 +2356,8 @@ void AttPosEKF::ResetStoredStates() // reset store index to first storeIndex = 0; - statetimeStamp[storeIndex] = millis(); - - // increment to next storage index - storeIndex++; + //Reset stored state to current state + StoreStates(millis()); } // Output the state vector stored at the time that best matches that specified by msec @@ -2505,27 +2517,6 @@ void AttPosEKF::quat2eul(float (&y)[3], const float (&u)[4]) y[2] = atan2f((2.0f*(u[1]*u[2]+u[0]*u[3])) , (u[0]*u[0]+u[1]*u[1]-u[2]*u[2]-u[3]*u[3])); } -void AttPosEKF::calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, float gpsVelD) -{ - velNED[0] = gpsGndSpd*cosf(gpsCourse); - velNED[1] = gpsGndSpd*sinf(gpsCourse); - velNED[2] = gpsVelD; -} - -void AttPosEKF::calcposNED(float (&posNED)[3], double lat, double lon, float hgt, double latReference, double lonReference, float hgtReference) -{ - posNED[0] = earthRadius * (lat - latReference); - posNED[1] = earthRadius * cos(latReference) * (lon - lonReference); - posNED[2] = -(hgt - hgtReference); -} - -void AttPosEKF::calcLLH(float posNED[3], double &lat, double &lon, float &hgt, double latRef, double lonRef, float hgtRef) -{ - lat = latRef + (double)posNED[0] * earthRadiusInv; - lon = lonRef + (double)posNED[1] * earthRadiusInv / cos(latRef); - hgt = hgtRef - posNED[2]; -} - void AttPosEKF::setOnGround(const bool isLanded) { _onGround = isLanded; @@ -2537,15 +2528,14 @@ void AttPosEKF::setOnGround(const bool isLanded) if (_onGround || !useAirspeed) { inhibitWindStates = true; } else { - inhibitWindStates =false; + inhibitWindStates = false; } + //Check if we are accelerating forward, only then is the mag offset is observable + bool isMovingForward = _accNavMagHorizontal > 0.5f; + // don't update magnetic field states if on ground or not using compass - if (_onGround || !useCompass) { - inhibitMagStates = true; - } else { - inhibitMagStates = false; - } + inhibitMagStates = (!useCompass || _onGround) || (!_isFixedWing && !isMovingForward); // don't update terrain offset state if there is no range finder and flying at low velocity or without GPS if ((_onGround || !useGPS) && !useRangeFinder) { @@ -2585,25 +2575,40 @@ void AttPosEKF::CovarianceInit() P[1][1] = 0.25f * sq(1.0f*deg2rad); P[2][2] = 0.25f * sq(1.0f*deg2rad); P[3][3] = 0.25f * sq(10.0f*deg2rad); + + //velocities P[4][4] = sq(0.7f); P[5][5] = P[4][4]; P[6][6] = sq(0.7f); + + //positions P[7][7] = sq(15.0f); P[8][8] = P[7][7]; P[9][9] = sq(5.0f); + + //delta angle biases P[10][10] = sq(0.1f*deg2rad*dtIMU); P[11][11] = P[10][10]; P[12][12] = P[10][10]; + + //Z delta velocity bias P[13][13] = sq(0.2f*dtIMU); - P[14][14] = sq(0.0f); + + //Wind velocities + P[14][14] = 0.0f; P[15][15] = P[14][14]; + + //Earth magnetic field P[16][16] = sq(0.02f); P[17][17] = P[16][16]; P[18][18] = P[16][16]; + + //Body magnetic field P[19][19] = sq(0.02f); P[20][20] = P[19][19]; P[21][21] = P[19][19]; + //Optical flow fScaleFactorVar = 0.001f; // focal length scale factor variance Popt[0][0] = 0.001f; } @@ -2621,9 +2626,11 @@ float AttPosEKF::ConstrainFloat(float val, float min_val, float max_val) ret = val; } +#if 0 if (!isfinite(val)) { - //ekf_debug("constrain: non-finite!"); + ekf_debug("constrain: non-finite!"); } +#endif return ret; } @@ -2700,6 +2707,11 @@ void AttPosEKF::ConstrainStates() // 16-18: Earth Magnetic Field Vector - gauss (North, East, Down) // 19-21: Body Magnetic Field Vector - gauss (X,Y,Z) + // Constrain dtIMUfilt + if (!isfinite(dtIMUfilt) || (fabsf(dtIMU - dtIMUfilt) > 0.01f)) { + dtIMUfilt = dtIMU; + } + // Constrain quaternion for (size_t i = 0; i <= 3; i++) { states[i] = ConstrainFloat(states[i], -1.0f, 1.0f); @@ -2721,11 +2733,11 @@ void AttPosEKF::ConstrainStates() // Angle bias limit - set to 8 degrees / sec for (size_t i = 10; i <= 12; i++) { - states[i] = ConstrainFloat(states[i], -0.12f * dtIMU, 0.12f * dtIMU); + states[i] = ConstrainFloat(states[i], -0.16f * dtIMUfilt, 0.16f * dtIMUfilt); } // Constrain delta velocity bias - states[13] = ConstrainFloat(states[13], -1.0f * dtIMU, 1.0f * dtIMU); + states[13] = ConstrainFloat(states[13], -1.0f * dtIMUfilt, 1.0f * dtIMUfilt); // Wind velocity limits - assume 120 m/s max velocity for (size_t i = 14; i <= 15; i++) { @@ -2789,8 +2801,8 @@ bool AttPosEKF::GyroOffsetsDiverged() // Protect against division by zero if (delta_len > 0.0f) { - float cov_mag = ConstrainFloat((P[10][10] + P[11][11] + P[12][12]), 1e-12f, 1e-8f); - delta_len_scaled = (5e-7 / (double)cov_mag) * (double)delta_len / (double)dtIMU; + float cov_mag = ConstrainFloat((P[10][10] + P[11][11] + P[12][12]), 1e-12f, 1e-2f); + delta_len_scaled = (5e-7 / (double)cov_mag) * (double)delta_len / (double)dtIMUfilt; } bool diverged = (delta_len_scaled > 1.0f); @@ -2856,8 +2868,12 @@ void AttPosEKF::ResetPosition(void) for (size_t i = 0; i < EKF_DATA_BUFFER_SIZE; ++i){ storedStates[7][i] = states[7]; storedStates[8][i] = states[8]; - } + } } + + //reset position covariance + P[7][7] = sq(15.0f); + P[8][8] = P[7][7]; } void AttPosEKF::ResetHeight(void) @@ -2869,6 +2885,10 @@ void AttPosEKF::ResetHeight(void) for (size_t i = 0; i < EKF_DATA_BUFFER_SIZE; ++i){ storedStates[9][i] = states[9]; } + + //reset altitude covariance + P[9][9] = sq(5.0f); + P[6][6] = sq(0.7f); } void AttPosEKF::ResetVelocity(void) @@ -2877,7 +2897,8 @@ void AttPosEKF::ResetVelocity(void) states[4] = 0.0f; states[5] = 0.0f; states[6] = 0.0f; - } else if (GPSstatus >= GPS_FIX_3D) { + } + else if (GPSstatus >= GPS_FIX_3D) { //Do not use Z velocity, we trust the Barometer history more states[4] = velNED[0]; // north velocity from last reading @@ -2887,8 +2908,12 @@ void AttPosEKF::ResetVelocity(void) for (size_t i = 0; i < EKF_DATA_BUFFER_SIZE; ++i){ storedStates[4][i] = states[4]; storedStates[5][i] = states[5]; - } + } } + + //reset velocities covariance + P[4][4] = sq(0.7f); + P[5][5] = P[4][4]; } bool AttPosEKF::StatesNaN() { @@ -3005,10 +3030,10 @@ int AttPosEKF::CheckAndBound(struct ekf_status_report *last_error) // Fill error report GetFilterState(&last_ekf_error); - ResetStoredStates(); ResetVelocity(); ResetPosition(); ResetHeight(); + ResetStoredStates(); // Timeout cleared with this reset current_ekf_state.imuTimeout = false; @@ -3022,10 +3047,10 @@ int AttPosEKF::CheckAndBound(struct ekf_status_report *last_error) // Fill error report, but not setting error flag GetFilterState(&last_ekf_error); - ResetStoredStates(); ResetVelocity(); ResetPosition(); ResetHeight(); + ResetStoredStates(); ret = 0; } @@ -3195,10 +3220,10 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination) states[20] = magBias.y; // Magnetic Field Bias Y states[21] = magBias.z; // Magnetic Field Bias Z - ResetStoredStates(); ResetVelocity(); ResetPosition(); ResetHeight(); + ResetStoredStates(); // initialise focal length scale factor estimator states flowStates[0] = 1.0f; @@ -3210,7 +3235,6 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination) //Define Earth rotation vector in the NED navigation frame calcEarthRateNED(earthRateNED, latRef); - } void AttPosEKF::InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt, float declination) @@ -3286,7 +3310,6 @@ void AttPosEKF::ZeroVariables() magstate.DCM.identity(); memset(¤t_ekf_state, 0, sizeof(current_ekf_state)); - } void AttPosEKF::GetFilterState(struct ekf_status_report *err) @@ -3303,13 +3326,6 @@ void AttPosEKF::GetFilterState(struct ekf_status_report *err) current_ekf_state.useAirspeed = useAirspeed; memcpy(err, ¤t_ekf_state, sizeof(*err)); - - // err->velHealth = current_ekf_state.velHealth; - // err->posHealth = current_ekf_state.posHealth; - // err->hgtHealth = current_ekf_state.hgtHealth; - // err->velTimeout = current_ekf_state.velTimeout; - // err->posTimeout = current_ekf_state.posTimeout; - // err->hgtTimeout = current_ekf_state.hgtTimeout; } void AttPosEKF::GetLastErrorState(struct ekf_status_report *last_error) @@ -3317,3 +3333,8 @@ void AttPosEKF::GetLastErrorState(struct ekf_status_report *last_error) memcpy(last_error, &last_ekf_error, sizeof(*last_error)); memset(&last_ekf_error, 0, sizeof(last_ekf_error)); } + +void AttPosEKF::setIsFixedWing(const bool fixedWing) +{ + _isFixedWing = fixedWing; +} diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.h b/src/modules/ekf_att_pos_estimator/estimator_22states.h index e15ded977..9b23f4df4 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_22states.h +++ b/src/modules/ekf_att_pos_estimator/estimator_22states.h @@ -288,7 +288,6 @@ public: * Recall the state vector. * * Recalls the vector stored at closest time to the one specified by msec - *FuseOptFlow * @return zero on success, integer indicating the number of invalid states on failure. * Does only copy valid states, if the statesForFusion vector was initialized * correctly by the caller, the result can be safely used, but is a mixture @@ -307,12 +306,6 @@ public: static void quat2eul(float (&eul)[3], const float (&quat)[4]); - static void calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, float gpsVelD); - - static void calcposNED(float (&posNED)[3], double lat, double lon, float hgt, double latRef, double lonRef, float hgtRef); - - static void calcLLH(float posNED[3], double &lat, double &lon, float &hgt, double latRef, double lonRef, float hgtRef); - //static void quat2Tnb(Mat3f &Tnb, const float (&quat)[4]); static inline float sq(float valIn) {return valIn * valIn;} @@ -362,8 +355,6 @@ public: */ void ResetVelocity(); - void ZeroVariables(); - void GetFilterState(struct ekf_status_report *state); void GetLastErrorState(struct ekf_status_report *last_error); @@ -372,6 +363,22 @@ public: void InitializeDynamic(float (&initvelNED)[3], float declination); + /** + * @brief + * Tells the EKF wheter the vehicle is a fixed wing frame or not. + * This changes the way the EKF fuses position or attitude calulations + * by making some assumptions on movement. + * @param fixedWing + * true if the vehicle moves like a Fixed Wing, false otherwise + **/ + void setIsFixedWing(const bool fixedWing); + + /** + * @brief + * Reset internal filter states and clear all variables to zero value + */ + void ZeroVariables(); + protected: /** @@ -399,10 +406,11 @@ protected: void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float declination, float *initQuat); void ResetStoredStates(); - -private: - bool _onGround; ///< boolean true when the flight vehicle is on the ground (not flying) +private: + bool _isFixedWing; ///< True if the vehicle is a fixed-wing frame type + bool _onGround; ///< boolean true when the flight vehicle is on the ground (not flying) + float _accNavMagHorizontal; ///< First-order low-pass filtered rate of change maneuver velocity }; uint32_t millis(); diff --git a/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp b/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp index e2f4c7e82..470eb4e2c 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp @@ -38,6 +38,7 @@ */ #include "estimator_utilities.h" +#include <algorithm> // Define EKF_DEBUG here to enable the debug print calls // if the macro is not set, these will be completely @@ -104,17 +105,17 @@ void Mat3f::identity() { z.z = 1.0f; } -Mat3f Mat3f::transpose(void) const +Mat3f Mat3f::transpose() const { Mat3f ret = *this; - swap_var(ret.x.y, ret.y.x); - swap_var(ret.x.z, ret.z.x); - swap_var(ret.y.z, ret.z.y); + std::swap(ret.x.y, ret.y.x); + std::swap(ret.x.z, ret.z.x); + std::swap(ret.y.z, ret.z.y); return ret; } // overload + operator to provide a vector addition -Vector3f operator+( Vector3f vecIn1, Vector3f vecIn2) +Vector3f operator+(const Vector3f &vecIn1, const Vector3f &vecIn2) { Vector3f vecOut; vecOut.x = vecIn1.x + vecIn2.x; @@ -124,7 +125,7 @@ Vector3f operator+( Vector3f vecIn1, Vector3f vecIn2) } // overload - operator to provide a vector subtraction -Vector3f operator-( Vector3f vecIn1, Vector3f vecIn2) +Vector3f operator-(const Vector3f &vecIn1, const Vector3f &vecIn2) { Vector3f vecOut; vecOut.x = vecIn1.x - vecIn2.x; @@ -134,7 +135,7 @@ Vector3f operator-( Vector3f vecIn1, Vector3f vecIn2) } // overload * operator to provide a matrix vector product -Vector3f operator*( Mat3f matIn, Vector3f vecIn) +Vector3f operator*(const Mat3f &matIn, const Vector3f &vecIn) { Vector3f vecOut; vecOut.x = matIn.x.x*vecIn.x + matIn.x.y*vecIn.y + matIn.x.z*vecIn.z; @@ -144,7 +145,7 @@ Vector3f operator*( Mat3f matIn, Vector3f vecIn) } // overload * operator to provide a matrix product -Mat3f operator*( Mat3f matIn1, Mat3f matIn2) +Mat3f operator*(const Mat3f &matIn1, const Mat3f &matIn2) { Mat3f matOut; matOut.x.x = matIn1.x.x*matIn2.x.x + matIn1.x.y*matIn2.y.x + matIn1.x.z*matIn2.z.x; @@ -163,7 +164,7 @@ Mat3f operator*( Mat3f matIn1, Mat3f matIn2) } // overload % operator to provide a vector cross product -Vector3f operator%( Vector3f vecIn1, Vector3f vecIn2) +Vector3f operator%(const Vector3f &vecIn1, const Vector3f &vecIn2) { Vector3f vecOut; vecOut.x = vecIn1.y*vecIn2.z - vecIn1.z*vecIn2.y; @@ -173,7 +174,7 @@ Vector3f operator%( Vector3f vecIn1, Vector3f vecIn2) } // overload * operator to provide a vector scaler product -Vector3f operator*(Vector3f vecIn1, float sclIn1) +Vector3f operator*(const Vector3f &vecIn1, const float sclIn1) { Vector3f vecOut; vecOut.x = vecIn1.x * sclIn1; @@ -183,7 +184,7 @@ Vector3f operator*(Vector3f vecIn1, float sclIn1) } // overload * operator to provide a vector scaler product -Vector3f operator*(float sclIn1, Vector3f vecIn1) +Vector3f operator*(float sclIn1, const Vector3f &vecIn1) { Vector3f vecOut; vecOut.x = vecIn1.x * sclIn1; @@ -192,9 +193,12 @@ Vector3f operator*(float sclIn1, Vector3f vecIn1) return vecOut; } -void swap_var(float &d1, float &d2) +// overload / operator to provide a vector scalar division +Vector3f operator/(const Vector3f &vec, const float scalar) { - float tmp = d1; - d1 = d2; - d2 = tmp; + Vector3f vecOut; + vecOut.x = vec.x / scalar; + vecOut.y = vec.y / scalar; + vecOut.z = vec.z / scalar; + return vecOut; } diff --git a/src/modules/ekf_att_pos_estimator/estimator_utilities.h b/src/modules/ekf_att_pos_estimator/estimator_utilities.h index 95b83ead4..c137209ff 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_utilities.h +++ b/src/modules/ekf_att_pos_estimator/estimator_utilities.h @@ -52,7 +52,6 @@ class Vector3f { -private: public: float x; float y; @@ -64,8 +63,8 @@ public: z(c) {} - float length(void) const; - void zero(void); + float length() const; + void zero(); }; class Mat3f @@ -79,18 +78,17 @@ public: Mat3f(); void identity(); - Mat3f transpose(void) const; + Mat3f transpose() const; }; -Vector3f operator*(float sclIn1, Vector3f vecIn1); -Vector3f operator+( Vector3f vecIn1, Vector3f vecIn2); -Vector3f operator-( Vector3f vecIn1, Vector3f vecIn2); -Vector3f operator*( Mat3f matIn, Vector3f vecIn); -Mat3f operator*( Mat3f matIn1, Mat3f matIn2); -Vector3f operator%( Vector3f vecIn1, Vector3f vecIn2); -Vector3f operator*(Vector3f vecIn1, float sclIn1); - -void swap_var(float &d1, float &d2); +Vector3f operator*(const float sclIn1, const Vector3f &vecIn1); +Vector3f operator+(const Vector3f &vecIn1, const Vector3f &vecIn2); +Vector3f operator-(const Vector3f &vecIn1, const Vector3f &vecIn2); +Vector3f operator*(const Mat3f &matIn, const Vector3f &vecIn); +Mat3f operator*(const Mat3f &matIn1, const Mat3f &matIn2); +Vector3f operator%(const Vector3f &vecIn1, const Vector3f &vecIn2); +Vector3f operator*(const Vector3f &vecIn1, const float sclIn1); +Vector3f operator/(const Vector3f &vec, const float scalar); enum GPS_FIX { GPS_FIX_NOFIX = 0, diff --git a/src/modules/fixedwing_backside/params.c b/src/modules/fixedwing_backside/params.c index db30af416..aa82a4f59 100644 --- a/src/modules/fixedwing_backside/params.c +++ b/src/modules/fixedwing_backside/params.c @@ -57,7 +57,7 @@ PARAM_DEFINE_FLOAT(FWB_V_CMD, 12.0f); // commanded velocity PARAM_DEFINE_FLOAT(FWB_V_MAX, 16.0f); // maximum commanded velocity // rate of climb -// this is what rate of climb is commanded (in m/s) +// this is what rate of climb is commanded (in m/s) // when the pitch stick is fully defelcted in simple mode PARAM_DEFINE_FLOAT(FWB_CR_MAX, 1.0f); diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 5daeae477..06df5fd97 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -1119,7 +1119,7 @@ FixedwingAttitudeControl::start() _control_task = task_spawn_cmd("fw_att_control", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, - 2048, + 1600, (main_t)&FixedwingAttitudeControl::task_main_trampoline, nullptr); diff --git a/src/modules/fw_att_control/module.mk b/src/modules/fw_att_control/module.mk index 3661a171f..390992699 100644 --- a/src/modules/fw_att_control/module.mk +++ b/src/modules/fw_att_control/module.mk @@ -40,6 +40,8 @@ MODULE_COMMAND = fw_att_control SRCS = fw_att_control_main.cpp \ fw_att_control_params.c +# Startup handler, the actual app stack size is +# in the task_spawn command MODULE_STACKSIZE = 1200 MAXOPTIMIZATION = -Os diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 0bdc285e7..427df9739 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -1624,7 +1624,7 @@ FixedwingPositionControl::start() _control_task = task_spawn_cmd("fw_pos_control_l1", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, - 2000, + 1600, (main_t)&FixedwingPositionControl::task_main_trampoline, nullptr); diff --git a/src/modules/fw_pos_control_l1/module.mk b/src/modules/fw_pos_control_l1/module.mk index 98e5c0a1e..8a8de708e 100644 --- a/src/modules/fw_pos_control_l1/module.mk +++ b/src/modules/fw_pos_control_l1/module.mk @@ -44,6 +44,8 @@ SRCS = fw_pos_control_l1_main.cpp \ mtecs/limitoverride.cpp \ mtecs/mTecs_params.c +# Startup handler, the actual app stack size is +# in the task_spawn command MODULE_STACKSIZE = 1200 MAXOPTIMIZATION = -Os diff --git a/src/modules/gpio_led/gpio_led.c b/src/modules/gpio_led/gpio_led.c index 7758faed7..b759b429e 100644 --- a/src/modules/gpio_led/gpio_led.c +++ b/src/modules/gpio_led/gpio_led.c @@ -66,7 +66,7 @@ struct gpio_led_s { int counter; }; -static struct gpio_led_s gpio_led_data; +static struct gpio_led_s *gpio_led_data; static bool gpio_led_started = false; __EXPORT int gpio_led_main(int argc, char *argv[]); @@ -170,10 +170,11 @@ int gpio_led_main(int argc, char *argv[]) } } - memset(&gpio_led_data, 0, sizeof(gpio_led_data)); - gpio_led_data.use_io = use_io; - gpio_led_data.pin = pin; - int ret = work_queue(LPWORK, &gpio_led_data.work, gpio_led_start, &gpio_led_data, 0); + gpio_led_data = malloc(sizeof(struct gpio_led_s)); + memset(gpio_led_data, 0, sizeof(struct gpio_led_s)); + gpio_led_data->use_io = use_io; + gpio_led_data->pin = pin; + int ret = work_queue(LPWORK, &(gpio_led_data->work), gpio_led_start, gpio_led_data, 0); if (ret != 0) { errx(1, "failed to queue work: %d", ret); diff --git a/src/modules/land_detector/module.mk b/src/modules/land_detector/module.mk index e08a4b7a8..4eb7a1cb5 100644 --- a/src/modules/land_detector/module.mk +++ b/src/modules/land_detector/module.mk @@ -11,3 +11,7 @@ SRCS = land_detector_main.cpp \ FixedwingLandDetector.cpp EXTRACXXFLAGS = -Weffc++ -Os + +# Startup handler, the actual app stack size is +# in the task_spawn command +MODULE_STACKSIZE = 1200 diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 0c34fc58a..e3f274b8b 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -127,6 +127,8 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _hil_local_proj_inited(0), _hil_local_alt0(0.0f), _hil_local_proj_ref{}, + _offboard_control_mode{}, + _rates_sp{}, _time_offset_avg_alpha(0.6), _time_offset(0) { @@ -756,8 +758,6 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) mavlink_set_attitude_target_t set_attitude_target; mavlink_msg_set_attitude_target_decode(msg, &set_attitude_target); - static struct offboard_control_mode_s offboard_control_mode = {}; - /* Only accept messages which are intended for this system */ if ((mavlink_system.sysid == set_attitude_target.target_system || set_attitude_target.target_system == 0) && @@ -765,7 +765,7 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) set_attitude_target.target_component == 0)) { /* set correct ignore flags for thrust field: copy from mavlink message */ - offboard_control_mode.ignore_thrust = (bool)(set_attitude_target.type_mask & (1 << 6)); + _offboard_control_mode.ignore_thrust = (bool)(set_attitude_target.type_mask & (1 << 6)); /* * The tricky part in parsing this message is that the offboard sender *can* set attitude and thrust @@ -782,26 +782,26 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) bool ignore_bodyrate = (bool)(set_attitude_target.type_mask & 0x7); bool ignore_attitude = (bool)(set_attitude_target.type_mask & (1 << 7)); - if (ignore_bodyrate && ignore_attitude && !offboard_control_mode.ignore_thrust) { + if (ignore_bodyrate && ignore_attitude && !_offboard_control_mode.ignore_thrust) { /* Message want's us to ignore everything except thrust: only ignore if previously ignored */ - offboard_control_mode.ignore_bodyrate = ignore_bodyrate && offboard_control_mode.ignore_bodyrate; - offboard_control_mode.ignore_attitude = ignore_attitude && offboard_control_mode.ignore_attitude; + _offboard_control_mode.ignore_bodyrate = ignore_bodyrate && _offboard_control_mode.ignore_bodyrate; + _offboard_control_mode.ignore_attitude = ignore_attitude && _offboard_control_mode.ignore_attitude; } else { - offboard_control_mode.ignore_bodyrate = ignore_bodyrate; - offboard_control_mode.ignore_attitude = ignore_attitude; + _offboard_control_mode.ignore_bodyrate = ignore_bodyrate; + _offboard_control_mode.ignore_attitude = ignore_attitude; } - offboard_control_mode.ignore_position = true; - offboard_control_mode.ignore_velocity = true; - offboard_control_mode.ignore_acceleration_force = true; + _offboard_control_mode.ignore_position = true; + _offboard_control_mode.ignore_velocity = true; + _offboard_control_mode.ignore_acceleration_force = true; - offboard_control_mode.timestamp = hrt_absolute_time(); + _offboard_control_mode.timestamp = hrt_absolute_time(); if (_offboard_control_mode_pub < 0) { - _offboard_control_mode_pub = orb_advertise(ORB_ID(offboard_control_mode), &offboard_control_mode); + _offboard_control_mode_pub = orb_advertise(ORB_ID(offboard_control_mode), &_offboard_control_mode); } else { - orb_publish(ORB_ID(offboard_control_mode), _offboard_control_mode_pub, &offboard_control_mode); + orb_publish(ORB_ID(offboard_control_mode), _offboard_control_mode_pub, &_offboard_control_mode); } /* If we are in offboard control mode and offboard control loop through is enabled @@ -816,14 +816,14 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) if (_control_mode.flag_control_offboard_enabled) { /* Publish attitude setpoint if attitude and thrust ignore bits are not set */ - if (!(offboard_control_mode.ignore_attitude)) { + if (!(_offboard_control_mode.ignore_attitude)) { static struct vehicle_attitude_setpoint_s att_sp = {}; att_sp.timestamp = hrt_absolute_time(); mavlink_quaternion_to_euler(set_attitude_target.q, &att_sp.roll_body, &att_sp.pitch_body, &att_sp.yaw_body); mavlink_quaternion_to_dcm(set_attitude_target.q, (float(*)[3])att_sp.R_body); att_sp.R_valid = true; - if (!offboard_control_mode.ignore_thrust) { // dont't overwrite thrust if it's invalid + if (!_offboard_control_mode.ignore_thrust) { // dont't overwrite thrust if it's invalid att_sp.thrust = set_attitude_target.thrust; } att_sp.yaw_sp_move_rate = 0.0; @@ -838,20 +838,19 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) /* Publish attitude rate setpoint if bodyrate and thrust ignore bits are not set */ ///XXX add support for ignoring individual axes - if (!(offboard_control_mode.ignore_bodyrate)) { - static struct vehicle_rates_setpoint_s rates_sp = {}; - rates_sp.timestamp = hrt_absolute_time(); - rates_sp.roll = set_attitude_target.body_roll_rate; - rates_sp.pitch = set_attitude_target.body_pitch_rate; - rates_sp.yaw = set_attitude_target.body_yaw_rate; - if (!offboard_control_mode.ignore_thrust) { // dont't overwrite thrust if it's invalid - rates_sp.thrust = set_attitude_target.thrust; + if (!(_offboard_control_mode.ignore_bodyrate)) { + _rates_sp.timestamp = hrt_absolute_time(); + _rates_sp.roll = set_attitude_target.body_roll_rate; + _rates_sp.pitch = set_attitude_target.body_pitch_rate; + _rates_sp.yaw = set_attitude_target.body_yaw_rate; + if (!_offboard_control_mode.ignore_thrust) { // dont't overwrite thrust if it's invalid + _rates_sp.thrust = set_attitude_target.thrust; } if (_att_sp_pub < 0) { - _rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp); + _rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_rates_sp); } else { - orb_publish(ORB_ID(vehicle_rates_setpoint), _rates_sp_pub, &rates_sp); + orb_publish(ORB_ID(vehicle_rates_setpoint), _rates_sp_pub, &_rates_sp); } } } @@ -1549,7 +1548,7 @@ MavlinkReceiver::receive_start(Mavlink *parent) param.sched_priority = SCHED_PRIORITY_MAX - 80; (void)pthread_attr_setschedparam(&receiveloop_attr, ¶m); - pthread_attr_setstacksize(&receiveloop_attr, 2100); + pthread_attr_setstacksize(&receiveloop_attr, 1800); pthread_t thread; pthread_create(&thread, &receiveloop_attr, MavlinkReceiver::start_helper, (void *)parent); diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 4886bbd0a..b510dbbb7 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -182,6 +182,8 @@ private: bool _hil_local_proj_inited; float _hil_local_alt0; struct map_projection_reference_s _hil_local_proj_ref; + struct offboard_control_mode_s _offboard_control_mode; + struct vehicle_rates_setpoint_s _rates_sp; double _time_offset_avg_alpha; uint64_t _time_offset; diff --git a/src/modules/mavlink/mavlink_stream.cpp b/src/modules/mavlink/mavlink_stream.cpp index 5b9e45d3e..57a92c8b5 100644 --- a/src/modules/mavlink/mavlink_stream.cpp +++ b/src/modules/mavlink/mavlink_stream.cpp @@ -80,6 +80,7 @@ MavlinkStream::update(const hrt_abstime t) if (dt > 0 && dt >= interval) { /* interval expired, send message */ send(t); + if (const_rate()) { _last_sent = (t / _interval) * _interval; diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 0243dc2f7..85a0dda9e 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -166,14 +166,17 @@ private: param_t roll_rate_p; param_t roll_rate_i; param_t roll_rate_d; + param_t roll_rate_ff; param_t pitch_p; param_t pitch_rate_p; param_t pitch_rate_i; param_t pitch_rate_d; + param_t pitch_rate_ff; param_t yaw_p; param_t yaw_rate_p; param_t yaw_rate_i; param_t yaw_rate_d; + param_t yaw_rate_ff; param_t yaw_ff; param_t yaw_rate_max; @@ -191,6 +194,7 @@ private: math::Vector<3> rate_p; /**< P gain for angular rate error */ math::Vector<3> rate_i; /**< I gain for angular rate error */ math::Vector<3> rate_d; /**< D gain for angular rate error */ + math::Vector<3> rate_ff; /**< Feedforward gain for desired rates */ float yaw_ff; /**< yaw control feed-forward */ float yaw_rate_max; /**< max yaw rate */ @@ -316,6 +320,7 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _params.rate_p.zero(); _params.rate_i.zero(); _params.rate_d.zero(); + _params.rate_ff.zero(); _params.yaw_ff = 0.0f; _params.yaw_rate_max = 0.0f; _params.man_roll_max = 0.0f; @@ -335,14 +340,17 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _params_handles.roll_rate_p = param_find("MC_ROLLRATE_P"); _params_handles.roll_rate_i = param_find("MC_ROLLRATE_I"); _params_handles.roll_rate_d = param_find("MC_ROLLRATE_D"); + _params_handles.roll_rate_ff = param_find("MC_ROLLRATE_FF"); _params_handles.pitch_p = param_find("MC_PITCH_P"); _params_handles.pitch_rate_p = param_find("MC_PITCHRATE_P"); _params_handles.pitch_rate_i = param_find("MC_PITCHRATE_I"); _params_handles.pitch_rate_d = param_find("MC_PITCHRATE_D"); + _params_handles.pitch_rate_ff = param_find("MC_PITCHRATE_FF"); _params_handles.yaw_p = param_find("MC_YAW_P"); _params_handles.yaw_rate_p = param_find("MC_YAWRATE_P"); _params_handles.yaw_rate_i = param_find("MC_YAWRATE_I"); _params_handles.yaw_rate_d = param_find("MC_YAWRATE_D"); + _params_handles.yaw_rate_ff = param_find("MC_YAWRATE_FF"); _params_handles.yaw_ff = param_find("MC_YAW_FF"); _params_handles.yaw_rate_max = param_find("MC_YAWRATE_MAX"); _params_handles.man_roll_max = param_find("MC_MAN_R_MAX"); @@ -394,6 +402,8 @@ MulticopterAttitudeControl::parameters_update() _params.rate_i(0) = v; param_get(_params_handles.roll_rate_d, &v); _params.rate_d(0) = v; + param_get(_params_handles.roll_rate_ff, &v); + _params.rate_ff(0) = v; /* pitch gains */ param_get(_params_handles.pitch_p, &v); @@ -404,6 +414,8 @@ MulticopterAttitudeControl::parameters_update() _params.rate_i(1) = v; param_get(_params_handles.pitch_rate_d, &v); _params.rate_d(1) = v; + param_get(_params_handles.pitch_rate_ff, &v); + _params.rate_ff(1) = v; /* yaw gains */ param_get(_params_handles.yaw_p, &v); @@ -414,6 +426,8 @@ MulticopterAttitudeControl::parameters_update() _params.rate_i(2) = v; param_get(_params_handles.yaw_rate_d, &v); _params.rate_d(2) = v; + param_get(_params_handles.yaw_rate_ff, &v); + _params.rate_ff(2) = v; param_get(_params_handles.yaw_ff, &_params.yaw_ff); param_get(_params_handles.yaw_rate_max, &_params.yaw_rate_max); @@ -653,7 +667,7 @@ MulticopterAttitudeControl::control_attitude_rates(float dt) /* angular rates error */ math::Vector<3> rates_err = _rates_sp - rates; - _att_control = _params.rate_p.emult(rates_err) + _params.rate_d.emult(_rates_prev - rates) / dt + _rates_int; + _att_control = _params.rate_p.emult(rates_err) + _params.rate_d.emult(_rates_prev - rates) / dt + _rates_int + _params.rate_ff.emult(_rates_sp); _rates_prev = rates; /* update integral only if not saturated on low limit */ @@ -835,7 +849,7 @@ MulticopterAttitudeControl::start() _control_task = task_spawn_cmd("mc_att_control", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, - 1800, + 1600, (main_t)&MulticopterAttitudeControl::task_main_trampoline, nullptr); diff --git a/src/modules/mc_att_control/mc_att_control_params.c b/src/modules/mc_att_control/mc_att_control_params.c index 302551959..6a21f9046 100644 --- a/src/modules/mc_att_control/mc_att_control_params.c +++ b/src/modules/mc_att_control/mc_att_control_params.c @@ -83,6 +83,16 @@ PARAM_DEFINE_FLOAT(MC_ROLLRATE_I, 0.0f); PARAM_DEFINE_FLOAT(MC_ROLLRATE_D, 0.002f); /** + * Roll rate feedforward + * + * Improves tracking performance. + * + * @min 0.0 + * @group Multicopter Attitude Control + */ +PARAM_DEFINE_FLOAT(MC_ROLLRATE_FF, 0.0f); + +/** * Pitch P gain * * Pitch proportional gain, i.e. desired angular speed in rad/s for error 1 rad. @@ -124,6 +134,16 @@ PARAM_DEFINE_FLOAT(MC_PITCHRATE_I, 0.0f); PARAM_DEFINE_FLOAT(MC_PITCHRATE_D, 0.002f); /** + * Pitch rate feedforward + * + * Improves tracking performance. + * + * @min 0.0 + * @group Multicopter Attitude Control + */ +PARAM_DEFINE_FLOAT(MC_PITCHRATE_FF, 0.0f); + +/** * Yaw P gain * * Yaw proportional gain, i.e. desired angular speed in rad/s for error 1 rad. @@ -165,6 +185,16 @@ PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.0f); PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f); /** + * Yaw rate feedforward + * + * Improves tracking performance. + * + * @min 0.0 + * @group Multicopter Attitude Control + */ +PARAM_DEFINE_FLOAT(MC_YAWRATE_FF, 0.0f); + +/** * Yaw feed forward * * Feed forward weight for manual yaw control. 0 will give slow responce and no overshot, 1 - fast responce and big overshot. diff --git a/src/modules/mc_att_control/module.mk b/src/modules/mc_att_control/module.mk index 64b876f69..f32bc71bf 100644 --- a/src/modules/mc_att_control/module.mk +++ b/src/modules/mc_att_control/module.mk @@ -39,3 +39,7 @@ MODULE_COMMAND = mc_att_control SRCS = mc_att_control_main.cpp \ mc_att_control_params.c + +# Startup handler, the actual app stack size is +# in the task_spawn command +MODULE_STACKSIZE = 1200 diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index d993692ab..58d20ebef 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -1426,7 +1426,7 @@ MulticopterPositionControl::start() _control_task = task_spawn_cmd("mc_pos_control", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, - 1800, + 1600, (main_t)&MulticopterPositionControl::task_main_trampoline, nullptr); diff --git a/src/modules/mc_pos_control/module.mk b/src/modules/mc_pos_control/module.mk index 0b566d7bd..48827897a 100644 --- a/src/modules/mc_pos_control/module.mk +++ b/src/modules/mc_pos_control/module.mk @@ -39,3 +39,7 @@ MODULE_COMMAND = mc_pos_control SRCS = mc_pos_control_main.cpp \ mc_pos_control_params.c + +# Startup handler, the actual app stack size is +# in the task_spawn command +MODULE_STACKSIZE = 1200 diff --git a/src/modules/navigator/geofence.h b/src/modules/navigator/geofence.h index 9d647cb68..37a41b68a 100644 --- a/src/modules/navigator/geofence.h +++ b/src/modules/navigator/geofence.h @@ -76,7 +76,7 @@ public: * @return true: system is inside fence, false: system is outside fence */ bool inside(const struct vehicle_global_position_s &global_position, - const struct vehicle_gps_position_s &gps_position,float baro_altitude_amsl); + const struct vehicle_gps_position_s &gps_position, float baro_altitude_amsl); bool inside_polygon(double lat, double lon, float altitude); int clearDm(); diff --git a/src/modules/position_estimator_inav/inertial_filter.c b/src/modules/position_estimator_inav/inertial_filter.c index a36a4688d..c70cbeb0e 100644 --- a/src/modules/position_estimator_inav/inertial_filter.c +++ b/src/modules/position_estimator_inav/inertial_filter.c @@ -15,6 +15,7 @@ void inertial_filter_predict(float dt, float x[2], float acc) if (!isfinite(acc)) { acc = 0.0f; } + x[0] += x[1] * dt + acc * dt * dt / 2.0f; x[1] += acc * dt; } diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.c b/src/modules/position_estimator_inav/position_estimator_inav_params.c index 91915fb53..ed98e222e 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -292,7 +292,7 @@ PARAM_DEFINE_INT32(CBRK_NO_VISION, 0); * INAV enabled * * If set to 1, use INAV for position estimation - * the system uses the compined attitude / position + * the system uses the combined attitude / position * filter framework. * * @min 0.0 @@ -300,7 +300,7 @@ PARAM_DEFINE_INT32(CBRK_NO_VISION, 0); * @unit s * @group Position Estimator INAV */ -PARAM_DEFINE_INT32(INAV_ENABLED, 0); +PARAM_DEFINE_INT32(INAV_ENABLED, 1); int parameters_init(struct position_estimator_inav_param_handles *h) { diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index d20f776d6..e04ffc940 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -45,6 +45,7 @@ #include <systemlib/perf_counter.h> #include <systemlib/ppm_decode.h> #include <rc/st24.h> +#include <rc/sumd.h> #include "px4io.h" @@ -53,7 +54,7 @@ #define RC_CHANNEL_LOW_THRESH -8000 /* 10% threshold */ static bool ppm_input(uint16_t *values, uint16_t *num_values, uint16_t *frame_len); -static bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated); +static bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool *sumd_updated); static perf_counter_t c_gather_dsm; static perf_counter_t c_gather_sbus; @@ -63,7 +64,7 @@ static int _dsm_fd; static uint16_t rc_value_override = 0; -bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated) +bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool *sumd_updated) { perf_begin(c_gather_dsm); uint16_t temp_count = r_raw_rc_count; @@ -106,7 +107,31 @@ bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated) r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE); } - return (*dsm_updated | *st24_updated); + + /* get data from FD and attempt to parse with SUMD libs */ + uint8_t sumd_rssi, sumd_rx_count; + uint16_t sumd_channel_count = 0; + + *sumd_updated = false; + + for (unsigned i = 0; i < n_bytes; i++) { + /* set updated flag if one complete packet was parsed */ + st24_rssi = RC_INPUT_RSSI_MAX; + *sumd_updated |= (OK == sumd_decode(bytes[i], &sumd_rssi, &sumd_rx_count, + &sumd_channel_count, r_raw_rc_values, PX4IO_RC_INPUT_CHANNELS)); + } + + if (*sumd_updated) { + + *rssi = sumd_rssi; + r_raw_rc_count = sumd_channel_count; + + r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SUMD; + r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP); + r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE); + } + + return (*dsm_updated | *st24_updated | *sumd_updated); } void @@ -169,14 +194,17 @@ controls_tick() { #endif perf_begin(c_gather_dsm); - bool dsm_updated, st24_updated; - (void)dsm_port_input(&rssi, &dsm_updated, &st24_updated); + bool dsm_updated, st24_updated, sumd_updated; + (void)dsm_port_input(&rssi, &dsm_updated, &st24_updated, &sumd_updated); if (dsm_updated) { r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM; } if (st24_updated) { r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_ST24; } + if (sumd_updated) { + r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SUMD; + } perf_end(c_gather_dsm); perf_begin(c_gather_sbus); @@ -238,7 +266,7 @@ controls_tick() { /* * If we received a new frame from any of the RC sources, process it. */ - if (dsm_updated || sbus_updated || ppm_updated || st24_updated) { + if (dsm_updated || sbus_updated || ppm_updated || st24_updated || sumd_updated) { /* record a bitmask of channels assigned */ unsigned assigned_channels = 0; @@ -438,7 +466,7 @@ controls_tick() { r_status_flags |= PX4IO_P_STATUS_FLAGS_OVERRIDE; /* mix new RC input control values to servos */ - if (dsm_updated || sbus_updated || ppm_updated || st24_updated) + if (dsm_updated || sbus_updated || ppm_updated || st24_updated || sumd_updated) mixer_tick(); } else { diff --git a/src/modules/px4iofirmware/module.mk b/src/modules/px4iofirmware/module.mk index 844e493cd..34c231174 100644 --- a/src/modules/px4iofirmware/module.mk +++ b/src/modules/px4iofirmware/module.mk @@ -14,7 +14,8 @@ SRCS = adc.c \ ../systemlib/mixer/mixer_multirotor.cpp \ ../systemlib/mixer/mixer_simple.cpp \ ../systemlib/pwm_limit/pwm_limit.c \ - ../../lib/rc/st24.c + ../../lib/rc/st24.c \ + ../../lib/rc/sumd.c ifeq ($(BOARD),px4io-v1) SRCS += i2c.c diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index bd777428f..874dc0c39 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -115,6 +115,7 @@ #define PX4IO_P_STATUS_FLAGS_SAFETY_OFF (1 << 12) /* safety is off */ #define PX4IO_P_STATUS_FLAGS_FMU_INITIALIZED (1 << 13) /* FMU was initialized and OK once */ #define PX4IO_P_STATUS_FLAGS_RC_ST24 (1 << 14) /* ST24 input is valid */ +#define PX4IO_P_STATUS_FLAGS_RC_SUMD (1 << 15) /* SUMD input is valid */ #define PX4IO_P_STATUS_ALARMS 3 /* alarm flags - alarms latch, write 1 to a bit to clear it */ #define PX4IO_P_STATUS_ALARMS_VBATT_LOW (1 << 0) /* [1] VBatt is very close to regulator dropout */ diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c index d76ec55f0..9d2849090 100644 --- a/src/modules/px4iofirmware/sbus.c +++ b/src/modules/px4iofirmware/sbus.c @@ -142,7 +142,7 @@ sbus1_output(uint16_t *values, uint16_t num_values) value = (uint16_t)(((values[i] - SBUS_SCALE_OFFSET) / SBUS_SCALE_FACTOR) + .5f); /*protect from out of bounds values and limit to 11 bits*/ - if (value > 0x07ff ) { + if (value > 0x07ff) { value = 0x07ff; } diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 7017a65ad..04bf71c17 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1216,12 +1216,13 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- VEHICLE STATUS --- */ if (status_updated) { log_msg.msg_type = LOG_STAT_MSG; - log_msg.body.log_STAT.main_state = (uint8_t) buf_status.main_state; - log_msg.body.log_STAT.arming_state = (uint8_t) buf_status.arming_state; - log_msg.body.log_STAT.failsafe_state = (uint8_t) buf_status.failsafe; + log_msg.body.log_STAT.main_state = buf_status.main_state; + log_msg.body.log_STAT.arming_state = buf_status.arming_state; + log_msg.body.log_STAT.failsafe = (uint8_t) buf_status.failsafe; log_msg.body.log_STAT.battery_remaining = buf_status.battery_remaining; - log_msg.body.log_STAT.battery_warning = (uint8_t) buf_status.battery_warning; + log_msg.body.log_STAT.battery_warning = buf_status.battery_warning; log_msg.body.log_STAT.landed = (uint8_t) buf_status.condition_landed; + log_msg.body.log_STAT.load = buf_status.load; LOGBUFFER_WRITE_AND_COUNT(STAT); } diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 87b4795bc..060dcf62f 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -171,10 +171,11 @@ struct log_ATTC_s { struct log_STAT_s { uint8_t main_state; uint8_t arming_state; - uint8_t failsafe_state; + uint8_t failsafe; float battery_remaining; uint8_t battery_warning; uint8_t landed; + float load; }; /* --- RC - RC INPUT CHANNELS --- */ @@ -480,7 +481,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(GPS, "QBffLLfffffBHHH", "GPSTime,Fix,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog,nSat,SNR,N,J"), LOG_FORMAT_S(ATTC, ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"), LOG_FORMAT_S(ATC1, ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"), - LOG_FORMAT(STAT, "BBBfBB", "MainState,ArmState,FailsafeState,BatRem,BatWarn,Landed"), + LOG_FORMAT(STAT, "BBBfBBf", "MainS,ArmS,Failsafe,BatRem,BatWarn,Landed,Load"), LOG_FORMAT(VTOL, "f", "Arsp"), LOG_FORMAT(RC, "ffffffffBB", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,Count,SignalLost"), LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"), diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 272e4b14f..1acc14fc6 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -682,7 +682,7 @@ PARAM_DEFINE_INT32(SENS_FLOW_ROT, 0); * * @group Sensor Calibration */ - PARAM_DEFINE_FLOAT(SENS_BOARD_Y_OFF, 0.0f); +PARAM_DEFINE_FLOAT(SENS_BOARD_Y_OFF, 0.0f); /** * Board rotation X (Roll) offset diff --git a/src/modules/systemlib/circuit_breaker.cpp b/src/modules/systemlib/circuit_breaker.cpp index ea478a60f..f5ff0afd4 100644 --- a/src/modules/systemlib/circuit_breaker.cpp +++ b/src/modules/systemlib/circuit_breaker.cpp @@ -45,7 +45,7 @@ #include <px4.h> #include <systemlib/circuit_breaker.h> -bool circuit_breaker_enabled(const char* breaker, int32_t magic) +bool circuit_breaker_enabled(const char *breaker, int32_t magic) { int32_t val; (void)PX4_PARAM_GET_BYNAME(breaker, &val); diff --git a/src/modules/systemlib/circuit_breaker.h b/src/modules/systemlib/circuit_breaker.h index c97dbc26f..c76e6c37f 100644 --- a/src/modules/systemlib/circuit_breaker.h +++ b/src/modules/systemlib/circuit_breaker.h @@ -61,7 +61,7 @@ __BEGIN_DECLS -extern "C" __EXPORT bool circuit_breaker_enabled(const char* breaker, int32_t magic); +extern "C" __EXPORT bool circuit_breaker_enabled(const char *breaker, int32_t magic); __END_DECLS diff --git a/src/modules/systemlib/rc_check.h b/src/modules/systemlib/rc_check.h index e70b83cce..035f63bef 100644 --- a/src/modules/systemlib/rc_check.h +++ b/src/modules/systemlib/rc_check.h @@ -39,7 +39,7 @@ #pragma once - __BEGIN_DECLS +__BEGIN_DECLS /** * Check the RC calibration diff --git a/src/modules/systemlib/system_params.c b/src/modules/systemlib/system_params.c index 497b7da37..17ce65d13 100644 --- a/src/modules/systemlib/system_params.c +++ b/src/modules/systemlib/system_params.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -97,3 +97,15 @@ PARAM_DEFINE_INT32(SYS_RESTART_TYPE, 2); * @group System */ PARAM_DEFINE_INT32(SYS_COMPANION, 0); + +/** +* Parameter version +* +* This monotonically increasing number encodes the parameter compatibility set. +* whenever it increases parameters might not be backwards compatible and +* ground control stations should suggest a fresh configuration. +* +* @min 0 +* @group System +*/ +PARAM_DEFINE_INT32(SYS_PARAM_VER, 1); diff --git a/src/modules/uORB/topics/esc_status.h b/src/modules/uORB/topics/esc_status.h index b45c49788..73fe0f936 100644 --- a/src/modules/uORB/topics/esc_status.h +++ b/src/modules/uORB/topics/esc_status.h @@ -92,7 +92,7 @@ struct esc_status_s { struct { enum ESC_VENDOR esc_vendor; /**< Vendor of current ESC */ uint32_t esc_errorcount; /**< Number of reported errors by ESC - if supported */ - int32_t esc_rpm; /**< Motor RPM, negative for reverse rotation [RPM] - if supported */ + int32_t esc_rpm; /**< Motor RPM, negative for reverse rotation [RPM] - if supported */ float esc_voltage; /**< Voltage measured from current ESC [V] - if supported */ float esc_current; /**< Current measured from current ESC [A] - if supported */ float esc_temperature; /**< Temperature measured from current ESC [degC] - if supported */ diff --git a/src/modules/uORB/topics/fence.h b/src/modules/uORB/topics/fence.h index a61f078ba..43cee89fe 100644 --- a/src/modules/uORB/topics/fence.h +++ b/src/modules/uORB/topics/fence.h @@ -65,7 +65,7 @@ struct fence_vertex_s { * This is the position of a geofence * */ -struct fence_s { +struct fence_s { unsigned count; /**< number of actual vertices */ struct fence_vertex_s vertices[GEOFENCE_MAX_VERTICES]; }; diff --git a/src/modules/uORB/topics/vehicle_command.h b/src/modules/uORB/topics/vehicle_command.h index 6b4cb483b..35161a326 100644 --- a/src/modules/uORB/topics/vehicle_command.h +++ b/src/modules/uORB/topics/vehicle_command.h @@ -38,6 +38,7 @@ * @author Thomas Gubler <thomasgubler@student.ethz.ch> * @author Julian Oes <joes@student.ethz.ch> * @author Lorenz Meier <lm@inf.ethz.ch> + * @author Anton Matosov <anton.matosov@gmail.com> */ #ifndef TOPIC_VEHICLE_COMMAND_H_ @@ -85,6 +86,15 @@ enum VEHICLE_CMD { VEHICLE_CMD_DO_REPEAT_SERVO = 184, /* Cycle a between its nominal setting and a desired PWM for a desired number of cycles with a desired period. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Cycle count| Cycle time (seconds)| Empty| Empty| Empty| */ VEHICLE_CMD_DO_FLIGHTTERMINATION=185, /* Terminate flight immediately |Flight termination activated if > 0.5| Empty| Empty| Empty| Empty| Empty| Empty| */ VEHICLE_CMD_DO_CONTROL_VIDEO = 200, /* Control onboard camera system. |Camera ID (-1 for all)| Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw| Transmission mode: 0: video stream, >0: single images every n seconds (decimal)| Recording: 0: disabled, 1: enabled compressed, 2: enabled raw| Empty| Empty| Empty| */ + VEHICLE_CMD_DO_MOUNT_CONFIGURE=204, /* Mission command to configure a camera or antenna mount |Mount operation mode (see MAV_MOUNT_MODE enum)| stabilize roll? (1 = yes, 0 = no)| stabilize pitch? (1 = yes, 0 = no)| stabilize yaw? (1 = yes, 0 = no)| Empty| Empty| Empty| */ + VEHICLE_CMD_DO_MOUNT_CONTROL=205, /* Mission command to control a camera or antenna mount |pitch or lat in degrees, depending on mount mode.| roll or lon in degrees depending on mount mode| yaw or alt (in meters) depending on mount mode| reserved| reserved| reserved| MAV_MOUNT_MODE enum value| */ + VEHICLE_CMD_DO_SET_CAM_TRIGG_DIST=206, /* Mission command to set CAM_TRIGG_DIST for this flight |Camera trigger distance (meters)| Empty| Empty| Empty| Empty| Empty| Empty| */ + VEHICLE_CMD_DO_FENCE_ENABLE=207, /* Mission command to enable the geofence |enable? (0=disable, 1=enable)| Empty| Empty| Empty| Empty| Empty| Empty| */ + VEHICLE_CMD_DO_PARACHUTE=208, /* Mission command to trigger a parachute |action (0=disable, 1=enable, 2=release, for some systems see PARACHUTE_ACTION enum, not in general message set.)| Empty| Empty| Empty| Empty| Empty| Empty| */ + VEHICLE_CMD_DO_INVERTED_FLIGHT=210, /* Change to/from inverted flight |inverted (0=normal, 1=inverted)| Empty| Empty| Empty| Empty| Empty| Empty| */ + VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT=220, /* Mission command to control a camera or antenna mount, using a quaternion as reference. |q1 - quaternion param #1, w (1 in null-rotation)| q2 - quaternion param #2, x (0 in null-rotation)| q3 - quaternion param #3, y (0 in null-rotation)| q4 - quaternion param #4, z (0 in null-rotation)| Empty| Empty| Empty| */ + VEHICLE_CMD_DO_GUIDED_MASTER=221, /* set id of master controller |System ID| Component ID| Empty| Empty| Empty| Empty| Empty| */ + VEHICLE_CMD_DO_GUIDED_LIMITS=222, /* set limits for external control |timeout - maximum time (in seconds) that external controller will be allowed to control vehicle. 0 means no timeout| absolute altitude min (in meters, AMSL) - if vehicle moves below this alt, the command will be aborted and the mission will continue. 0 means no lower altitude limit| absolute altitude max (in meters)- if vehicle moves above this alt, the command will be aborted and the mission will continue. 0 means no upper altitude limit| horizontal move limit (in meters, AMSL) - if vehicle moves more than this distance from it's location at the moment the command was executed, the command will be aborted and the mission will continue. 0 means no horizontal altitude limit| Empty| Empty| Empty| */ VEHICLE_CMD_DO_LAST = 240, /* NOP - This command is only used to mark the upper limit of the DO commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ VEHICLE_CMD_PREFLIGHT_CALIBRATION = 241, /* Trigger calibration. This command will be only accepted if in pre-flight mode. |Gyro calibration: 0: no, 1: yes| Magnetometer calibration: 0: no, 1: yes| Ground pressure: 0: no, 1: yes| Radio calibration: 0: no, 1: yes| Accelerometer calibration: 0: no, 1: yes| Empty| Empty| */ VEHICLE_CMD_PREFLIGHT_SET_SENSOR_OFFSETS = 242, /* Set sensor offsets. This command will be only accepted if in pre-flight mode. |Sensor to adjust the offsets for: 0: gyros, 1: accelerometer, 2: magnetometer, 3: barometer, 4: optical flow| X axis offset (or generic dimension 1), in the sensor's raw units| Y axis offset (or generic dimension 2), in the sensor's raw units| Z axis offset (or generic dimension 3), in the sensor's raw units| Generic dimension 4, in the sensor's raw units| Generic dimension 5, in the sensor's raw units| Generic dimension 6, in the sensor's raw units| */ @@ -101,7 +111,7 @@ enum VEHICLE_CMD { /** * Commands for commander app. * - * Should contain all of MAVLink's VEHICLE_CMD_RESULT values + * Should contain all of MAVLink's MAV_CMD_RESULT values * but can contain additional ones. */ enum VEHICLE_CMD_RESULT { @@ -114,6 +124,22 @@ enum VEHICLE_CMD_RESULT { }; /** + * Commands for gimbal app. + * + * Should contain all of MAVLink's MAV_MOUNT_MODE values + * but can contain additional ones. + */ +typedef enum VEHICLE_MOUNT_MODE +{ + VEHICLE_MOUNT_MODE_RETRACT=0, /* Load and keep safe position (Roll,Pitch,Yaw) from permant memory and stop stabilization | */ + VEHICLE_MOUNT_MODE_NEUTRAL=1, /* Load and keep neutral position (Roll,Pitch,Yaw) from permanent memory. | */ + VEHICLE_MOUNT_MODE_MAVLINK_TARGETING=2, /* Load neutral position and start MAVLink Roll,Pitch,Yaw control with stabilization | */ + VEHICLE_MOUNT_MODE_RC_TARGETING=3, /* Load neutral position and start RC Roll,Pitch,Yaw control with stabilization | */ + VEHICLE_MOUNT_MODE_GPS_POINT=4, /* Load neutral position and start to point to Lat,Lon,Alt | */ + VEHICLE_MOUNT_MODE_ENUM_END=5, /* | */ +} VEHICLE_MOUNT_MODE; + +/** * @addtogroup topics * @{ */ diff --git a/src/modules/uORB/topics/vehicle_global_position.h b/src/modules/uORB/topics/vehicle_global_position.h index 137c86dd5..50c942961 100644 --- a/src/modules/uORB/topics/vehicle_global_position.h +++ b/src/modules/uORB/topics/vehicle_global_position.h @@ -74,6 +74,7 @@ struct vehicle_global_position_s { float epv; /**< Standard deviation of position vertically */ float terrain_alt; /**< Terrain altitude in m, WGS84 */ bool terrain_alt_valid; /**< Terrain altitude estimate is valid */ + bool dead_reckoning; /**< True if this position is estimated through dead-reckoning*/ }; /** diff --git a/src/platforms/ros/nodes/demo_offboard_position_setpoints/demo_offboard_position_setpoints.cpp b/src/platforms/ros/nodes/demo_offboard_position_setpoints/demo_offboard_position_setpoints.cpp index 7366d7fc6..125ceaddc 100644 --- a/src/platforms/ros/nodes/demo_offboard_position_setpoints/demo_offboard_position_setpoints.cpp +++ b/src/platforms/ros/nodes/demo_offboard_position_setpoints/demo_offboard_position_setpoints.cpp @@ -66,6 +66,7 @@ int DemoOffboardPositionSetpoints::main() pose.pose.position.z = 1; _local_position_sp_pub.publish(pose); } + return 0; } diff --git a/src/platforms/ros/nodes/mavlink/mavlink.cpp b/src/platforms/ros/nodes/mavlink/mavlink.cpp index 5459fcffd..2758979a2 100644 --- a/src/platforms/ros/nodes/mavlink/mavlink.cpp +++ b/src/platforms/ros/nodes/mavlink/mavlink.cpp @@ -57,7 +57,8 @@ Mavlink::Mavlink() : { _link = mavconn::MAVConnInterface::open_url("udp://localhost:14565@localhost:14560"); _link->message_received.connect(boost::bind(&Mavlink::handle_msg, this, _1, _2, _3)); - + _att_sp = {}; + _offboard_control_mode = {}; } int main(int argc, char **argv) @@ -127,10 +128,8 @@ void Mavlink::handle_msg_set_attitude_target(const mavlink_message_t *mmsg) mavlink_set_attitude_target_t set_attitude_target; mavlink_msg_set_attitude_target_decode(mmsg, &set_attitude_target); - static offboard_control_mode offboard_control_mode; - /* set correct ignore flags for thrust field: copy from mavlink message */ - offboard_control_mode.ignore_thrust = (bool)(set_attitude_target.type_mask & (1 << 6)); + _offboard_control_mode.ignore_thrust = (bool)(set_attitude_target.type_mask & (1 << 6)); /* * if attitude or body rate have been used (not ignored) previously and this message only sends @@ -140,45 +139,46 @@ void Mavlink::handle_msg_set_attitude_target(const mavlink_message_t *mmsg) bool ignore_bodyrate = (bool)(set_attitude_target.type_mask & 0x7); bool ignore_attitude = (bool)(set_attitude_target.type_mask & (1 << 7)); - if (ignore_bodyrate && ignore_attitude && !offboard_control_mode.ignore_thrust) { + if (ignore_bodyrate && ignore_attitude && !_offboard_control_mode.ignore_thrust) { /* Message want's us to ignore everything except thrust: only ignore if previously ignored */ - offboard_control_mode.ignore_bodyrate = ignore_bodyrate && offboard_control_mode.ignore_bodyrate; - offboard_control_mode.ignore_attitude = ignore_attitude && offboard_control_mode.ignore_attitude; + _offboard_control_mode.ignore_bodyrate = ignore_bodyrate && _offboard_control_mode.ignore_bodyrate; + _offboard_control_mode.ignore_attitude = ignore_attitude && _offboard_control_mode.ignore_attitude; } else { - offboard_control_mode.ignore_bodyrate = ignore_bodyrate; - offboard_control_mode.ignore_attitude = ignore_attitude; + _offboard_control_mode.ignore_bodyrate = ignore_bodyrate; + _offboard_control_mode.ignore_attitude = ignore_attitude; } - offboard_control_mode.ignore_position = true; - offboard_control_mode.ignore_velocity = true; - offboard_control_mode.ignore_acceleration_force = true; - - offboard_control_mode.timestamp = get_time_micros(); - _offboard_control_mode_pub.publish(offboard_control_mode); + _offboard_control_mode.ignore_position = true; + _offboard_control_mode.ignore_velocity = true; + _offboard_control_mode.ignore_acceleration_force = true; - static vehicle_attitude_setpoint att_sp = {}; + _offboard_control_mode.timestamp = get_time_micros(); + _offboard_control_mode_pub.publish(_offboard_control_mode); /* The real mavlink app has a ckeck at this location which makes sure that the attitude setpoint * gets published only if in offboard mode. We leave that out for now. */ - att_sp.timestamp = get_time_micros(); - mavlink_quaternion_to_euler(set_attitude_target.q, &att_sp.roll_body, &att_sp.pitch_body, - &att_sp.yaw_body); - mavlink_quaternion_to_dcm(set_attitude_target.q, (float(*)[3])att_sp.R_body.data()); - att_sp.R_valid = true; + _att_sp.timestamp = get_time_micros(); + if (!ignore_attitude) { + mavlink_quaternion_to_euler(set_attitude_target.q, &_att_sp.roll_body, &_att_sp.pitch_body, + &_att_sp.yaw_body); + mavlink_quaternion_to_dcm(set_attitude_target.q, (float(*)[3])_att_sp.R_body.data()); + _att_sp.R_valid = true; + } + - if (!offboard_control_mode.ignore_thrust) { - att_sp.thrust = set_attitude_target.thrust; + if (!_offboard_control_mode.ignore_thrust) { + _att_sp.thrust = set_attitude_target.thrust; } - if (!offboard_control_mode.ignore_attitude) { + if (!ignore_attitude) { for (ssize_t i = 0; i < 4; i++) { - att_sp.q_d[i] = set_attitude_target.q[i]; + _att_sp.q_d[i] = set_attitude_target.q[i]; } - att_sp.q_d_valid = true; + _att_sp.q_d_valid = true; } - _v_att_sp_pub.publish(att_sp); + _v_att_sp_pub.publish(_att_sp); //XXX real mavlink publishes rate sp here diff --git a/src/platforms/ros/nodes/mavlink/mavlink.h b/src/platforms/ros/nodes/mavlink/mavlink.h index acb2408f3..8b7d08d24 100644 --- a/src/platforms/ros/nodes/mavlink/mavlink.h +++ b/src/platforms/ros/nodes/mavlink/mavlink.h @@ -69,6 +69,8 @@ protected: ros::Publisher _pos_sp_triplet_pub; ros::Publisher _offboard_control_mode_pub; ros::Publisher _force_sp_pub; + vehicle_attitude_setpoint _att_sp; + offboard_control_mode _offboard_control_mode; /** * diff --git a/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp index 1098ec73b..9b48294b6 100644 --- a/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp +++ b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp @@ -40,7 +40,7 @@ #include <ros/ros.h> #include <px4.h> #include <lib/mathlib/mathlib.h> -#include <mav_msgs/MotorSpeed.h> +#include <mav_msgs/CommandMotorSpeed.h> #include <string> class MultirotorMixer @@ -129,7 +129,7 @@ MultirotorMixer::MultirotorMixer(): _rotors(_config_index[0]) { _sub = _n.subscribe("actuator_controls_0", 1, &MultirotorMixer::actuatorControlsCallback, this); - _pub = _n.advertise<mav_msgs::MotorSpeed>("/mixed_motor_commands", 10); + _pub = _n.advertise<mav_msgs::CommandMotorSpeed>("command/motor_speed", 10); if (!_n.hasParam("motor_scaling_radps")) { _n.setParam("motor_scaling_radps", 150.0); @@ -237,7 +237,7 @@ void MultirotorMixer::actuatorControlsCallback(const px4::actuator_controls_0 &m mix(); // publish message - mav_msgs::MotorSpeed rotor_vel_msg; + mav_msgs::CommandMotorSpeed rotor_vel_msg; double scaling; double offset; _n.getParamCached("motor_scaling_radps", scaling); diff --git a/src/systemcmds/boardinfo/boardinfo.c b/src/systemcmds/boardinfo/boardinfo.c deleted file mode 100644 index 3ff5a066c..000000000 --- a/src/systemcmds/boardinfo/boardinfo.c +++ /dev/null @@ -1,409 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file boardinfo.c - * autopilot and carrier board information app - */ - -#include <nuttx/config.h> -#include <unistd.h> -#include <stdio.h> -#include <string.h> -#include <stdlib.h> - -#include <nuttx/i2c.h> - -#include <systemlib/systemlib.h> -#include <systemlib/err.h> -#include <systemlib/bson/tinybson.h> - -__EXPORT int boardinfo_main(int argc, char *argv[]); - -struct eeprom_info_s -{ - unsigned bus; - unsigned address; - unsigned page_size; - unsigned page_count; - unsigned page_write_delay; -}; - -/* XXX currently code below only supports 8-bit addressing */ -const struct eeprom_info_s eeprom_info[] = { - {3, 0x57, 8, 16, 3300}, -}; - -struct board_parameter_s { - const char *name; - bson_type_t type; -}; - -const struct board_parameter_s board_parameters[] = { - {"name", BSON_STRING}, /* ascii board name */ - {"vers", BSON_INT32}, /* board version (major << 8) | minor */ - {"date", BSON_INT32}, /* manufacture date */ - {"build", BSON_INT32} /* build code (fab << 8) | tester */ -}; - -const unsigned num_parameters = sizeof(board_parameters) / sizeof(board_parameters[0]); - -static int -eeprom_write(const struct eeprom_info_s *eeprom, uint8_t *buf, unsigned size) -{ - int result = -1; - - struct i2c_dev_s *dev = up_i2cinitialize(eeprom->bus); - if (dev == NULL) { - warnx("failed to init bus %d for EEPROM", eeprom->bus); - goto out; - } - I2C_SETFREQUENCY(dev, 400000); - - /* loop until all data has been transferred */ - for (unsigned address = 0; address < size; ) { - - uint8_t pagebuf[eeprom->page_size + 1]; - - /* how many bytes available to transfer? */ - unsigned count = size - address; - - /* constrain writes to the page size */ - if (count > eeprom->page_size) - count = eeprom->page_size; - - pagebuf[0] = address & 0xff; - memcpy(pagebuf + 1, buf + address, count); - - struct i2c_msg_s msgv[1] = { - { - .addr = eeprom->address, - .flags = 0, - .buffer = pagebuf, - .length = count + 1 - } - }; - - result = I2C_TRANSFER(dev, msgv, 1); - if (result != OK) { - warnx("EEPROM write failed: %d", result); - goto out; - } - usleep(eeprom->page_write_delay); - address += count; - } - -out: - if (dev != NULL) - up_i2cuninitialize(dev); - return result; -} - -static int -eeprom_read(const struct eeprom_info_s *eeprom, uint8_t *buf, unsigned size) -{ - int result = -1; - - struct i2c_dev_s *dev = up_i2cinitialize(eeprom->bus); - if (dev == NULL) { - warnx("failed to init bus %d for EEPROM", eeprom->bus); - goto out; - } - I2C_SETFREQUENCY(dev, 400000); - - /* loop until all data has been transferred */ - for (unsigned address = 0; address < size; ) { - - /* how many bytes available to transfer? */ - unsigned count = size - address; - - /* constrain transfers to the page size (bus anti-hog) */ - if (count > eeprom->page_size) - count = eeprom->page_size; - - uint8_t addr = address; - struct i2c_msg_s msgv[2] = { - { - .addr = eeprom->address, - .flags = 0, - .buffer = &addr, - .length = 1 - }, - { - .addr = eeprom->address, - .flags = I2C_M_READ, - .buffer = buf + address, - .length = count - } - }; - - result = I2C_TRANSFER(dev, msgv, 2); - if (result != OK) { - warnx("EEPROM read failed: %d", result); - goto out; - } - address += count; - } - -out: - if (dev != NULL) - up_i2cuninitialize(dev); - return result; -} - -static void * -idrom_read(const struct eeprom_info_s *eeprom) -{ - uint32_t size = 0xffffffff; - int result; - void *buf = NULL; - - result = eeprom_read(eeprom, (uint8_t *)&size, sizeof(size)); - if (result != 0) { - warnx("failed reading ID ROM length"); - goto fail; - } - if (size > (eeprom->page_size * eeprom->page_count)) { - warnx("ID ROM not programmed"); - goto fail; - } - - buf = malloc(size); - if (buf == NULL) { - warnx("could not allocate %d bytes for ID ROM", size); - goto fail; - } - result = eeprom_read(eeprom, buf, size); - if (result != 0) { - warnx("failed reading ID ROM"); - goto fail; - } - return buf; - -fail: - if (buf != NULL) - free(buf); - return NULL; -} - -static void -boardinfo_set(const struct eeprom_info_s *eeprom, char *spec) -{ - struct bson_encoder_s encoder; - int result = 1; - char *state, *token; - unsigned i; - - /* create the encoder and make a writable copy of the spec */ - bson_encoder_init_buf(&encoder, NULL, 0); - - for (i = 0, token = strtok_r(spec, ",", &state); - token && (i < num_parameters); - i++, token = strtok_r(NULL, ",", &state)) { - - switch (board_parameters[i].type) { - case BSON_STRING: - result = bson_encoder_append_string(&encoder, board_parameters[i].name, token); - break; - case BSON_INT32: - result = bson_encoder_append_int(&encoder, board_parameters[i].name, strtoul(token, NULL, 0)); - break; - default: - result = 1; - } - if (result) { - warnx("bson append failed for %s<%s>", board_parameters[i].name, token); - goto out; - } - } - bson_encoder_fini(&encoder); - if (i != num_parameters) { - warnx("incorrect parameter list, expected: \"<name>,<version><date>,<buildcode>\""); - result = 1; - goto out; - } - if (bson_encoder_buf_size(&encoder) > (int)(eeprom->page_size * eeprom->page_count)) { - warnx("data too large for EEPROM"); - result = 1; - goto out; - } - if ((int)*(uint32_t *)bson_encoder_buf_data(&encoder) != bson_encoder_buf_size(&encoder)) { - warnx("buffer length mismatch"); - result = 1; - goto out; - } - warnx("writing %p/%u", bson_encoder_buf_data(&encoder), bson_encoder_buf_size(&encoder)); - - result = eeprom_write(eeprom, (uint8_t *)bson_encoder_buf_data(&encoder), bson_encoder_buf_size(&encoder)); - if (result < 0) { - warnc(-result, "error writing EEPROM"); - result = 1; - } else { - result = 0; - } - -out: - free(bson_encoder_buf_data(&encoder)); - - exit(result); -} - -static int -boardinfo_print(bson_decoder_t decoder, void *private, bson_node_t node) -{ - switch (node->type) { - case BSON_INT32: - printf("%s: %d / 0x%08x\n", node->name, (int)node->i, (unsigned)node->i); - break; - case BSON_STRING: { - char buf[bson_decoder_data_pending(decoder)]; - bson_decoder_copy_data(decoder, buf); - printf("%s: %s\n", node->name, buf); - break; - } - case BSON_EOO: - break; - default: - warnx("unexpected node type %d", node->type); - break; - } - return 1; -} - -static void -boardinfo_show(const struct eeprom_info_s *eeprom) -{ - struct bson_decoder_s decoder; - void *buf; - - buf = idrom_read(eeprom); - if (buf == NULL) - errx(1, "ID ROM read failed"); - - if (bson_decoder_init_buf(&decoder, buf, 0, boardinfo_print, NULL) == 0) { - while (bson_decoder_next(&decoder) > 0) - ; - } else { - warnx("failed to init decoder"); - } - free(buf); - exit(0); -} - -struct { - const char *property; - const char *value; -} test_args; - -static int -boardinfo_test_callback(bson_decoder_t decoder, void *private, bson_node_t node) -{ - /* reject nodes with non-matching names */ - if (strcmp(node->name, test_args.property)) - return 1; - - /* compare node values to check for a match */ - switch (node->type) { - case BSON_STRING: { - char buf[bson_decoder_data_pending(decoder)]; - bson_decoder_copy_data(decoder, buf); - - /* check for a match */ - if (!strcmp(test_args.value, buf)) { - return 2; - } - break; - } - - case BSON_INT32: { - int32_t val = strtol(test_args.value, NULL, 0); - - /* check for a match */ - if (node->i == val) { - return 2; - } - break; - } - - default: - break; - } - - return 1; -} - -static void -boardinfo_test(const struct eeprom_info_s *eeprom, const char *property, const char *value) -{ - struct bson_decoder_s decoder; - void *buf; - int result = -1; - - if ((property == NULL) || (strlen(property) == 0) || - (value == NULL) || (strlen(value) == 0)) - errx(1, "missing property name or value"); - - test_args.property = property; - test_args.value = value; - - buf = idrom_read(eeprom); - if (buf == NULL) - errx(1, "ID ROM read failed"); - - if (bson_decoder_init_buf(&decoder, buf, 0, boardinfo_test_callback, NULL) == 0) { - do { - result = bson_decoder_next(&decoder); - } while (result == 1); - } else { - warnx("failed to init decoder"); - } - free(buf); - - /* if we matched, we exit with zero success */ - exit((result == 2) ? 0 : 1); -} - -int -boardinfo_main(int argc, char *argv[]) -{ - if (!strcmp(argv[1], "set")) - boardinfo_set(&eeprom_info[0], argv[2]); - - if (!strcmp(argv[1], "show")) - boardinfo_show(&eeprom_info[0]); - - if (!strcmp(argv[1], "test")) - boardinfo_test(&eeprom_info[0], argv[2], argv[3]); - - errx(1, "missing/unrecognised command, try one of 'set', 'show', 'test'"); -} diff --git a/src/systemcmds/tests/module.mk b/src/systemcmds/tests/module.mk index 1d728e857..4b7b73ac6 100644 --- a/src/systemcmds/tests/module.mk +++ b/src/systemcmds/tests/module.mk @@ -32,7 +32,8 @@ SRCS = test_adc.c \ test_ppm_loopback.c \ test_rc.c \ test_conv.cpp \ - test_mount.c + test_mount.c \ + test_eigen.cpp EXTRACXXFLAGS = -Wframe-larger-than=2500 -Wno-float-equal -Wno-double-promotion diff --git a/src/systemcmds/tests/test_eigen.cpp b/src/systemcmds/tests/test_eigen.cpp new file mode 100644 index 000000000..de5630cc3 --- /dev/null +++ b/src/systemcmds/tests/test_eigen.cpp @@ -0,0 +1,422 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file test_eigen.cpp + * + * Eigen test (based of mathlib test) + * @author Johan Jansen <jnsn.johan@gmail.com> + */ + +#include <px4_eigen.h> +#include <float.h> +#include <stdio.h> +#include <stdlib.h> +#include <unistd.h> +#include <string.h> +#include <time.h> +#include <systemlib/err.h> +#include <drivers/drv_hrt.h> + +#include "tests.h" + +namespace Eigen +{ + typedef Matrix<float, 10, 1> Vector10f; +} + +static constexpr size_t OPERATOR_ITERATIONS = 60000; + +#define TEST_OP(_title, _op) \ +{ \ + const hrt_abstime t0 = hrt_absolute_time(); \ + for (size_t j = 0; j < OPERATOR_ITERATIONS; j++) { \ + _op; \ + } \ + printf(_title ": %.6fus", static_cast<double>(hrt_absolute_time() - t0) / OPERATOR_ITERATIONS); \ +} + +#define VERIFY_OP(_title, _op, __OP_TEST__) \ +{ \ + _op; \ + if(!(__OP_TEST__)) { \ + printf(_title " Failed! ("#__OP_TEST__")"); \ + } \ +} + +#define TEST_OP_VERIFY(_title, _op, __OP_TEST__) \ + VERIFY_OP(_title, _op, __OP_TEST__) \ + TEST_OP(_title, _op) + +/** +* @brief +* Prints an Eigen::Matrix to stdout +**/ +template<typename T> +void printEigen(const Eigen::MatrixBase<T>& b) +{ + for(int i = 0; i < b.rows(); ++i) { + printf("("); + for(int j = 0; j < b.cols(); ++i) { + if(j > 0) { + printf(","); + } + + printf("%.3f", static_cast<double>(b(i, j))); + } + printf(")%s\n", i+1 < b.rows() ? "," : ""); + } +} + +/** +* @brief +* Construct new Eigen::Quaternion from euler angles +**/ +template<typename T> +Eigen::Quaternion<T> quatFromEuler(const T roll, const T pitch, const T yaw) +{ + Eigen::AngleAxis<T> rollAngle(roll, Eigen::Matrix<T, 3, 1>::UnitZ()); + Eigen::AngleAxis<T> yawAngle(yaw, Eigen::Matrix<T, 3, 1>::UnitY()); + Eigen::AngleAxis<T> pitchAngle(pitch, Eigen::Matrix<T, 3, 1>::UnitX()); + return rollAngle * yawAngle * pitchAngle; +} + + +int test_eigen(int argc, char *argv[]) +{ + int rc = 0; + warnx("testing eigen"); + + { + Eigen::Vector2f v; + Eigen::Vector2f v1(1.0f, 2.0f); + Eigen::Vector2f v2(1.0f, -1.0f); + float data[2] = {1.0f, 2.0f}; + TEST_OP("Constructor Vector2f()", Eigen::Vector2f v3); + TEST_OP_VERIFY("Constructor Vector2f(Vector2f)", Eigen::Vector2f v3(v1), v3.isApprox(v1)); + TEST_OP_VERIFY("Constructor Vector2f(float[])", Eigen::Vector2f v3(data), v3[0] == data[0] && v3[1] == data[1]); + TEST_OP_VERIFY("Constructor Vector2f(float, float)", Eigen::Vector2f v3(1.0f, 2.0f), v3(0) == 1.0f && v3(1) == 2.0f); + TEST_OP_VERIFY("Vector2f = Vector2f", v = v1, v.isApprox(v1)); + TEST_OP_VERIFY("Vector2f + Vector2f", v + v1, v.isApprox(v1+v1)); + TEST_OP_VERIFY("Vector2f - Vector2f", v - v1, v.isApprox(v1)); + TEST_OP_VERIFY("Vector2f += Vector2f", v += v1, v.isApprox(v1+v1)); + TEST_OP_VERIFY("Vector2f -= Vector2f", v -= v1, v.isApprox(v1)); + TEST_OP_VERIFY("Vector2f dot Vector2f", v.dot(v1), fabs(v.dot(v1) - 5.0f) <= FLT_EPSILON); + //TEST_OP("Vector2f cross Vector2f", v1.cross(v2)); //cross product for 2d array? + } + + { + Eigen::Vector3f v; + Eigen::Vector3f v1(1.0f, 2.0f, 0.0f); + Eigen::Vector3f v2(1.0f, -1.0f, 2.0f); + float data[3] = {1.0f, 2.0f, 3.0f}; + TEST_OP("Constructor Vector3f()", Eigen::Vector3f v3); + TEST_OP("Constructor Vector3f(Vector3f)", Eigen::Vector3f v3(v1)); + TEST_OP("Constructor Vector3f(float[])", Eigen::Vector3f v3(data)); + TEST_OP("Constructor Vector3f(float, float, float)", Eigen::Vector3f v3(1.0f, 2.0f, 3.0f)); + TEST_OP("Vector3f = Vector3f", v = v1); + TEST_OP("Vector3f + Vector3f", v + v1); + TEST_OP("Vector3f - Vector3f", v - v1); + TEST_OP("Vector3f += Vector3f", v += v1); + TEST_OP("Vector3f -= Vector3f", v -= v1); + TEST_OP("Vector3f * float", v1 * 2.0f); + TEST_OP("Vector3f / float", v1 / 2.0f); + TEST_OP("Vector3f *= float", v1 *= 2.0f); + TEST_OP("Vector3f /= float", v1 /= 2.0f); + TEST_OP("Vector3f dot Vector3f", v.dot(v1)); + TEST_OP("Vector3f cross Vector3f", v1.cross(v2)); + TEST_OP("Vector3f length", v1.norm()); + TEST_OP("Vector3f length squared", v1.squaredNorm()); +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wunused-variable" + // Need pragma here intead of moving variable out of TEST_OP and just reference because + // TEST_OP measures performance of vector operations. + TEST_OP("Vector<3> element read", volatile float a = v1(0)); + TEST_OP("Vector<3> element read direct", volatile float a = v1.data()[0]); +#pragma GCC diagnostic pop + TEST_OP("Vector<3> element write", v1(0) = 1.0f); + TEST_OP("Vector<3> element write direct", v1.data()[0] = 1.0f); + } + + { + Eigen::Vector4f v; + Eigen::Vector4f v1(1.0f, 2.0f, 0.0f, -1.0f); + Eigen::Vector4f v2(1.0f, -1.0f, 2.0f, 0.0f); + float data[4] = {1.0f, 2.0f, 3.0f, 4.0f}; + TEST_OP("Constructor Vector<4>()", Eigen::Vector4f v3); + TEST_OP("Constructor Vector<4>(Vector<4>)", Eigen::Vector4f v3(v1)); + TEST_OP("Constructor Vector<4>(float[])", Eigen::Vector4f v3(data)); + TEST_OP("Constructor Vector<4>(float, float, float, float)", Eigen::Vector4f v3(1.0f, 2.0f, 3.0f, 4.0f)); + TEST_OP("Vector<4> = Vector<4>", v = v1); + TEST_OP("Vector<4> + Vector<4>", v + v1); + TEST_OP("Vector<4> - Vector<4>", v - v1); + TEST_OP("Vector<4> += Vector<4>", v += v1); + TEST_OP("Vector<4> -= Vector<4>", v -= v1); + TEST_OP("Vector<4> dot Vector<4>", v.dot(v1)); + } + + { + Eigen::Vector10f v1; + v1.Zero(); + float data[10]; + TEST_OP("Constructor Vector<10>()", Eigen::Vector10f v3); + TEST_OP("Constructor Vector<10>(Vector<10>)", Eigen::Vector10f v3(v1)); + TEST_OP("Constructor Vector<10>(float[])", Eigen::Vector10f v3(data)); + } + + { + Eigen::Matrix3f m1; + m1.setIdentity(); + Eigen::Matrix3f m2; + m2.setIdentity(); + Eigen::Vector3f v1(1.0f, 2.0f, 0.0f); + TEST_OP("Matrix3f * Vector3f", m1 * v1); + TEST_OP("Matrix3f + Matrix3f", m1 + m2); + TEST_OP("Matrix3f * Matrix3f", m1 * m2); + } + + { + Eigen::Matrix<float, 10, 10> m1; + m1.setIdentity(); + Eigen::Matrix<float, 10, 10> m2; + m2.setIdentity(); + Eigen::Vector10f v1; + v1.Zero(); + TEST_OP("Matrix<10, 10> * Vector<10>", m1 * v1); + TEST_OP("Matrix<10, 10> + Matrix<10, 10>", m1 + m2); + TEST_OP("Matrix<10, 10> * Matrix<10, 10>", m1 * m2); + } + + { + warnx("Nonsymmetric matrix operations test"); + // test nonsymmetric +, -, +=, -= + + const Eigen::Matrix<float, 2, 3> m1_orig = + (Eigen::Matrix<float, 2, 3>() << 1, 3, 3, + 4, 6, 6).finished(); + + Eigen::Matrix<float, 2, 3> m1(m1_orig); + + Eigen::Matrix<float, 2, 3> m2; + m2 << 2, 4, 6, + 8, 10, 12; + + Eigen::Matrix<float, 2, 3> m3; + m3 << 3, 6, 9, + 12, 15, 18; + + if (m1 + m2 != m3) { + warnx("Matrix<2, 3> + Matrix<2, 3> failed!"); + printEigen(m1 + m2); + printf("!=\n"); + printEigen(m3); + rc = 1; + } + + if (m3 - m2 != m1) { + warnx("Matrix<2, 3> - Matrix<2, 3> failed!"); + printEigen(m3 - m2); + printf("!=\n"); + printEigen(m1); + rc = 1; + } + + m1 += m2; + + if (m1 != m3) { + warnx("Matrix<2, 3> += Matrix<2, 3> failed!"); + printEigen(m1); + printf("!=\n"); + printEigen(m3); + rc = 1; + } + + m1 -= m2; + + if (m1 != m1_orig) { + warnx("Matrix<2, 3> -= Matrix<2, 3> failed!"); + printEigen(m1); + printf("!=\n"); + printEigen(m1_orig); + rc = 1; + } + + } + + { + // test conversion rotation matrix to quaternion and back + Eigen::Matrix3f R_orig; + Eigen::Matrix3f R; + Eigen::Quaternionf q; + float diff = 0.1f; + float tol = 0.00001f; + + warnx("Quaternion transformation methods test."); + + for (float roll = -M_PI_F; roll <= M_PI_F; roll += diff) { + for (float pitch = -M_PI_2_F; pitch <= M_PI_2_F; pitch += diff) { + for (float yaw = -M_PI_F; yaw <= M_PI_F; yaw += diff) { + R_orig.eulerAngles(roll, pitch, yaw); + q = R_orig; //from_dcm + R = q.toRotationMatrix(); + + for (size_t i = 0; i < 3; i++) { + for (size_t j = 0; j < 3; j++) { + if (fabsf(R_orig(i, j) - R(i, j)) > 0.00001f) { + warnx("Quaternion method 'from_dcm' or 'toRotationMatrix' outside tolerance!"); + rc = 1; + } + } + } + } + } + } + + // test against some known values + tol = 0.0001f; + Eigen::Quaternionf q_true = {1.0f, 0.0f, 0.0f, 0.0f}; + R_orig.setIdentity(); + q = R_orig; + + for (size_t i = 0; i < 4; i++) { + if (!q.isApprox(q_true, tol)) { + warnx("Quaternion method 'from_dcm()' outside tolerance!"); + rc = 1; + } + } + + q_true = quatFromEuler(0.3f, 0.2f, 0.1f); + q = {0.9833f, 0.1436f, 0.1060f, 0.0343f}; + + for (size_t i = 0; i < 4; i++) { + if (!q.isApprox(q_true, tol)) { + warnx("Quaternion method 'eulerAngles()' outside tolerance!"); + rc = 1; + } + } + + q_true = quatFromEuler(-1.5f, -0.2f, 0.5f); + q = {0.7222f, -0.6391f, -0.2386f, 0.1142f}; + + for (size_t i = 0; i < 4; i++) { + if (!q.isApprox(q_true, tol)) { + warnx("Quaternion method 'eulerAngles()' outside tolerance!"); + rc = 1; + } + } + + q_true = quatFromEuler(M_PI_2_F, -M_PI_2_F, -M_PI_F / 3); + q = {0.6830f, 0.1830f, -0.6830f, 0.1830f}; + + for (size_t i = 0; i < 4; i++) { + if (!q.isApprox(q_true, tol)) { + warnx("Quaternion method 'eulerAngles()' outside tolerance!"); + rc = 1; + } + } + + } + + { + // test quaternion method "rotate" (rotate vector by quaternion) + Eigen::Vector3f vector = {1.0f,1.0f,1.0f}; + Eigen::Vector3f vector_q; + Eigen::Vector3f vector_r; + Eigen::Quaternionf q; + Eigen::Matrix3f R; + float diff = 0.1f; + float tol = 0.00001f; + + warnx("Quaternion vector rotation method test."); + + for (float roll = -M_PI_F; roll <= M_PI_F; roll += diff) { + for (float pitch = -M_PI_2_F; pitch <= M_PI_2_F; pitch += diff) { + for (float yaw = -M_PI_F; yaw <= M_PI_F; yaw += diff) { + R.eulerAngles(roll, pitch, yaw); + q = quatFromEuler(roll,pitch,yaw); + vector_r = R*vector; + vector_q = q._transformVector(vector); + for (int i = 0; i < 3; i++) { + if(fabsf(vector_r(i) - vector_q(i)) > tol) { + warnx("Quaternion method 'rotate' outside tolerance"); + rc = 1; + } + } + } + } + } + + // test some values calculated with matlab + tol = 0.0001f; + q = quatFromEuler(M_PI_2_F,0.0f,0.0f); + vector_q = q._transformVector(vector); + Eigen::Vector3f vector_true = {1.00f,-1.00f,1.00f}; + for(size_t i = 0;i<3;i++) { + if(fabsf(vector_true(i) - vector_q(i)) > tol) { + warnx("Quaternion method 'rotate' outside tolerance"); + rc = 1; + } + } + + q = quatFromEuler(0.3f,0.2f,0.1f); + vector_q = q._transformVector(vector); + vector_true = {1.1566,0.7792,1.0273}; + for(size_t i = 0;i<3;i++) { + if(fabsf(vector_true(i) - vector_q(i)) > tol) { + warnx("Quaternion method 'rotate' outside tolerance"); + rc = 1; + } + } + + q = quatFromEuler(-1.5f,-0.2f,0.5f); + vector_q = q._transformVector(vector); + vector_true = {0.5095,1.4956,-0.7096}; + for(size_t i = 0;i<3;i++) { + if(fabsf(vector_true(i) - vector_q(i)) > tol) { + warnx("Quaternion method 'rotate' outside tolerance"); + rc = 1; + } + } + + q = quatFromEuler(M_PI_2_F,-M_PI_2_F,-M_PI_F/3.0f); + vector_q = q._transformVector(vector); + vector_true = {-1.3660,0.3660,1.0000}; + for(size_t i = 0;i<3;i++) { + if(fabsf(vector_true(i) - vector_q(i)) > tol) { + warnx("Quaternion method 'rotate' outside tolerance"); + rc = 1; + } + } + } + return rc; +}
\ No newline at end of file diff --git a/src/systemcmds/tests/test_jig_voltages.c b/src/systemcmds/tests/test_jig_voltages.c index 98a105cb3..554125302 100644 --- a/src/systemcmds/tests/test_jig_voltages.c +++ b/src/systemcmds/tests/test_jig_voltages.c @@ -162,7 +162,7 @@ int test_jig_voltages(int argc, char *argv[]) errout_with_dev: - if (fd != 0) close(fd); + if (fd != 0) { close(fd); } return ret; } diff --git a/src/systemcmds/tests/test_mathlib.cpp b/src/systemcmds/tests/test_mathlib.cpp index 682514cb7..a66cebd2f 100644 --- a/src/systemcmds/tests/test_mathlib.cpp +++ b/src/systemcmds/tests/test_mathlib.cpp @@ -282,5 +282,76 @@ int test_mathlib(int argc, char *argv[]) } + { + // test quaternion method "rotate" (rotate vector by quaternion) + Vector<3> vector = {1.0f,1.0f,1.0f}; + Vector<3> vector_q; + Vector<3> vector_r; + Quaternion q; + Matrix<3,3> R; + float diff = 0.1f; + float tol = 0.00001f; + + warnx("Quaternion vector rotation method test."); + + for (float roll = -M_PI_F; roll <= M_PI_F; roll += diff) { + for (float pitch = -M_PI_2_F; pitch <= M_PI_2_F; pitch += diff) { + for (float yaw = -M_PI_F; yaw <= M_PI_F; yaw += diff) { + R.from_euler(roll, pitch, yaw); + q.from_euler(roll,pitch,yaw); + vector_r = R*vector; + vector_q = q.rotate(vector); + for (int i = 0; i < 3; i++) { + if(fabsf(vector_r(i) - vector_q(i)) > tol) { + warnx("Quaternion method 'rotate' outside tolerance"); + rc = 1; + } + } + } + } + } + + // test some values calculated with matlab + tol = 0.0001f; + q.from_euler(M_PI_2_F,0.0f,0.0f); + vector_q = q.rotate(vector); + Vector<3> vector_true = {1.00f,-1.00f,1.00f}; + for(unsigned i = 0;i<3;i++) { + if(fabsf(vector_true(i) - vector_q(i)) > tol) { + warnx("Quaternion method 'rotate' outside tolerance"); + rc = 1; + } + } + + q.from_euler(0.3f,0.2f,0.1f); + vector_q = q.rotate(vector); + vector_true = {1.1566,0.7792,1.0273}; + for(unsigned i = 0;i<3;i++) { + if(fabsf(vector_true(i) - vector_q(i)) > tol) { + warnx("Quaternion method 'rotate' outside tolerance"); + rc = 1; + } + } + + q.from_euler(-1.5f,-0.2f,0.5f); + vector_q = q.rotate(vector); + vector_true = {0.5095,1.4956,-0.7096}; + for(unsigned i = 0;i<3;i++) { + if(fabsf(vector_true(i) - vector_q(i)) > tol) { + warnx("Quaternion method 'rotate' outside tolerance"); + rc = 1; + } + } + + q.from_euler(M_PI_2_F,-M_PI_2_F,-M_PI_F/3.0f); + vector_q = q.rotate(vector); + vector_true = {-1.3660,0.3660,1.0000}; + for(unsigned i = 0;i<3;i++) { + if(fabsf(vector_true(i) - vector_q(i)) > tol) { + warnx("Quaternion method 'rotate' outside tolerance"); + rc = 1; + } + } + } return rc; -} +}
\ No newline at end of file diff --git a/src/systemcmds/tests/test_ppm_loopback.c b/src/systemcmds/tests/test_ppm_loopback.c index a753f45c2..6866531d7 100644 --- a/src/systemcmds/tests/test_ppm_loopback.c +++ b/src/systemcmds/tests/test_ppm_loopback.c @@ -105,16 +105,15 @@ int test_ppm_loopback(int argc, char *argv[]) int pwm_values[] = {1200, 1300, 1900, 1700, 1500, 1250, 1800, 1400}; - // for (unsigned i = 0; (i < servo_count) && (i < sizeof(pwm_values) / sizeof(pwm_values[0])); i++) { - // result = ioctl(servo_fd, PWM_SERVO_SET(i), pwm_values[i]); - - // if (result) { - // (void)close(servo_fd); - // return ERROR; - // } else { - // warnx("channel %d set to %d", i, pwm_values[i]); - // } - // } + for (unsigned i = 0; (i < servo_count) && (i < sizeof(pwm_values) / sizeof(pwm_values[0])); i++) { + result = ioctl(servo_fd, PWM_SERVO_SET(i), pwm_values[i]); + if (result) { + (void)close(servo_fd); + return ERROR; + } else { + warnx("channel %d set to %d", i, pwm_values[i]); + } + } warnx("servo count: %d", servo_count); diff --git a/src/systemcmds/tests/test_rc.c b/src/systemcmds/tests/test_rc.c index c9f9ef439..3a8144077 100644 --- a/src/systemcmds/tests/test_rc.c +++ b/src/systemcmds/tests/test_rc.c @@ -126,7 +126,7 @@ int test_rc(int argc, char *argv[]) warnx("TIMEOUT, less than 10 Hz updates"); (void)close(_rc_sub); return ERROR; - } + } } else { /* key pressed, bye bye */ diff --git a/src/systemcmds/tests/test_uart_send.c b/src/systemcmds/tests/test_uart_send.c index 7e1e8d307..70a9c719e 100644 --- a/src/systemcmds/tests/test_uart_send.c +++ b/src/systemcmds/tests/test_uart_send.c @@ -95,7 +95,7 @@ int test_uart_send(int argc, char *argv[]) /* input handling */ char *uart_name = "/dev/ttyS3"; - if (argc > 1) uart_name = argv[1]; + if (argc > 1) { uart_name = argv[1]; } /* assuming NuttShell is on UART1 (/dev/ttyS0) */ int test_uart = open(uart_name, O_RDWR | O_NONBLOCK | O_NOCTTY); // diff --git a/src/systemcmds/tests/tests.h b/src/systemcmds/tests/tests.h index 69fb0e57b..9d13d50d7 100644 --- a/src/systemcmds/tests/tests.h +++ b/src/systemcmds/tests/tests.h @@ -113,6 +113,7 @@ extern int test_rc(int argc, char *argv[]); extern int test_conv(int argc, char *argv[]); extern int test_mount(int argc, char *argv[]); extern int test_mathlib(int argc, char *argv[]); +extern int test_eigen(int argc, char *argv[]); __END_DECLS diff --git a/src/systemcmds/tests/tests_main.c b/src/systemcmds/tests/tests_main.c index 0768c1d5a..7ffd47892 100644 --- a/src/systemcmds/tests/tests_main.c +++ b/src/systemcmds/tests/tests_main.c @@ -112,6 +112,7 @@ const struct { #ifndef TESTS_MATHLIB_DISABLE {"mathlib", test_mathlib, 0}, #endif + {"eigen", test_eigen, OPT_NOALLTEST}, {"help", test_help, OPT_NOALLTEST | OPT_NOHELP | OPT_NOJIGTEST}, {NULL, NULL, 0} }; diff --git a/src/systemcmds/usb_connected/usb_connected.c b/src/systemcmds/usb_connected/usb_connected.c index 98bbbc4be..530fee340 100644 --- a/src/systemcmds/usb_connected/usb_connected.c +++ b/src/systemcmds/usb_connected/usb_connected.c @@ -52,5 +52,5 @@ __EXPORT int usb_connected_main(int argc, char *argv[]); int usb_connected_main(int argc, char *argv[]) { - return stm32_gpioread(GPIO_OTGFS_VBUS) ? 0:1; + return stm32_gpioread(GPIO_OTGFS_VBUS) ? 0 : 1; } diff --git a/unittests/CMakeLists.txt b/unittests/CMakeLists.txt index 1a203fa30..84078c3e5 100644 --- a/unittests/CMakeLists.txt +++ b/unittests/CMakeLists.txt @@ -75,6 +75,10 @@ add_gtest(sbus2_test) add_executable(st24_test st24_test.cpp hrt.cpp ${PX_SRC}/lib/rc/st24.c) add_gtest(st24_test) +# sumd_test +add_executable(sumd_test sumd_test.cpp hrt.cpp ${PX_SRC}/lib/rc/sumd.c) +add_gtest(sumd_test) + # sf0x_test add_executable(sf0x_test sf0x_test.cpp ${PX_SRC}/drivers/sf0x/sf0x_parser.cpp) add_gtest(sf0x_test) diff --git a/unittests/autodeclination_test.cpp b/unittests/autodeclination_test.cpp index a27d5f100..8c89243f6 100644 --- a/unittests/autodeclination_test.cpp +++ b/unittests/autodeclination_test.cpp @@ -1,6 +1,6 @@ #include <math.h> #include <stdio.h> -#include <stdlib.h> +#include <stdlib.h> #include <string.h> #include <unistd.h> @@ -12,6 +12,7 @@ #include "gtest/gtest.h" -TEST(AutoDeclinationTest, AutoDeclination) { +TEST(AutoDeclinationTest, AutoDeclination) +{ ASSERT_NEAR(get_mag_declination(47.0, 8.0), 0.6, 0.5) << "declination differs more than 1 degree"; } diff --git a/unittests/conversion_test.cpp b/unittests/conversion_test.cpp index 12d2213e2..7a2909dc3 100644 --- a/unittests/conversion_test.cpp +++ b/unittests/conversion_test.cpp @@ -4,6 +4,7 @@ #include "gtest/gtest.h" -TEST(ConversionTest, quad_w_main) { - ASSERT_EQ(test_conv(0, NULL), 0) << "Conversion test failed"; +TEST(ConversionTest, quad_w_main) +{ + ASSERT_EQ(test_conv(0, NULL), 0) << "Conversion test failed"; } diff --git a/unittests/hrt.cpp b/unittests/hrt.cpp index 01b5958b7..d7b4670db 100644 --- a/unittests/hrt.cpp +++ b/unittests/hrt.cpp @@ -3,14 +3,16 @@ #include <drivers/drv_hrt.h> #include <stdio.h> -hrt_abstime hrt_absolute_time() { - struct timeval te; - gettimeofday(&te, NULL); // get current time - hrt_abstime us = static_cast<uint64_t>(te.tv_sec) * 1e6 + te.tv_usec; // caculate us - return us; +hrt_abstime hrt_absolute_time() +{ + struct timeval te; + gettimeofday(&te, NULL); // get current time + hrt_abstime us = static_cast<uint64_t>(te.tv_sec) * 1e6 + te.tv_usec; // caculate us + return us; } -hrt_abstime hrt_elapsed_time(const volatile hrt_abstime *then) { +hrt_abstime hrt_elapsed_time(const volatile hrt_abstime *then) +{ // not thread safe - return hrt_absolute_time() - *then; + return hrt_absolute_time() - *then; } diff --git a/unittests/mixer_test.cpp b/unittests/mixer_test.cpp index 1271dab5e..5be4cb032 100644 --- a/unittests/mixer_test.cpp +++ b/unittests/mixer_test.cpp @@ -5,7 +5,8 @@ #include "gtest/gtest.h" -TEST(MixerTest, Mixer) { - char* args[] = {"empty", "../ROMFS/px4fmu_common/mixers/IO_pass.mix", "../ROMFS/px4fmu_common/mixers/quad_w.main.mix"}; +TEST(MixerTest, Mixer) +{ + char *args[] = {"empty", "../ROMFS/px4fmu_common/mixers/IO_pass.mix", "../ROMFS/px4fmu_common/mixers/quad_w.main.mix"}; ASSERT_EQ(test_mixer(3, args), 0) << "IO_pass.mix failed"; } diff --git a/unittests/param_test.cpp b/unittests/param_test.cpp index 5ea6bcc60..44ae4df06 100644 --- a/unittests/param_test.cpp +++ b/unittests/param_test.cpp @@ -13,7 +13,8 @@ struct param_info_s *param_info_limit; /* * Adds test parameters */ -void _add_parameters() { +void _add_parameters() +{ struct param_info_s test_1 = { "TEST_1", PARAM_TYPE_INT32 @@ -44,17 +45,19 @@ void _add_parameters() { param_array[3] = rc2_x; param_info_base = (struct param_info_s *) ¶m_array[0]; param_info_limit = (struct param_info_s *) ¶m_array[4]; // needs to point at the end of the data, - // therefore number of params + 1 + // therefore number of params + 1 } -void _assert_parameter_int_value(param_t param, int32_t expected) { +void _assert_parameter_int_value(param_t param, int32_t expected) +{ int32_t value; int result = param_get(param, &value); ASSERT_EQ(0, result) << printf("param_get (%i) did not return parameter\n", param); ASSERT_EQ(expected, value) << printf("value for param (%i) doesn't match default value\n", param); } -void _set_all_int_parameters_to(int32_t value) { +void _set_all_int_parameters_to(int32_t value) +{ param_set((param_t)0, &value); param_set((param_t)1, &value); param_set((param_t)2, &value); @@ -66,9 +69,10 @@ void _set_all_int_parameters_to(int32_t value) { _assert_parameter_int_value((param_t)3, value); } -TEST(ParamTest, SimpleFind) { +TEST(ParamTest, SimpleFind) +{ _add_parameters(); - + param_t param = param_find("TEST_2"); ASSERT_NE(PARAM_INVALID, param) << "param_find did not find parameter"; @@ -78,7 +82,8 @@ TEST(ParamTest, SimpleFind) { ASSERT_EQ(4, value) << "value of returned parameter does not match"; } -TEST(ParamTest, ResetAll) { +TEST(ParamTest, ResetAll) +{ _add_parameters(); _set_all_int_parameters_to(50); @@ -90,11 +95,12 @@ TEST(ParamTest, ResetAll) { _assert_parameter_int_value((param_t)3, 16); } -TEST(ParamTest, ResetAllExcludesOne) { +TEST(ParamTest, ResetAllExcludesOne) +{ _add_parameters(); _set_all_int_parameters_to(50); - const char* excludes[] = {"RC_X"}; + const char *excludes[] = {"RC_X"}; param_reset_excludes(excludes, 1); _assert_parameter_int_value((param_t)0, 2); @@ -103,11 +109,12 @@ TEST(ParamTest, ResetAllExcludesOne) { _assert_parameter_int_value((param_t)3, 16); } -TEST(ParamTest, ResetAllExcludesTwo) { +TEST(ParamTest, ResetAllExcludesTwo) +{ _add_parameters(); _set_all_int_parameters_to(50); - const char* excludes[] = {"RC_X", "TEST_1"}; + const char *excludes[] = {"RC_X", "TEST_1"}; param_reset_excludes(excludes, 2); _assert_parameter_int_value((param_t)0, 50); @@ -116,11 +123,12 @@ TEST(ParamTest, ResetAllExcludesTwo) { _assert_parameter_int_value((param_t)3, 16); } -TEST(ParamTest, ResetAllExcludesBoundaryCheck) { +TEST(ParamTest, ResetAllExcludesBoundaryCheck) +{ _add_parameters(); _set_all_int_parameters_to(50); - const char* excludes[] = {"RC_X", "TEST_1"}; + const char *excludes[] = {"RC_X", "TEST_1"}; param_reset_excludes(excludes, 1); _assert_parameter_int_value((param_t)0, 2); @@ -129,11 +137,12 @@ TEST(ParamTest, ResetAllExcludesBoundaryCheck) { _assert_parameter_int_value((param_t)3, 16); } -TEST(ParamTest, ResetAllExcludesWildcard) { +TEST(ParamTest, ResetAllExcludesWildcard) +{ _add_parameters(); _set_all_int_parameters_to(50); - const char* excludes[] = {"RC*"}; + const char *excludes[] = {"RC*"}; param_reset_excludes(excludes, 1); _assert_parameter_int_value((param_t)0, 2); diff --git a/unittests/queue.h b/unittests/queue.h index 0fdb170db..e056bc263 100644 --- a/unittests/queue.h +++ b/unittests/queue.h @@ -67,30 +67,26 @@ * Global Type Declarations ************************************************************************/ -struct sq_entry_s -{ - FAR struct sq_entry_s *flink; +struct sq_entry_s { + FAR struct sq_entry_s *flink; }; typedef struct sq_entry_s sq_entry_t; -struct dq_entry_s -{ - FAR struct dq_entry_s *flink; - FAR struct dq_entry_s *blink; +struct dq_entry_s { + FAR struct dq_entry_s *flink; + FAR struct dq_entry_s *blink; }; typedef struct dq_entry_s dq_entry_t; -struct sq_queue_s -{ - FAR sq_entry_t *head; - FAR sq_entry_t *tail; +struct sq_queue_s { + FAR sq_entry_t *head; + FAR sq_entry_t *tail; }; typedef struct sq_queue_s sq_queue_t; -struct dq_queue_s -{ - FAR dq_entry_t *head; - FAR dq_entry_t *tail; +struct dq_queue_s { + FAR dq_entry_t *head; + FAR dq_entry_t *tail; }; typedef struct dq_queue_s dq_queue_t; @@ -110,11 +106,11 @@ EXTERN void dq_addfirst(FAR dq_entry_t *node, dq_queue_t *queue); EXTERN void sq_addlast(FAR sq_entry_t *node, sq_queue_t *queue); EXTERN void dq_addlast(FAR dq_entry_t *node, dq_queue_t *queue); EXTERN void sq_addafter(FAR sq_entry_t *prev, FAR sq_entry_t *node, - sq_queue_t *queue); + sq_queue_t *queue); EXTERN void dq_addafter(FAR dq_entry_t *prev, FAR dq_entry_t *node, - dq_queue_t *queue); + dq_queue_t *queue); EXTERN void dq_addbefore(FAR dq_entry_t *next, FAR dq_entry_t *node, - dq_queue_t *queue); + dq_queue_t *queue); EXTERN FAR sq_entry_t *sq_remafter(FAR sq_entry_t *node, sq_queue_t *queue); EXTERN void sq_rem(FAR sq_entry_t *node, sq_queue_t *queue); diff --git a/unittests/sbus2_test.cpp b/unittests/sbus2_test.cpp index 15a21c86b..41f49627d 100644 --- a/unittests/sbus2_test.cpp +++ b/unittests/sbus2_test.cpp @@ -10,11 +10,12 @@ #include "gtest/gtest.h" -TEST(SBUS2Test, SBUS2) { +TEST(SBUS2Test, SBUS2) +{ const char *filepath = "testdata/sbus2_r7008SB.txt"; FILE *fp; - fp = fopen(filepath,"rt"); + fp = fopen(filepath, "rt"); ASSERT_TRUE(fp); warnx("loading data from: %s", filepath); @@ -51,8 +52,9 @@ TEST(SBUS2Test, SBUS2) { //warnx("%f: 0x%02x, first: 0x%02x, last: 0x%02x, pcount: %u", (double)f, x, frame[0], frame[24], partial_frame_count); - if (partial_frame_count == sizeof(frame)) + if (partial_frame_count == sizeof(frame)) { partial_frame_count = 0; + } last_time = f; @@ -60,7 +62,7 @@ TEST(SBUS2Test, SBUS2) { hrt_abstime now = hrt_absolute_time(); //if (partial_frame_count % 25 == 0) - //sbus_parse(now, frame, &partial_frame_count, rc_values, &num_values, &sbus_failsafe, &sbus_frame_drop, max_channels); + //sbus_parse(now, frame, &partial_frame_count, rc_values, &num_values, &sbus_failsafe, &sbus_frame_drop, max_channels); } ASSERT_EQ(ret, EOF); diff --git a/unittests/sf0x_test.cpp b/unittests/sf0x_test.cpp index b161f1ffd..56e673a06 100644 --- a/unittests/sf0x_test.cpp +++ b/unittests/sf0x_test.cpp @@ -9,7 +9,8 @@ #include "gtest/gtest.h" -TEST(SF0XTest, SF0X) { +TEST(SF0XTest, SF0X) +{ const char _LINE_MAX = 20; char _linebuf[_LINE_MAX]; _linebuf[0] = '\0'; diff --git a/unittests/st24_test.cpp b/unittests/st24_test.cpp index 89c7ffb1c..42fa07ae7 100644 --- a/unittests/st24_test.cpp +++ b/unittests/st24_test.cpp @@ -8,8 +8,9 @@ #include "gtest/gtest.h" -TEST(ST24Test, ST24) { - const char* filepath = "testdata/st24_data.txt"; +TEST(ST24Test, ST24) +{ + const char *filepath = "testdata/st24_data.txt"; warnx("loading data from: %s", filepath); diff --git a/unittests/sumd_test.cpp b/unittests/sumd_test.cpp new file mode 100644 index 000000000..a919de31b --- /dev/null +++ b/unittests/sumd_test.cpp @@ -0,0 +1,63 @@ +#include <stdio.h> +#include <unistd.h> +#include <string.h> +#include <systemlib/err.h> +#include <drivers/drv_hrt.h> +#include <rc/sumd.h> +#include "../../src/systemcmds/tests/tests.h" + +#include "gtest/gtest.h" + +TEST(SUMDTest, SUMD) { + const char* filepath = "testdata/sumd_data.txt"; + + warnx("loading data from: %s", filepath); + + FILE *fp; + + fp = fopen(filepath, "rt"); + //ASSERT_TRUE(fp); + + float f; + unsigned x; + int ret; + + // Trash the first 20 lines + for (unsigned i = 0; i < 20; i++) { + char buf[200]; + (void)fgets(buf, sizeof(buf), fp); + } + + float last_time = 0; + + while (EOF != (ret = fscanf(fp, "%f,%x,,", &f, &x))) { + if (((f - last_time) * 1000 * 1000) > 3000) { + // warnx("FRAME RESET\n\n"); + } + + uint8_t b = static_cast<uint8_t>(x); + + last_time = f; + + // Pipe the data into the parser + hrt_abstime now = hrt_absolute_time(); + + uint8_t rssi; + uint8_t rx_count; + uint16_t channel_count; + uint16_t channels[32]; + + + if (!sumd_decode(b, &rssi, &rx_count, &channel_count, channels, 32)) { + //warnx("decoded: %u channels (converted to PPM range)", (unsigned)channel_count); + + for (unsigned i = 0; i < channel_count; i++) { + + int16_t val = channels[i]; + //warnx("channel %u: %d 0x%03X", i, static_cast<int>(val), static_cast<int>(val)); + } + } + } + + ASSERT_EQ(EOF, ret); +} diff --git a/unittests/testdata/sumd_data.txt b/unittests/testdata/sumd_data.txt new file mode 100644 index 000000000..ac7fced5d --- /dev/null +++ b/unittests/testdata/sumd_data.txt @@ -0,0 +1,14472 @@ +Time [s],Value,Parity Error,Framing Error +0.001457,0xA8,, +0.001544,0x01,, +0.001631,0x0C,, +0.001718,0x22,, +0.001805,0x74,, +0.001892,0x2E,, +0.001979,0xD8,, +0.002066,0x2E,, +0.002153,0xE0,, +0.002239,0x2E,, +0.002326,0xD8,, +0.002413,0x22,, +0.0025,0x60,, +0.002587,0x22,, +0.002674,0x60,, +0.002761,0x3B,, +0.002848,0x60,, +0.002935,0x22,, +0.003022,0x60,, +0.003109,0x22,, +0.003196,0x60,, +0.003283,0x3B,, +0.00337,0x60,, +0.003457,0x2E,, +0.003544,0xE0,, +0.003631,0x2E,, +0.003718,0xE0,, +0.003804,0xF9,, +0.003891,0x9B,, +0.011504,0xA8,, +0.011591,0x01,, +0.011678,0x0C,, +0.011765,0x22,, +0.011852,0x74,, +0.011939,0x2E,, +0.012026,0xD8,, +0.012113,0x2E,, +0.0122,0xE0,, +0.012287,0x2E,, +0.012374,0xD8,, +0.012461,0x22,, +0.012548,0x60,, +0.012635,0x22,, +0.012722,0x60,, +0.012809,0x3B,, +0.012895,0x60,, +0.012982,0x22,, +0.013069,0x60,, +0.013156,0x22,, +0.013243,0x60,, +0.01333,0x3B,, +0.013417,0x60,, +0.013504,0x2E,, +0.013591,0xE0,, +0.013678,0x2E,, +0.013765,0xE0,, +0.013852,0xF9,, +0.013939,0x9B,, +0.021551,0xA8,, +0.021638,0x01,, +0.021725,0x0C,, +0.021812,0x22,, +0.021899,0x74,, +0.021986,0x2E,, +0.022073,0xD8,, +0.02216,0x2E,, +0.022247,0xE4,, +0.022334,0x2E,, +0.022421,0xD8,, +0.022508,0x22,, +0.022595,0x60,, +0.022682,0x22,, +0.022769,0x60,, +0.022856,0x3B,, +0.022943,0x60,, +0.023029,0x22,, +0.023116,0x60,, +0.023203,0x22,, +0.02329,0x60,, +0.023377,0x3B,, +0.023464,0x60,, +0.023551,0x2E,, +0.023638,0xE0,, +0.023725,0x2E,, +0.023812,0xE0,, +0.023899,0xB1,, +0.023986,0x84,, +0.031599,0xA8,, +0.031686,0x01,, +0.031773,0x0C,, +0.03186,0x22,, +0.031947,0x74,, +0.032034,0x2E,, +0.032121,0xD8,, +0.032208,0x2E,, +0.032294,0xE0,, +0.032381,0x2E,, +0.032468,0xD8,, +0.032555,0x22,, +0.032642,0x60,, +0.032729,0x22,, +0.032816,0x60,, +0.032903,0x3B,, +0.03299,0x60,, +0.033077,0x22,, +0.033164,0x60,, +0.033251,0x22,, +0.033338,0x60,, +0.033425,0x3B,, +0.033512,0x60,, +0.033599,0x2E,, +0.033686,0xE0,, +0.033772,0x2E,, +0.033859,0xE0,, +0.033946,0xF9,, +0.034033,0x9B,, +0.041647,0xA8,, +0.041734,0x01,, +0.041821,0x0C,, +0.041907,0x22,, +0.041994,0x74,, +0.042081,0x2E,, +0.042168,0xD8,, +0.042255,0x2E,, +0.042342,0xE0,, +0.042429,0x2E,, +0.042516,0xD8,, +0.042603,0x22,, +0.04269,0x60,, +0.042777,0x22,, +0.042864,0x60,, +0.042951,0x3B,, +0.043038,0x60,, +0.043125,0x22,, +0.043212,0x60,, +0.043299,0x22,, +0.043386,0x60,, +0.043472,0x3B,, +0.043559,0x60,, +0.043646,0x2E,, +0.043733,0xE0,, +0.04382,0x2E,, +0.043907,0xE0,, +0.043994,0xF9,, +0.044081,0x9B,, +0.051694,0xA8,, +0.051781,0x01,, +0.051868,0x0C,, +0.051955,0x22,, +0.052042,0x74,, +0.052129,0x2E,, +0.052216,0xD8,, +0.052303,0x2E,, +0.05239,0xE0,, +0.052477,0x2E,, +0.052564,0xD8,, +0.052651,0x22,, +0.052737,0x60,, +0.052824,0x22,, +0.052911,0x60,, +0.052998,0x3B,, +0.053085,0x60,, +0.053172,0x22,, +0.053259,0x60,, +0.053346,0x22,, +0.053433,0x60,, +0.05352,0x3B,, +0.053607,0x60,, +0.053694,0x2E,, +0.053781,0xE0,, +0.053868,0x2E,, +0.053955,0xE0,, +0.054042,0xF9,, +0.054129,0x9B,, +0.061742,0xA8,, +0.061828,0x01,, +0.061915,0x0C,, +0.062002,0x22,, +0.062089,0x74,, +0.062176,0x2E,, +0.062263,0xD8,, +0.06235,0x2E,, +0.062437,0xE0,, +0.062524,0x2E,, +0.062611,0xD8,, +0.062698,0x22,, +0.062785,0x60,, +0.062872,0x22,, +0.062959,0x60,, +0.063046,0x3B,, +0.063133,0x60,, +0.06322,0x22,, +0.063306,0x60,, +0.063393,0x22,, +0.06348,0x60,, +0.063567,0x3B,, +0.063654,0x60,, +0.063741,0x2E,, +0.063828,0xE0,, +0.063915,0x2E,, +0.064002,0xE0,, +0.064089,0xF9,, +0.064176,0x9B,, +0.071789,0xA8,, +0.071876,0x01,, +0.071963,0x0C,, +0.07205,0x22,, +0.072137,0x74,, +0.072224,0x2E,, +0.072311,0xD8,, +0.072398,0x2E,, +0.072485,0xE4,, +0.072571,0x2E,, +0.072658,0xD8,, +0.072745,0x22,, +0.072832,0x60,, +0.072919,0x22,, +0.073006,0x60,, +0.073093,0x3B,, +0.07318,0x60,, +0.073267,0x22,, +0.073354,0x60,, +0.073441,0x22,, +0.073528,0x60,, +0.073615,0x3B,, +0.073702,0x60,, +0.073789,0x2E,, +0.073876,0xE0,, +0.073963,0x2E,, +0.07405,0xE0,, +0.074136,0xB1,, +0.074223,0x84,, +0.081836,0xA8,, +0.081923,0x01,, +0.08201,0x0C,, +0.082097,0x22,, +0.082184,0x74,, +0.082271,0x2E,, +0.082358,0xD8,, +0.082445,0x2E,, +0.082532,0xE4,, +0.082619,0x2E,, +0.082706,0xD8,, +0.082793,0x22,, +0.08288,0x60,, +0.082967,0x22,, +0.083054,0x60,, +0.08314,0x3B,, +0.083227,0x60,, +0.083314,0x22,, +0.083401,0x60,, +0.083488,0x22,, +0.083575,0x60,, +0.083662,0x3B,, +0.083749,0x60,, +0.083836,0x2E,, +0.083923,0xE0,, +0.08401,0x2E,, +0.084097,0xE0,, +0.084184,0xB1,, +0.084271,0x84,, +0.091884,0xA8,, +0.091971,0x01,, +0.092057,0x0C,, +0.092144,0x22,, +0.092231,0x74,, +0.092318,0x2E,, +0.092405,0xD8,, +0.092492,0x2E,, +0.092579,0xE0,, +0.092666,0x2E,, +0.092753,0xD8,, +0.09284,0x22,, +0.092927,0x60,, +0.093014,0x22,, +0.093101,0x60,, +0.093188,0x3B,, +0.093275,0x60,, +0.093362,0x22,, +0.093449,0x60,, +0.093535,0x22,, +0.093622,0x60,, +0.093709,0x3B,, +0.093796,0x60,, +0.093883,0x2E,, +0.09397,0xE0,, +0.094057,0x2E,, +0.094144,0xE0,, +0.094231,0xF9,, +0.094318,0x9B,, +0.101931,0xA8,, +0.102018,0x01,, +0.102105,0x0C,, +0.102192,0x22,, +0.102279,0x74,, +0.102366,0x2E,, +0.102453,0xD8,, +0.10254,0x2E,, +0.102627,0xE0,, +0.102714,0x2E,, +0.102801,0xD8,, +0.102888,0x22,, +0.102975,0x60,, +0.103062,0x22,, +0.103149,0x60,, +0.103235,0x3B,, +0.103322,0x60,, +0.103409,0x22,, +0.103496,0x60,, +0.103583,0x22,, +0.10367,0x60,, +0.103757,0x3B,, +0.103844,0x60,, +0.103931,0x2E,, +0.104018,0xE0,, +0.104105,0x2E,, +0.104192,0xE0,, +0.104279,0xF9,, +0.104366,0x9B,, +0.111978,0xA8,, +0.112065,0x01,, +0.112152,0x0C,, +0.112239,0x22,, +0.112326,0x74,, +0.112413,0x2E,, +0.1125,0xD8,, +0.112587,0x2E,, +0.112674,0xE0,, +0.112761,0x2E,, +0.112848,0xD8,, +0.112935,0x22,, +0.113022,0x60,, +0.113109,0x22,, +0.113196,0x60,, +0.113283,0x3B,, +0.113369,0x60,, +0.113456,0x22,, +0.113543,0x60,, +0.11363,0x22,, +0.113717,0x60,, +0.113804,0x3B,, +0.113891,0x60,, +0.113978,0x2E,, +0.114065,0xE0,, +0.114152,0x2E,, +0.114239,0xE0,, +0.114326,0xF9,, +0.114413,0x9B,, +0.122026,0xA8,, +0.122112,0x01,, +0.122199,0x0C,, +0.122286,0x22,, +0.122373,0x74,, +0.12246,0x2E,, +0.122547,0xD8,, +0.122634,0x2E,, +0.122721,0xE0,, +0.122808,0x2E,, +0.122895,0xD8,, +0.122982,0x22,, +0.123069,0x60,, +0.123156,0x22,, +0.123243,0x60,, +0.12333,0x3B,, +0.123417,0x60,, +0.123504,0x22,, +0.12359,0x60,, +0.123677,0x22,, +0.123764,0x60,, +0.123851,0x3B,, +0.123938,0x60,, +0.124025,0x2E,, +0.124112,0xE0,, +0.124199,0x2E,, +0.124286,0xE0,, +0.124373,0xF9,, +0.12446,0x9B,, +0.132073,0xA8,, +0.13216,0x01,, +0.132247,0x0C,, +0.132334,0x22,, +0.132421,0x74,, +0.132508,0x2E,, +0.132595,0xD8,, +0.132682,0x2E,, +0.132769,0xE0,, +0.132856,0x2E,, +0.132942,0xD8,, +0.133029,0x22,, +0.133116,0x60,, +0.133203,0x22,, +0.13329,0x60,, +0.133377,0x3B,, +0.133464,0x60,, +0.133551,0x22,, +0.133638,0x60,, +0.133725,0x22,, +0.133812,0x60,, +0.133899,0x3B,, +0.133986,0x60,, +0.134073,0x2E,, +0.13416,0xE0,, +0.134247,0x2E,, +0.134334,0xE0,, +0.13442,0xF9,, +0.134507,0x9B,, +0.14212,0xA8,, +0.142207,0x01,, +0.142294,0x0C,, +0.142381,0x22,, +0.142468,0x74,, +0.142555,0x2E,, +0.142642,0xD8,, +0.142729,0x2E,, +0.142816,0xE4,, +0.142903,0x2E,, +0.14299,0xD8,, +0.143077,0x22,, +0.143164,0x60,, +0.143251,0x22,, +0.143338,0x60,, +0.143424,0x3B,, +0.143511,0x60,, +0.143598,0x22,, +0.143685,0x60,, +0.143772,0x22,, +0.143859,0x60,, +0.143946,0x3B,, +0.144033,0x60,, +0.14412,0x2E,, +0.144207,0xE0,, +0.144294,0x2E,, +0.144381,0xE0,, +0.144468,0xB1,, +0.144555,0x84,, +0.152167,0xA8,, +0.152254,0x01,, +0.152341,0x0C,, +0.152428,0x22,, +0.152515,0x74,, +0.152602,0x2E,, +0.152689,0xD8,, +0.152776,0x2E,, +0.152863,0xE0,, +0.15295,0x2E,, +0.153037,0xD8,, +0.153124,0x22,, +0.153211,0x60,, +0.153298,0x22,, +0.153385,0x60,, +0.153472,0x3B,, +0.153559,0x60,, +0.153646,0x22,, +0.153732,0x60,, +0.153819,0x22,, +0.153906,0x60,, +0.153993,0x3B,, +0.15408,0x60,, +0.154167,0x2E,, +0.154254,0xE0,, +0.154341,0x2E,, +0.154428,0xE0,, +0.154515,0xF9,, +0.154602,0x9B,, +0.162215,0xA8,, +0.162302,0x01,, +0.162389,0x0C,, +0.162476,0x22,, +0.162563,0x74,, +0.16265,0x2E,, +0.162737,0xD8,, +0.162823,0x2E,, +0.16291,0xE0,, +0.162997,0x2E,, +0.163084,0xD8,, +0.163171,0x22,, +0.163258,0x60,, +0.163345,0x22,, +0.163432,0x60,, +0.163519,0x3B,, +0.163606,0x60,, +0.163693,0x22,, +0.16378,0x60,, +0.163867,0x22,, +0.163954,0x60,, +0.164041,0x3B,, +0.164128,0x60,, +0.164215,0x2E,, +0.164301,0xE0,, +0.164388,0x2E,, +0.164475,0xE0,, +0.164562,0xF9,, +0.164649,0x9B,, +0.172263,0xA8,, +0.17235,0x01,, +0.172436,0x0C,, +0.172523,0x22,, +0.17261,0x74,, +0.172697,0x2E,, +0.172784,0xD8,, +0.172871,0x2E,, +0.172958,0xE0,, +0.173045,0x2E,, +0.173132,0xD8,, +0.173219,0x22,, +0.173306,0x60,, +0.173393,0x22,, +0.17348,0x60,, +0.173567,0x3B,, +0.173654,0x60,, +0.173741,0x22,, +0.173828,0x60,, +0.173914,0x22,, +0.174001,0x60,, +0.174088,0x3B,, +0.174175,0x60,, +0.174262,0x2E,, +0.174349,0xE0,, +0.174436,0x2E,, +0.174523,0xE0,, +0.17461,0xF9,, +0.174697,0x9B,, +0.18231,0xA8,, +0.182397,0x01,, +0.182484,0x0C,, +0.182571,0x22,, +0.182658,0x74,, +0.182745,0x2E,, +0.182832,0xD8,, +0.182919,0x2E,, +0.183005,0xE4,, +0.183092,0x2E,, +0.183179,0xD8,, +0.183266,0x22,, +0.183353,0x60,, +0.18344,0x22,, +0.183527,0x60,, +0.183614,0x3B,, +0.183701,0x60,, +0.183788,0x22,, +0.183875,0x60,, +0.183962,0x22,, +0.184049,0x60,, +0.184136,0x3B,, +0.184223,0x60,, +0.18431,0x2E,, +0.184397,0xE0,, +0.184484,0x2E,, +0.18457,0xE0,, +0.184657,0xB1,, +0.184744,0x84,, +0.192357,0xA8,, +0.192444,0x01,, +0.192531,0x0C,, +0.192618,0x22,, +0.192705,0x74,, +0.192792,0x2E,, +0.192879,0xD8,, +0.192966,0x2E,, +0.193053,0xE0,, +0.19314,0x2E,, +0.193227,0xD8,, +0.193314,0x22,, +0.1934,0x60,, +0.193487,0x22,, +0.193574,0x60,, +0.193661,0x3B,, +0.193748,0x60,, +0.193835,0x22,, +0.193922,0x60,, +0.194009,0x22,, +0.194096,0x60,, +0.194183,0x3B,, +0.19427,0x60,, +0.194357,0x2E,, +0.194444,0xE0,, +0.194531,0x2E,, +0.194618,0xE0,, +0.194705,0xF9,, +0.194792,0x9B,, +0.202404,0xA8,, +0.202491,0x01,, +0.202578,0x0C,, +0.202665,0x22,, +0.202752,0x74,, +0.202839,0x2E,, +0.202926,0xD8,, +0.203013,0x2E,, +0.2031,0xE4,, +0.203187,0x2E,, +0.203274,0xD8,, +0.203361,0x22,, +0.203448,0x60,, +0.203535,0x22,, +0.203622,0x60,, +0.203709,0x3B,, +0.203796,0x60,, +0.203882,0x22,, +0.203969,0x60,, +0.204056,0x22,, +0.204143,0x60,, +0.20423,0x3B,, +0.204317,0x60,, +0.204404,0x2E,, +0.204491,0xE0,, +0.204578,0x2E,, +0.204665,0xE0,, +0.204752,0xB1,, +0.204839,0x84,, +0.212452,0xA8,, +0.212539,0x01,, +0.212626,0x0C,, +0.212713,0x22,, +0.2128,0x74,, +0.212887,0x2E,, +0.212974,0xD8,, +0.213061,0x2E,, +0.213148,0xE0,, +0.213235,0x2E,, +0.213321,0xD8,, +0.213408,0x22,, +0.213495,0x60,, +0.213582,0x22,, +0.213669,0x60,, +0.213756,0x3B,, +0.213843,0x60,, +0.21393,0x22,, +0.214017,0x60,, +0.214104,0x22,, +0.214191,0x60,, +0.214278,0x3B,, +0.214365,0x60,, +0.214452,0x2E,, +0.214539,0xE0,, +0.214626,0x2E,, +0.214713,0xE0,, +0.214799,0xF9,, +0.214886,0x9B,, +0.2225,0xA8,, +0.222586,0x01,, +0.222673,0x0C,, +0.22276,0x22,, +0.222847,0x74,, +0.222934,0x2E,, +0.223021,0xD8,, +0.223108,0x2E,, +0.223195,0xE4,, +0.223282,0x2E,, +0.223369,0xD8,, +0.223456,0x22,, +0.223543,0x60,, +0.22363,0x22,, +0.223717,0x60,, +0.223804,0x3B,, +0.223891,0x60,, +0.223978,0x22,, +0.224064,0x60,, +0.224151,0x22,, +0.224238,0x60,, +0.224325,0x3B,, +0.224412,0x60,, +0.224499,0x2E,, +0.224586,0xE0,, +0.224673,0x2E,, +0.22476,0xE0,, +0.224847,0xB1,, +0.224934,0x84,, +0.232547,0xA8,, +0.232634,0x01,, +0.232721,0x0C,, +0.232808,0x22,, +0.232895,0x74,, +0.232982,0x2E,, +0.233069,0xD8,, +0.233156,0x2E,, +0.233243,0xE4,, +0.233329,0x2E,, +0.233416,0xD8,, +0.233503,0x22,, +0.23359,0x60,, +0.233677,0x22,, +0.233764,0x60,, +0.233851,0x3B,, +0.233938,0x60,, +0.234025,0x22,, +0.234112,0x60,, +0.234199,0x22,, +0.234286,0x60,, +0.234373,0x3B,, +0.23446,0x60,, +0.234547,0x2E,, +0.234634,0xE0,, +0.234721,0x2E,, +0.234808,0xE0,, +0.234894,0xB1,, +0.234981,0x84,, +0.24261,0xA8,, +0.242697,0x01,, +0.242784,0x0C,, +0.242871,0x22,, +0.242958,0x74,, +0.243045,0x2E,, +0.243132,0xE0,, +0.243219,0x2E,, +0.243306,0xE0,, +0.243393,0x2E,, +0.24348,0xD8,, +0.243567,0x22,, +0.243653,0x60,, +0.24374,0x22,, +0.243827,0x60,, +0.243914,0x3B,, +0.244001,0x60,, +0.244088,0x22,, +0.244175,0x60,, +0.244262,0x22,, +0.244349,0x60,, +0.244436,0x3B,, +0.244523,0x60,, +0.24461,0x2E,, +0.244697,0xE0,, +0.244784,0x2E,, +0.244871,0xE0,, +0.244958,0xB5,, +0.245045,0x9B,, +0.252662,0xA8,, +0.252749,0x01,, +0.252836,0x0C,, +0.252923,0x22,, +0.25301,0x74,, +0.253097,0x2E,, +0.253184,0xD8,, +0.253271,0x2E,, +0.253358,0xE0,, +0.253444,0x2E,, +0.253531,0xD8,, +0.253618,0x22,, +0.253705,0x60,, +0.253792,0x22,, +0.253879,0x60,, +0.253966,0x3B,, +0.254053,0x60,, +0.25414,0x22,, +0.254227,0x60,, +0.254314,0x22,, +0.254401,0x60,, +0.254488,0x3B,, +0.254575,0x60,, +0.254662,0x2E,, +0.254749,0xE0,, +0.254836,0x2E,, +0.254922,0xE0,, +0.255009,0xF9,, +0.255096,0x9B,, +0.262713,0xA8,, +0.2628,0x01,, +0.262887,0x0C,, +0.262974,0x22,, +0.263061,0x74,, +0.263147,0x2E,, +0.263234,0xD8,, +0.263321,0x2E,, +0.263408,0xE4,, +0.263495,0x2E,, +0.263582,0xD8,, +0.263669,0x22,, +0.263756,0x60,, +0.263843,0x22,, +0.26393,0x60,, +0.264017,0x3B,, +0.264104,0x60,, +0.264191,0x22,, +0.264278,0x60,, +0.264365,0x22,, +0.264452,0x60,, +0.264539,0x3B,, +0.264625,0x60,, +0.264712,0x2E,, +0.264799,0xE0,, +0.264886,0x2E,, +0.264973,0xE0,, +0.26506,0xB1,, +0.265147,0x84,, +0.27276,0xA8,, +0.272847,0x01,, +0.272934,0x0C,, +0.273021,0x22,, +0.273108,0x74,, +0.273195,0x2E,, +0.273282,0xD8,, +0.273369,0x2E,, +0.273455,0xE4,, +0.273542,0x2E,, +0.273629,0xD8,, +0.273716,0x22,, +0.273803,0x60,, +0.27389,0x22,, +0.273977,0x60,, +0.274064,0x3B,, +0.274151,0x60,, +0.274238,0x22,, +0.274325,0x60,, +0.274412,0x22,, +0.274499,0x60,, +0.274586,0x3B,, +0.274673,0x60,, +0.27476,0x2E,, +0.274847,0xE0,, +0.274933,0x2E,, +0.27502,0xE0,, +0.275107,0xB1,, +0.275194,0x84,, +0.282808,0xA8,, +0.282895,0x01,, +0.282982,0x0C,, +0.283069,0x22,, +0.283156,0x74,, +0.283242,0x2E,, +0.283329,0xD8,, +0.283416,0x2E,, +0.283503,0xE0,, +0.28359,0x2E,, +0.283677,0xD8,, +0.283764,0x22,, +0.283851,0x60,, +0.283938,0x22,, +0.284025,0x60,, +0.284112,0x3B,, +0.284199,0x60,, +0.284286,0x22,, +0.284373,0x60,, +0.28446,0x22,, +0.284547,0x60,, +0.284634,0x3B,, +0.28472,0x60,, +0.284807,0x2E,, +0.284894,0xE0,, +0.284981,0x2E,, +0.285068,0xE0,, +0.285155,0xF9,, +0.285242,0x9B,, +0.292856,0xA8,, +0.292943,0x01,, +0.29303,0x0C,, +0.293117,0x22,, +0.293204,0x74,, +0.293291,0x2E,, +0.293378,0xD8,, +0.293465,0x2E,, +0.293552,0xE0,, +0.293638,0x2E,, +0.293725,0xD8,, +0.293812,0x22,, +0.293899,0x60,, +0.293986,0x22,, +0.294073,0x60,, +0.29416,0x3B,, +0.294247,0x60,, +0.294334,0x22,, +0.294421,0x60,, +0.294508,0x22,, +0.294595,0x60,, +0.294682,0x3B,, +0.294769,0x60,, +0.294856,0x2E,, +0.294943,0xE0,, +0.29503,0x2E,, +0.295116,0xE0,, +0.295203,0xF9,, +0.29529,0x9B,, +0.302904,0xA8,, +0.302991,0x01,, +0.303078,0x0C,, +0.303165,0x22,, +0.303252,0x74,, +0.303339,0x2E,, +0.303425,0xD8,, +0.303512,0x2E,, +0.303599,0xE0,, +0.303686,0x2E,, +0.303773,0xD8,, +0.30386,0x22,, +0.303947,0x60,, +0.304034,0x22,, +0.304121,0x60,, +0.304208,0x3B,, +0.304295,0x60,, +0.304382,0x22,, +0.304469,0x60,, +0.304556,0x22,, +0.304643,0x60,, +0.30473,0x3B,, +0.304816,0x60,, +0.304903,0x2E,, +0.30499,0xE0,, +0.305077,0x2E,, +0.305164,0xE0,, +0.305251,0xF9,, +0.305338,0x9B,, +0.312952,0xA8,, +0.313039,0x01,, +0.313126,0x0C,, +0.313213,0x22,, +0.3133,0x74,, +0.313387,0x2E,, +0.313474,0xD8,, +0.313561,0x2E,, +0.313648,0xE0,, +0.313735,0x2E,, +0.313822,0xD8,, +0.313909,0x22,, +0.313996,0x60,, +0.314082,0x22,, +0.314169,0x60,, +0.314256,0x3B,, +0.314343,0x60,, +0.31443,0x22,, +0.314517,0x60,, +0.314604,0x22,, +0.314691,0x60,, +0.314778,0x3B,, +0.314865,0x60,, +0.314952,0x2E,, +0.315039,0xE0,, +0.315126,0x2E,, +0.315213,0xE0,, +0.3153,0xF9,, +0.315387,0x9B,, +0.323,0xA8,, +0.323087,0x01,, +0.323174,0x0C,, +0.323261,0x22,, +0.323348,0x74,, +0.323435,0x2E,, +0.323521,0xD8,, +0.323608,0x2E,, +0.323695,0xE4,, +0.323782,0x2E,, +0.323869,0xD8,, +0.323956,0x22,, +0.324043,0x60,, +0.32413,0x22,, +0.324217,0x60,, +0.324304,0x3B,, +0.324391,0x60,, +0.324478,0x22,, +0.324565,0x60,, +0.324652,0x22,, +0.324739,0x60,, +0.324826,0x3B,, +0.324913,0x60,, +0.324999,0x2E,, +0.325086,0xE0,, +0.325173,0x2E,, +0.32526,0xE0,, +0.325347,0xB1,, +0.325434,0x84,, +0.333048,0xA8,, +0.333135,0x01,, +0.333222,0x0C,, +0.333309,0x22,, +0.333396,0x74,, +0.333483,0x2E,, +0.333569,0xD8,, +0.333656,0x2E,, +0.333743,0xE0,, +0.33383,0x2E,, +0.333917,0xD8,, +0.334004,0x22,, +0.334091,0x60,, +0.334178,0x22,, +0.334265,0x60,, +0.334352,0x3B,, +0.334439,0x60,, +0.334526,0x22,, +0.334613,0x60,, +0.3347,0x22,, +0.334787,0x60,, +0.334874,0x3B,, +0.334961,0x60,, +0.335047,0x2E,, +0.335134,0xE0,, +0.335221,0x2E,, +0.335308,0xE0,, +0.335395,0xF9,, +0.335482,0x9B,, +0.343096,0xA8,, +0.343183,0x01,, +0.34327,0x0C,, +0.343356,0x22,, +0.343443,0x74,, +0.34353,0x2E,, +0.343617,0xD8,, +0.343704,0x2E,, +0.343791,0xE0,, +0.343878,0x2E,, +0.343965,0xD8,, +0.344052,0x22,, +0.344139,0x60,, +0.344226,0x22,, +0.344313,0x60,, +0.3444,0x3B,, +0.344487,0x60,, +0.344574,0x22,, +0.344661,0x60,, +0.344748,0x22,, +0.344835,0x60,, +0.344921,0x3B,, +0.345008,0x60,, +0.345095,0x2E,, +0.345182,0xE0,, +0.345269,0x2E,, +0.345356,0xE0,, +0.345443,0xF9,, +0.34553,0x9B,, +0.353143,0xA8,, +0.35323,0x01,, +0.353317,0x0C,, +0.353404,0x22,, +0.353491,0x74,, +0.353578,0x2E,, +0.353665,0xD8,, +0.353752,0x2E,, +0.353839,0xE0,, +0.353926,0x2E,, +0.354012,0xD8,, +0.354099,0x22,, +0.354186,0x60,, +0.354273,0x22,, +0.35436,0x60,, +0.354447,0x3B,, +0.354534,0x60,, +0.354621,0x22,, +0.354708,0x60,, +0.354795,0x22,, +0.354882,0x60,, +0.354969,0x3B,, +0.355056,0x60,, +0.355143,0x2E,, +0.35523,0xE0,, +0.355317,0x2E,, +0.355404,0xE0,, +0.355491,0xF9,, +0.355577,0x9B,, +0.363689,0xA8,, +0.363776,0x01,, +0.363863,0x0C,, +0.36395,0x22,, +0.364037,0x74,, +0.364124,0x2E,, +0.364211,0xD8,, +0.364297,0x2E,, +0.364384,0xE4,, +0.364471,0x2E,, +0.364558,0xDC,, +0.364645,0x22,, +0.364732,0x60,, +0.364819,0x22,, +0.364906,0x60,, +0.364993,0x3B,, +0.36508,0x60,, +0.365167,0x22,, +0.365254,0x60,, +0.365341,0x22,, +0.365428,0x60,, +0.365515,0x3B,, +0.365602,0x60,, +0.365689,0x2E,, +0.365775,0xE0,, +0.365862,0x2E,, +0.365949,0xE0,, +0.366036,0xF2,, +0.366123,0x0C,, +0.373736,0xA8,, +0.373823,0x01,, +0.37391,0x0C,, +0.373997,0x22,, +0.374084,0x74,, +0.374171,0x2E,, +0.374258,0xD8,, +0.374345,0x2E,, +0.374432,0xE0,, +0.374519,0x2E,, +0.374606,0xDC,, +0.374693,0x22,, +0.37478,0x60,, +0.374867,0x22,, +0.374954,0x60,, +0.37504,0x3B,, +0.375127,0x60,, +0.375214,0x22,, +0.375301,0x60,, +0.375388,0x22,, +0.375475,0x60,, +0.375562,0x3B,, +0.375649,0x60,, +0.375736,0x2E,, +0.375823,0xE0,, +0.37591,0x2E,, +0.375997,0xE0,, +0.376084,0xBA,, +0.376171,0x13,, +0.383898,0xA8,, +0.383985,0x01,, +0.384072,0x0C,, +0.384159,0x22,, +0.384246,0x74,, +0.384333,0x2E,, +0.38442,0xD8,, +0.384507,0x2E,, +0.384594,0xE4,, +0.38468,0x2E,, +0.384767,0xD8,, +0.384854,0x22,, +0.384941,0x60,, +0.385028,0x22,, +0.385115,0x60,, +0.385202,0x3B,, +0.385289,0x60,, +0.385376,0x22,, +0.385463,0x60,, +0.38555,0x22,, +0.385637,0x60,, +0.385724,0x3B,, +0.385811,0x60,, +0.385898,0x2E,, +0.385985,0xE0,, +0.386072,0x2E,, +0.386158,0xE0,, +0.386245,0xB1,, +0.386332,0x84,, +0.393945,0xA8,, +0.394032,0x01,, +0.394119,0x0C,, +0.394206,0x22,, +0.394293,0x74,, +0.39438,0x2E,, +0.394467,0xD8,, +0.394554,0x2E,, +0.394641,0xE0,, +0.394728,0x2E,, +0.394815,0xD8,, +0.394902,0x22,, +0.394989,0x60,, +0.395075,0x22,, +0.395162,0x60,, +0.395249,0x3B,, +0.395336,0x60,, +0.395423,0x22,, +0.39551,0x60,, +0.395597,0x22,, +0.395684,0x60,, +0.395771,0x3B,, +0.395858,0x60,, +0.395945,0x2E,, +0.396032,0xE0,, +0.396119,0x2E,, +0.396206,0xE0,, +0.396293,0xF9,, +0.39638,0x9B,, +0.403993,0xA8,, +0.40408,0x01,, +0.404167,0x0C,, +0.404253,0x22,, +0.40434,0x74,, +0.404427,0x2E,, +0.404514,0xD8,, +0.404601,0x2E,, +0.404688,0xE0,, +0.404775,0x2E,, +0.404862,0xD8,, +0.404949,0x22,, +0.405036,0x60,, +0.405123,0x22,, +0.40521,0x60,, +0.405297,0x3B,, +0.405384,0x60,, +0.405471,0x22,, +0.405558,0x60,, +0.405645,0x22,, +0.405731,0x60,, +0.405818,0x3B,, +0.405905,0x60,, +0.405992,0x2E,, +0.406079,0xE0,, +0.406166,0x2E,, +0.406253,0xE0,, +0.40634,0xF9,, +0.406427,0x9B,, +0.414066,0xA8,, +0.414153,0x01,, +0.41424,0x0C,, +0.414327,0x22,, +0.414413,0x74,, +0.4145,0x2E,, +0.414587,0xD8,, +0.414674,0x2E,, +0.414761,0xE0,, +0.414848,0x2E,, +0.414935,0xD8,, +0.415022,0x22,, +0.415109,0x60,, +0.415196,0x22,, +0.415283,0x60,, +0.41537,0x3B,, +0.415457,0x60,, +0.415544,0x22,, +0.415631,0x60,, +0.415718,0x22,, +0.415805,0x60,, +0.415891,0x3B,, +0.415978,0x60,, +0.416065,0x2E,, +0.416152,0xE0,, +0.416239,0x2E,, +0.416326,0xE0,, +0.416413,0xF9,, +0.4165,0x9B,, +0.424113,0xA8,, +0.4242,0x01,, +0.424287,0x0C,, +0.424374,0x22,, +0.424461,0x74,, +0.424548,0x2E,, +0.424635,0xD8,, +0.424721,0x2E,, +0.424808,0xE0,, +0.424895,0x2E,, +0.424982,0xD8,, +0.425069,0x22,, +0.425156,0x60,, +0.425243,0x22,, +0.42533,0x60,, +0.425417,0x3B,, +0.425504,0x60,, +0.425591,0x22,, +0.425678,0x60,, +0.425765,0x22,, +0.425852,0x60,, +0.425939,0x3B,, +0.426026,0x60,, +0.426113,0x2E,, +0.426199,0xE0,, +0.426286,0x2E,, +0.426373,0xE0,, +0.42646,0xF9,, +0.426547,0x9B,, +0.43416,0xA8,, +0.434247,0x01,, +0.434334,0x0C,, +0.434421,0x22,, +0.434508,0x70,, +0.434595,0x2E,, +0.434682,0xD8,, +0.434769,0x2E,, +0.434856,0xE0,, +0.434943,0x2E,, +0.43503,0xD8,, +0.435117,0x22,, +0.435204,0x60,, +0.435291,0x22,, +0.435378,0x60,, +0.435464,0x3B,, +0.435551,0x60,, +0.435638,0x22,, +0.435725,0x60,, +0.435812,0x22,, +0.435899,0x60,, +0.435986,0x3B,, +0.436073,0x60,, +0.43616,0x2E,, +0.436247,0xE0,, +0.436334,0x2E,, +0.436421,0xE0,, +0.436508,0x7D,, +0.436595,0x96,, +0.444248,0xA8,, +0.444335,0x01,, +0.444422,0x0C,, +0.444509,0x22,, +0.444596,0x74,, +0.444683,0x2E,, +0.44477,0xD8,, +0.444857,0x2E,, +0.444944,0xE4,, +0.445031,0x2E,, +0.445118,0xD8,, +0.445205,0x22,, +0.445292,0x60,, +0.445379,0x22,, +0.445466,0x60,, +0.445552,0x3B,, +0.445639,0x60,, +0.445726,0x22,, +0.445813,0x60,, +0.4459,0x22,, +0.445987,0x60,, +0.446074,0x3B,, +0.446161,0x60,, +0.446248,0x2E,, +0.446335,0xE0,, +0.446422,0x2E,, +0.446509,0xE0,, +0.446596,0xB1,, +0.446683,0x84,, +0.454295,0xA8,, +0.454382,0x01,, +0.454469,0x0C,, +0.454556,0x22,, +0.454643,0x74,, +0.45473,0x2E,, +0.454817,0xD8,, +0.454904,0x2E,, +0.454991,0xE4,, +0.455078,0x2E,, +0.455165,0xD8,, +0.455252,0x22,, +0.455339,0x60,, +0.455426,0x22,, +0.455513,0x60,, +0.455599,0x3B,, +0.455686,0x60,, +0.455773,0x22,, +0.45586,0x60,, +0.455947,0x22,, +0.456034,0x60,, +0.456121,0x3B,, +0.456208,0x60,, +0.456295,0x2E,, +0.456382,0xE0,, +0.456469,0x2E,, +0.456556,0xE0,, +0.456643,0xB1,, +0.45673,0x84,, +0.464343,0xA8,, +0.46443,0x01,, +0.464517,0x0C,, +0.464604,0x22,, +0.464691,0x74,, +0.464778,0x2E,, +0.464865,0xD8,, +0.464952,0x2E,, +0.465039,0xE4,, +0.465125,0x2E,, +0.465212,0xD8,, +0.465299,0x22,, +0.465386,0x60,, +0.465473,0x22,, +0.46556,0x60,, +0.465647,0x3B,, +0.465734,0x60,, +0.465821,0x22,, +0.465908,0x60,, +0.465995,0x22,, +0.466082,0x60,, +0.466169,0x3B,, +0.466256,0x60,, +0.466343,0x2E,, +0.46643,0xE0,, +0.466517,0x2E,, +0.466604,0xE0,, +0.46669,0xB1,, +0.466777,0x84,, +0.474391,0xA8,, +0.474478,0x01,, +0.474564,0x0C,, +0.474651,0x22,, +0.474738,0x74,, +0.474825,0x2E,, +0.474912,0xD8,, +0.474999,0x2E,, +0.475086,0xE0,, +0.475173,0x2E,, +0.47526,0xD8,, +0.475347,0x22,, +0.475434,0x60,, +0.475521,0x22,, +0.475608,0x60,, +0.475695,0x3B,, +0.475782,0x60,, +0.475869,0x22,, +0.475956,0x60,, +0.476043,0x22,, +0.476129,0x60,, +0.476216,0x3B,, +0.476303,0x60,, +0.47639,0x2E,, +0.476477,0xE0,, +0.476564,0x2E,, +0.476651,0xE0,, +0.476738,0xF9,, +0.476825,0x9B,, +0.484438,0xA8,, +0.484525,0x01,, +0.484612,0x0C,, +0.484699,0x22,, +0.484786,0x74,, +0.484873,0x2E,, +0.48496,0xD8,, +0.485047,0x2E,, +0.485134,0xE0,, +0.485221,0x2E,, +0.485308,0xD8,, +0.485395,0x22,, +0.485482,0x60,, +0.485569,0x22,, +0.485655,0x60,, +0.485742,0x3B,, +0.485829,0x60,, +0.485916,0x22,, +0.486003,0x60,, +0.48609,0x22,, +0.486177,0x60,, +0.486264,0x3B,, +0.486351,0x60,, +0.486438,0x2E,, +0.486525,0xE0,, +0.486612,0x2E,, +0.486699,0xE0,, +0.486786,0xF9,, +0.486873,0x9B,, +0.494486,0xA8,, +0.494573,0x01,, +0.49466,0x0C,, +0.494747,0x22,, +0.494834,0x74,, +0.494921,0x2E,, +0.495008,0xD8,, +0.495095,0x2E,, +0.495182,0xE4,, +0.495269,0x2E,, +0.495356,0xD8,, +0.495442,0x22,, +0.495529,0x60,, +0.495616,0x22,, +0.495703,0x60,, +0.49579,0x3B,, +0.495877,0x60,, +0.495964,0x22,, +0.496051,0x60,, +0.496138,0x22,, +0.496225,0x60,, +0.496312,0x3B,, +0.496399,0x60,, +0.496486,0x2E,, +0.496573,0xE0,, +0.49666,0x2E,, +0.496747,0xE0,, +0.496834,0xB1,, +0.496921,0x84,, +0.504534,0xA8,, +0.504621,0x01,, +0.504708,0x0C,, +0.504795,0x22,, +0.504882,0x74,, +0.504969,0x2E,, +0.505056,0xD8,, +0.505143,0x2E,, +0.50523,0xE0,, +0.505317,0x2E,, +0.505404,0xD8,, +0.50549,0x22,, +0.505577,0x60,, +0.505664,0x22,, +0.505751,0x60,, +0.505838,0x3B,, +0.505925,0x60,, +0.506012,0x22,, +0.506099,0x60,, +0.506186,0x22,, +0.506273,0x60,, +0.50636,0x3B,, +0.506447,0x60,, +0.506534,0x2E,, +0.506621,0xE0,, +0.506708,0x2E,, +0.506795,0xE0,, +0.506882,0xF9,, +0.506968,0x9B,, +0.514581,0xA8,, +0.514668,0x01,, +0.514755,0x0C,, +0.514842,0x22,, +0.514929,0x78,, +0.515016,0x2E,, +0.515102,0xD8,, +0.515189,0x2E,, +0.515276,0xE4,, +0.515363,0x2E,, +0.51545,0xD8,, +0.515537,0x22,, +0.515624,0x60,, +0.515711,0x22,, +0.515798,0x60,, +0.515885,0x3B,, +0.515972,0x60,, +0.516059,0x22,, +0.516146,0x60,, +0.516233,0x22,, +0.51632,0x60,, +0.516407,0x3B,, +0.516494,0x60,, +0.51658,0x2E,, +0.516667,0xE0,, +0.516754,0x2E,, +0.516841,0xE0,, +0.516928,0x2D,, +0.517015,0xB2,, +0.524628,0xA8,, +0.524715,0x01,, +0.524802,0x0C,, +0.524889,0x22,, +0.524976,0xB8,, +0.525063,0x2E,, +0.52515,0xD8,, +0.525237,0x2E,, +0.525324,0xE4,, +0.525411,0x2E,, +0.525498,0xD8,, +0.525585,0x22,, +0.525672,0x60,, +0.525758,0x22,, +0.525845,0x60,, +0.525932,0x3B,, +0.526019,0x60,, +0.526106,0x22,, +0.526193,0x60,, +0.52628,0x22,, +0.526367,0x60,, +0.526454,0x3B,, +0.526541,0x60,, +0.526628,0x2E,, +0.526715,0xE0,, +0.526802,0x2E,, +0.526889,0xE0,, +0.526976,0x7F,, +0.527063,0xFB,, +0.534676,0xA8,, +0.534763,0x01,, +0.53485,0x0C,, +0.534937,0x23,, +0.535024,0x00,, +0.535111,0x2E,, +0.535198,0xD8,, +0.535285,0x2E,, +0.535372,0xE4,, +0.535459,0x2E,, +0.535546,0xD8,, +0.535633,0x22,, +0.53572,0x60,, +0.535806,0x22,, +0.535893,0x60,, +0.53598,0x3B,, +0.536067,0x60,, +0.536154,0x22,, +0.536241,0x60,, +0.536328,0x22,, +0.536415,0x60,, +0.536502,0x3B,, +0.536589,0x60,, +0.536676,0x2E,, +0.536763,0xE0,, +0.53685,0x2E,, +0.536937,0xE0,, +0.537024,0x93,, +0.537111,0xC8,, +0.544724,0xA8,, +0.544811,0x01,, +0.544898,0x0C,, +0.544985,0x23,, +0.545072,0x40,, +0.545159,0x2E,, +0.545246,0xD8,, +0.545333,0x2E,, +0.54542,0xE0,, +0.545507,0x2E,, +0.545594,0xD8,, +0.545681,0x22,, +0.545768,0x60,, +0.545854,0x22,, +0.545941,0x60,, +0.546028,0x3B,, +0.546115,0x60,, +0.546202,0x22,, +0.546289,0x60,, +0.546376,0x22,, +0.546463,0x60,, +0.54655,0x3B,, +0.546637,0x60,, +0.546724,0x2E,, +0.546811,0xE0,, +0.546898,0x2E,, +0.546985,0xE0,, +0.547072,0x1A,, +0.547159,0x0F,, +0.554771,0xA8,, +0.554858,0x01,, +0.554945,0x0C,, +0.555032,0x23,, +0.555119,0x80,, +0.555206,0x2E,, +0.555293,0xD8,, +0.55538,0x2E,, +0.555467,0xE0,, +0.555554,0x2E,, +0.555641,0xDC,, +0.555728,0x22,, +0.555815,0x60,, +0.555902,0x22,, +0.555988,0x60,, +0.556075,0x3B,, +0.556162,0x60,, +0.556249,0x22,, +0.556336,0x60,, +0.556423,0x22,, +0.55651,0x60,, +0.556597,0x3B,, +0.556684,0x60,, +0.556771,0x2E,, +0.556858,0xE0,, +0.556945,0x2E,, +0.557032,0xE0,, +0.557119,0x0B,, +0.557206,0xCE,, +0.564819,0xA8,, +0.564906,0x01,, +0.564993,0x0C,, +0.56508,0x23,, +0.565167,0xC8,, +0.565254,0x2E,, +0.565341,0xD8,, +0.565427,0x2E,, +0.565514,0xE0,, +0.565601,0x2E,, +0.565688,0xD8,, +0.565775,0x22,, +0.565862,0x60,, +0.565949,0x22,, +0.566036,0x60,, +0.566123,0x3B,, +0.56621,0x60,, +0.566297,0x22,, +0.566384,0x60,, +0.566471,0x22,, +0.566558,0x60,, +0.566645,0x3B,, +0.566732,0x60,, +0.566819,0x2E,, +0.566905,0xE0,, +0.566992,0x2E,, +0.567079,0xE0,, +0.567166,0x91,, +0.567253,0xA5,, +0.574866,0xA8,, +0.574953,0x01,, +0.57504,0x0C,, +0.575127,0x24,, +0.575214,0x04,, +0.575301,0x2E,, +0.575388,0xD8,, +0.575475,0x2E,, +0.575562,0xE4,, +0.575649,0x2E,, +0.575736,0xDC,, +0.575823,0x22,, +0.57591,0x60,, +0.575996,0x22,, +0.576083,0x60,, +0.57617,0x3B,, +0.576257,0x60,, +0.576344,0x22,, +0.576431,0x60,, +0.576518,0x22,, +0.576605,0x60,, +0.576692,0x3B,, +0.576779,0x60,, +0.576866,0x2E,, +0.576953,0xE0,, +0.57704,0x2E,, +0.577127,0xE0,, +0.577214,0xD1,, +0.577301,0x44,, +0.584914,0xA8,, +0.585001,0x01,, +0.585088,0x0C,, +0.585175,0x24,, +0.585262,0x48,, +0.585349,0x2E,, +0.585435,0xD8,, +0.585522,0x2E,, +0.585609,0xE4,, +0.585696,0x2E,, +0.585783,0xDC,, +0.58587,0x22,, +0.585957,0x60,, +0.586044,0x22,, +0.586131,0x60,, +0.586218,0x3B,, +0.586305,0x60,, +0.586392,0x22,, +0.586479,0x60,, +0.586566,0x22,, +0.586653,0x60,, +0.58674,0x3B,, +0.586826,0x60,, +0.586913,0x2E,, +0.587,0xE0,, +0.587087,0x2E,, +0.587174,0xE0,, +0.587261,0x8C,, +0.587348,0xAA,, +0.594962,0xA8,, +0.595049,0x01,, +0.595136,0x0C,, +0.595223,0x24,, +0.59531,0x88,, +0.595397,0x2E,, +0.595484,0xD8,, +0.595571,0x2E,, +0.595658,0xE0,, +0.595745,0x2E,, +0.595832,0xDC,, +0.595919,0x22,, +0.596006,0x60,, +0.596092,0x22,, +0.596179,0x60,, +0.596266,0x3B,, +0.596353,0x60,, +0.59644,0x22,, +0.596527,0x60,, +0.596614,0x22,, +0.596701,0x60,, +0.596788,0x3B,, +0.596875,0x60,, +0.596962,0x2E,, +0.597049,0xE0,, +0.597136,0x2E,, +0.597223,0xE0,, +0.59731,0x96,, +0.597397,0xFC,, +0.605009,0xA8,, +0.605096,0x01,, +0.605183,0x0C,, +0.60527,0x24,, +0.605357,0xD4,, +0.605444,0x2E,, +0.605531,0xD8,, +0.605617,0x2E,, +0.605704,0xE0,, +0.605791,0x2E,, +0.605878,0xDC,, +0.605965,0x22,, +0.606052,0x60,, +0.606139,0x22,, +0.606226,0x60,, +0.606313,0x3B,, +0.6064,0x60,, +0.606487,0x22,, +0.606574,0x60,, +0.606661,0x22,, +0.606748,0x60,, +0.606835,0x3B,, +0.606922,0x60,, +0.607008,0x2E,, +0.607095,0xE0,, +0.607182,0x2E,, +0.607269,0xE0,, +0.607356,0xFB,, +0.607443,0x64,, +0.615056,0xA8,, +0.615143,0x01,, +0.61523,0x0C,, +0.615317,0x25,, +0.615404,0x28,, +0.615491,0x2E,, +0.615578,0xD8,, +0.615665,0x2E,, +0.615752,0xEC,, +0.615839,0x2E,, +0.615926,0xE0,, +0.616013,0x22,, +0.616099,0x60,, +0.616186,0x22,, +0.616273,0x60,, +0.61636,0x3B,, +0.616447,0x60,, +0.616534,0x22,, +0.616621,0x60,, +0.616708,0x22,, +0.616795,0x60,, +0.616882,0x3B,, +0.616969,0x60,, +0.617056,0x2E,, +0.617143,0xE0,, +0.61723,0x2E,, +0.617317,0xE0,, +0.617404,0x6C,, +0.617491,0x38,, +0.625104,0xA8,, +0.625191,0x01,, +0.625278,0x0C,, +0.625365,0x25,, +0.625452,0x80,, +0.625539,0x2E,, +0.625626,0xE0,, +0.625713,0x2E,, +0.6258,0xE0,, +0.625887,0x2E,, +0.625973,0xE0,, +0.62606,0x22,, +0.626147,0x60,, +0.626234,0x22,, +0.626321,0x60,, +0.626408,0x3B,, +0.626495,0x60,, +0.626582,0x22,, +0.626669,0x60,, +0.626756,0x22,, +0.626843,0x60,, +0.62693,0x3B,, +0.627017,0x60,, +0.627104,0x2E,, +0.627191,0xE0,, +0.627278,0x2E,, +0.627365,0xE0,, +0.627451,0x13,, +0.627538,0x5F,, +0.635152,0xA8,, +0.635239,0x01,, +0.635326,0x0C,, +0.635413,0x25,, +0.6355,0xD4,, +0.635587,0x2E,, +0.635674,0xD8,, +0.635761,0x2E,, +0.635848,0xE4,, +0.635935,0x2E,, +0.636022,0xE0,, +0.636108,0x22,, +0.636195,0x60,, +0.636282,0x22,, +0.636369,0x60,, +0.636456,0x3B,, +0.636543,0x60,, +0.63663,0x22,, +0.636717,0x60,, +0.636804,0x22,, +0.636891,0x60,, +0.636978,0x3B,, +0.637065,0x60,, +0.637152,0x2E,, +0.637239,0xE0,, +0.637326,0x2E,, +0.637413,0xE0,, +0.6375,0x62,, +0.637586,0xE3,, +0.645199,0xA8,, +0.645286,0x01,, +0.645373,0x0C,, +0.64546,0x26,, +0.645547,0x2C,, +0.645634,0x2E,, +0.645721,0xD8,, +0.645808,0x2E,, +0.645895,0xE4,, +0.645982,0x2E,, +0.646069,0xE0,, +0.646156,0x22,, +0.646243,0x60,, +0.64633,0x22,, +0.646417,0x60,, +0.646504,0x3B,, +0.646591,0x60,, +0.646677,0x22,, +0.646764,0x60,, +0.646851,0x22,, +0.646938,0x60,, +0.647025,0x3B,, +0.647112,0x60,, +0.647199,0x2E,, +0.647286,0xE0,, +0.647373,0x2E,, +0.64746,0xE0,, +0.647547,0x21,, +0.647634,0x0E,, +0.655247,0xA8,, +0.655334,0x01,, +0.655421,0x0C,, +0.655508,0x26,, +0.655595,0x84,, +0.655682,0x2E,, +0.655769,0xD8,, +0.655856,0x2E,, +0.655943,0xE0,, +0.65603,0x2E,, +0.656117,0xE0,, +0.656203,0x22,, +0.65629,0x60,, +0.656377,0x22,, +0.656464,0x60,, +0.656551,0x3B,, +0.656638,0x60,, +0.656725,0x22,, +0.656812,0x60,, +0.656899,0x22,, +0.656986,0x60,, +0.657073,0x3B,, +0.65716,0x60,, +0.657247,0x2E,, +0.657334,0xE0,, +0.657421,0x2E,, +0.657508,0xE0,, +0.657595,0x82,, +0.657681,0x57,, +0.665295,0xA8,, +0.665382,0x01,, +0.665469,0x0C,, +0.665556,0x26,, +0.665643,0xE4,, +0.66573,0x2E,, +0.665817,0xD8,, +0.665904,0x2E,, +0.665991,0xE4,, +0.666078,0x2E,, +0.666165,0xE4,, +0.666252,0x22,, +0.666339,0x60,, +0.666425,0x22,, +0.666512,0x60,, +0.666599,0x3B,, +0.666686,0x60,, +0.666773,0x22,, +0.66686,0x60,, +0.666947,0x22,, +0.667034,0x60,, +0.667121,0x3B,, +0.667208,0x60,, +0.667295,0x2E,, +0.667382,0xE0,, +0.667469,0x2E,, +0.667556,0xE0,, +0.667643,0x28,, +0.66773,0xF4,, +0.675342,0xA8,, +0.675429,0x01,, +0.675516,0x0C,, +0.675603,0x27,, +0.67569,0x38,, +0.675777,0x2E,, +0.675864,0xD8,, +0.675951,0x2E,, +0.676038,0xE4,, +0.676125,0x2E,, +0.676211,0xE4,, +0.676298,0x22,, +0.676385,0x60,, +0.676472,0x22,, +0.676559,0x60,, +0.676646,0x3B,, +0.676733,0x60,, +0.67682,0x22,, +0.676907,0x60,, +0.676994,0x22,, +0.677081,0x60,, +0.677168,0x3B,, +0.677255,0x60,, +0.677342,0x2E,, +0.677429,0xE0,, +0.677516,0x2E,, +0.677603,0xE0,, +0.67769,0xE1,, +0.677776,0xFE,, +0.68539,0xA8,, +0.685477,0x01,, +0.685564,0x0C,, +0.685651,0x27,, +0.685737,0x98,, +0.685824,0x2E,, +0.685911,0xD8,, +0.685998,0x2E,, +0.686085,0xE4,, +0.686172,0x2E,, +0.686259,0xE4,, +0.686346,0x22,, +0.686433,0x60,, +0.68652,0x22,, +0.686607,0x60,, +0.686694,0x3B,, +0.686781,0x60,, +0.686868,0x22,, +0.686955,0x60,, +0.687042,0x22,, +0.687129,0x60,, +0.687216,0x3B,, +0.687302,0x60,, +0.687389,0x2E,, +0.687476,0xE0,, +0.687563,0x2E,, +0.68765,0xE0,, +0.687737,0x12,, +0.687824,0x83,, +0.695438,0xA8,, +0.695525,0x01,, +0.695612,0x0C,, +0.695699,0x28,, +0.695786,0x0C,, +0.695873,0x2E,, +0.695959,0xD0,, +0.696046,0x2E,, +0.696133,0xE0,, +0.69622,0x2E,, +0.696307,0xE4,, +0.696394,0x22,, +0.696481,0x60,, +0.696568,0x22,, +0.696655,0x60,, +0.696742,0x3B,, +0.696829,0x60,, +0.696916,0x22,, +0.697003,0x60,, +0.69709,0x22,, +0.697177,0x60,, +0.697264,0x3B,, +0.697351,0x60,, +0.697437,0x2E,, +0.697524,0xE0,, +0.697611,0x2E,, +0.697698,0xE0,, +0.697785,0x9F,, +0.697872,0x30,, +0.705485,0xA8,, +0.705572,0x01,, +0.705659,0x0C,, +0.705746,0x28,, +0.705833,0x74,, +0.70592,0x2E,, +0.706007,0xD8,, +0.706094,0x2E,, +0.70618,0xE4,, +0.706267,0x2E,, +0.706354,0xE4,, +0.706441,0x22,, +0.706528,0x60,, +0.706615,0x22,, +0.706702,0x60,, +0.706789,0x3B,, +0.706876,0x60,, +0.706963,0x22,, +0.70705,0x60,, +0.707137,0x22,, +0.707224,0x60,, +0.707311,0x3B,, +0.707398,0x60,, +0.707485,0x2E,, +0.707572,0xE0,, +0.707658,0x2E,, +0.707745,0xE0,, +0.707832,0x91,, +0.707919,0x20,, +0.715533,0xA8,, +0.71562,0x01,, +0.715707,0x0C,, +0.715794,0x28,, +0.715881,0xEC,, +0.715967,0x2E,, +0.716054,0xD8,, +0.716141,0x2E,, +0.716228,0xE0,, +0.716315,0x2E,, +0.716402,0xE0,, +0.716489,0x22,, +0.716576,0x60,, +0.716663,0x22,, +0.71675,0x60,, +0.716837,0x3B,, +0.716924,0x60,, +0.717011,0x22,, +0.717098,0x60,, +0.717185,0x22,, +0.717272,0x60,, +0.717359,0x3B,, +0.717445,0x60,, +0.717532,0x2E,, +0.717619,0xE0,, +0.717706,0x2E,, +0.717793,0xE0,, +0.71788,0x21,, +0.717967,0x6B,, +0.72558,0xA8,, +0.725667,0x01,, +0.725754,0x0C,, +0.725841,0x29,, +0.725928,0x74,, +0.726015,0x2E,, +0.726102,0xD8,, +0.726189,0x2E,, +0.726276,0xE4,, +0.726362,0x2E,, +0.726449,0xE4,, +0.726536,0x22,, +0.726623,0x60,, +0.72671,0x22,, +0.726797,0x60,, +0.726884,0x3B,, +0.726971,0x60,, +0.727058,0x22,, +0.727145,0x60,, +0.727232,0x22,, +0.727319,0x60,, +0.727406,0x3B,, +0.727493,0x60,, +0.72758,0x2E,, +0.727667,0xE0,, +0.727754,0x2E,, +0.727841,0xE0,, +0.727927,0xA6,, +0.728014,0x23,, +0.735628,0xA8,, +0.735715,0x01,, +0.735802,0x0C,, +0.735889,0x29,, +0.735976,0xFC,, +0.736063,0x2E,, +0.73615,0xD8,, +0.736237,0x2E,, +0.736324,0xE0,, +0.736411,0x2E,, +0.736498,0xE0,, +0.736585,0x22,, +0.736671,0x60,, +0.736758,0x22,, +0.736845,0x60,, +0.736932,0x3B,, +0.737019,0x60,, +0.737106,0x22,, +0.737193,0x60,, +0.73728,0x22,, +0.737367,0x60,, +0.737454,0x3B,, +0.737541,0x60,, +0.737628,0x2E,, +0.737715,0xE0,, +0.737802,0x2E,, +0.737889,0xE0,, +0.737976,0x26,, +0.738063,0x1E,, +0.745675,0xA8,, +0.745762,0x01,, +0.745849,0x0C,, +0.745936,0x2A,, +0.746023,0x78,, +0.74611,0x2E,, +0.746197,0xD8,, +0.746284,0x2E,, +0.746371,0xE4,, +0.746458,0x2E,, +0.746545,0xE4,, +0.746631,0x22,, +0.746718,0x60,, +0.746805,0x22,, +0.746892,0x60,, +0.746979,0x3B,, +0.747066,0x60,, +0.747153,0x22,, +0.74724,0x60,, +0.747327,0x22,, +0.747414,0x60,, +0.747501,0x3B,, +0.747588,0x60,, +0.747675,0x2E,, +0.747762,0xE0,, +0.747849,0x2E,, +0.747936,0xE0,, +0.748023,0x63,, +0.748109,0x10,, +0.755723,0xA8,, +0.75581,0x01,, +0.755897,0x0C,, +0.755984,0x2A,, +0.756071,0xF8,, +0.756158,0x2E,, +0.756244,0xD8,, +0.756331,0x2E,, +0.756418,0xE0,, +0.756505,0x2E,, +0.756592,0xE4,, +0.756679,0x22,, +0.756766,0x60,, +0.756853,0x22,, +0.75694,0x60,, +0.757027,0x3B,, +0.757114,0x60,, +0.757201,0x22,, +0.757288,0x60,, +0.757375,0x22,, +0.757462,0x60,, +0.757549,0x3B,, +0.757636,0x60,, +0.757722,0x2E,, +0.757809,0xE0,, +0.757896,0x2E,, +0.757983,0xE0,, +0.75807,0xB8,, +0.758157,0x9E,, +0.765773,0xA8,, +0.76586,0x01,, +0.765947,0x0C,, +0.766034,0x2B,, +0.766121,0x88,, +0.766208,0x2E,, +0.766295,0xD8,, +0.766382,0x2E,, +0.766469,0xE0,, +0.766556,0x2E,, +0.766643,0xE4,, +0.76673,0x22,, +0.766817,0x60,, +0.766904,0x22,, +0.766991,0x60,, +0.767078,0x3B,, +0.767164,0x60,, +0.767251,0x22,, +0.767338,0x60,, +0.767425,0x22,, +0.767512,0x60,, +0.767599,0x3B,, +0.767686,0x60,, +0.767773,0x2E,, +0.76786,0xE0,, +0.767947,0x2E,, +0.768034,0xE0,, +0.768121,0x1E,, +0.768208,0xDF,, +0.775821,0xA8,, +0.775908,0x01,, +0.775995,0x0C,, +0.776082,0x2B,, +0.776169,0xF4,, +0.776256,0x2E,, +0.776343,0xD8,, +0.77643,0x2E,, +0.776517,0xE0,, +0.776604,0x2E,, +0.776691,0xE8,, +0.776778,0x22,, +0.776865,0x60,, +0.776952,0x22,, +0.777039,0x60,, +0.777125,0x3B,, +0.777212,0x60,, +0.777299,0x22,, +0.777386,0x60,, +0.777473,0x22,, +0.77756,0x60,, +0.777647,0x3B,, +0.777734,0x60,, +0.777821,0x2E,, +0.777908,0xE0,, +0.777995,0x2E,, +0.778082,0xE0,, +0.778169,0xD7,, +0.778256,0x33,, +0.785868,0xA8,, +0.785955,0x01,, +0.786042,0x0C,, +0.786129,0x2C,, +0.786216,0x9C,, +0.786303,0x2E,, +0.78639,0xD8,, +0.786477,0x2E,, +0.786564,0xE0,, +0.786651,0x2E,, +0.786738,0xE8,, +0.786825,0x22,, +0.786912,0x60,, +0.786999,0x22,, +0.787086,0x60,, +0.787173,0x3B,, +0.787259,0x60,, +0.787346,0x22,, +0.787433,0x60,, +0.78752,0x22,, +0.787607,0x60,, +0.787694,0x3B,, +0.787781,0x60,, +0.787868,0x2E,, +0.787955,0xE0,, +0.788042,0x2E,, +0.788129,0xE0,, +0.788216,0xEB,, +0.788303,0x35,, +0.795915,0xA8,, +0.796002,0x01,, +0.796089,0x0C,, +0.796176,0x2D,, +0.796263,0x1C,, +0.79635,0x2E,, +0.796437,0xD8,, +0.796524,0x2E,, +0.796611,0xE4,, +0.796698,0x2E,, +0.796785,0xE8,, +0.796872,0x22,, +0.796959,0x60,, +0.797046,0x22,, +0.797133,0x60,, +0.79722,0x3B,, +0.797306,0x60,, +0.797393,0x22,, +0.79748,0x60,, +0.797567,0x22,, +0.797654,0x60,, +0.797741,0x3B,, +0.797828,0x60,, +0.797915,0x2E,, +0.798002,0xE0,, +0.798089,0x2E,, +0.798176,0xE0,, +0.798263,0x07,, +0.79835,0xB8,, +0.805963,0xA8,, +0.80605,0x01,, +0.806137,0x0C,, +0.806224,0x2D,, +0.806311,0xA0,, +0.806398,0x2E,, +0.806485,0xD8,, +0.806572,0x2E,, +0.806659,0xE4,, +0.806746,0x2E,, +0.806833,0xE8,, +0.80692,0x22,, +0.807007,0x60,, +0.807094,0x22,, +0.807181,0x60,, +0.807267,0x3B,, +0.807354,0x60,, +0.807441,0x22,, +0.807528,0x60,, +0.807615,0x22,, +0.807702,0x60,, +0.807789,0x3B,, +0.807876,0x60,, +0.807963,0x2E,, +0.80805,0xE0,, +0.808137,0x2E,, +0.808224,0xE0,, +0.808311,0x58,, +0.808398,0x85,, +0.816011,0xA8,, +0.816098,0x01,, +0.816185,0x0C,, +0.816272,0x2E,, +0.816359,0x44,, +0.816446,0x2E,, +0.816533,0xD8,, +0.81662,0x2E,, +0.816707,0xE0,, +0.816794,0x2E,, +0.816881,0xEC,, +0.816968,0x22,, +0.817055,0x60,, +0.817141,0x22,, +0.817228,0x60,, +0.817315,0x3B,, +0.817402,0x60,, +0.817489,0x22,, +0.817576,0x60,, +0.817663,0x22,, +0.81775,0x60,, +0.817837,0x3B,, +0.817924,0x60,, +0.818011,0x2E,, +0.818098,0xE0,, +0.818185,0x2E,, +0.818272,0xE0,, +0.818359,0xBC,, +0.818446,0xBF,, +0.826059,0xA8,, +0.826146,0x01,, +0.826233,0x0C,, +0.826319,0x2E,, +0.826406,0xE0,, +0.826493,0x2E,, +0.82658,0xD8,, +0.826667,0x2E,, +0.826754,0xE0,, +0.826841,0x2E,, +0.826928,0xEC,, +0.827015,0x22,, +0.827102,0x60,, +0.827189,0x22,, +0.827276,0x60,, +0.827363,0x3B,, +0.82745,0x60,, +0.827537,0x22,, +0.827624,0x60,, +0.827711,0x22,, +0.827797,0x60,, +0.827884,0x3B,, +0.827971,0x60,, +0.828058,0x2E,, +0.828145,0xE0,, +0.828232,0x2E,, +0.828319,0xE0,, +0.828406,0xCB,, +0.828493,0xCF,, +0.836106,0xA8,, +0.836193,0x01,, +0.83628,0x0C,, +0.836367,0x2F,, +0.836454,0x80,, +0.836541,0x2E,, +0.836628,0xD8,, +0.836715,0x2E,, +0.836802,0xE0,, +0.836889,0x2E,, +0.836976,0xF0,, +0.837063,0x22,, +0.83715,0x60,, +0.837236,0x22,, +0.837323,0x60,, +0.83741,0x3B,, +0.837497,0x60,, +0.837584,0x22,, +0.837671,0x60,, +0.837758,0x22,, +0.837845,0x60,, +0.837932,0x3B,, +0.838019,0x60,, +0.838106,0x2E,, +0.838193,0xE0,, +0.83828,0x2E,, +0.838367,0xE0,, +0.838454,0x87,, +0.838541,0x61,, +0.846154,0xA8,, +0.846241,0x01,, +0.846328,0x0C,, +0.846415,0x30,, +0.846501,0x24,, +0.846588,0x2E,, +0.846675,0xD8,, +0.846762,0x2E,, +0.846849,0xE0,, +0.846936,0x2E,, +0.847023,0xF0,, +0.84711,0x22,, +0.847197,0x60,, +0.847284,0x22,, +0.847371,0x60,, +0.847458,0x3B,, +0.847545,0x60,, +0.847632,0x22,, +0.847719,0x60,, +0.847806,0x22,, +0.847893,0x60,, +0.847979,0x3B,, +0.848066,0x60,, +0.848153,0x2E,, +0.84824,0xE0,, +0.848327,0x2E,, +0.848414,0xE0,, +0.848501,0x9D,, +0.848588,0x72,, +0.856201,0xA8,, +0.856288,0x01,, +0.856375,0x0C,, +0.856462,0x30,, +0.856549,0xE0,, +0.856636,0x2E,, +0.856723,0xD8,, +0.85681,0x2E,, +0.856896,0xE0,, +0.856983,0x2E,, +0.85707,0xF8,, +0.857157,0x22,, +0.857244,0x60,, +0.857331,0x22,, +0.857418,0x60,, +0.857505,0x3B,, +0.857592,0x60,, +0.857679,0x22,, +0.857766,0x60,, +0.857853,0x22,, +0.85794,0x60,, +0.858027,0x3B,, +0.858114,0x60,, +0.858201,0x2E,, +0.858288,0xE0,, +0.858374,0x2E,, +0.858461,0xE0,, +0.858548,0xCC,, +0.858635,0x26,, +0.866248,0xA8,, +0.866335,0x01,, +0.866422,0x0C,, +0.866509,0x31,, +0.866596,0x84,, +0.866683,0x2E,, +0.86677,0xD8,, +0.866857,0x2E,, +0.866944,0xE0,, +0.867031,0x2E,, +0.867118,0xF8,, +0.867205,0x22,, +0.867292,0x60,, +0.867378,0x22,, +0.867465,0x60,, +0.867552,0x3B,, +0.867639,0x60,, +0.867726,0x22,, +0.867813,0x60,, +0.8679,0x22,, +0.867987,0x60,, +0.868074,0x3B,, +0.868161,0x60,, +0.868248,0x2E,, +0.868335,0xE0,, +0.868422,0x2E,, +0.868509,0xE0,, +0.868596,0xDE,, +0.868683,0x1C,, +0.876297,0xA8,, +0.876383,0x01,, +0.87647,0x0C,, +0.876557,0x32,, +0.876644,0x34,, +0.876731,0x2E,, +0.876818,0xD8,, +0.876905,0x2E,, +0.876992,0xD8,, +0.877079,0x2E,, +0.877166,0xF8,, +0.877253,0x22,, +0.87734,0x60,, +0.877427,0x22,, +0.877514,0x60,, +0.877601,0x3B,, +0.877688,0x60,, +0.877775,0x22,, +0.877861,0x60,, +0.877948,0x22,, +0.878035,0x60,, +0.878122,0x3B,, +0.878209,0x60,, +0.878296,0x2E,, +0.878383,0xE0,, +0.87847,0x2E,, +0.878557,0xE0,, +0.878644,0x84,, +0.878731,0xCB,, +0.886851,0xA8,, +0.886938,0x01,, +0.887025,0x0C,, +0.887112,0x32,, +0.887199,0xCC,, +0.887286,0x2E,, +0.887373,0xDC,, +0.88746,0x2E,, +0.887547,0xE4,, +0.887634,0x2E,, +0.887721,0xF8,, +0.887808,0x22,, +0.887895,0x60,, +0.887982,0x22,, +0.888069,0x60,, +0.888155,0x3B,, +0.888242,0x60,, +0.888329,0x22,, +0.888416,0x60,, +0.888503,0x22,, +0.88859,0x60,, +0.888677,0x3B,, +0.888764,0x60,, +0.888851,0x2E,, +0.888938,0xE0,, +0.889025,0x2E,, +0.889112,0xE0,, +0.889199,0x71,, +0.889286,0x5E,, +0.897007,0xA8,, +0.897094,0x01,, +0.897181,0x0C,, +0.897268,0x33,, +0.897355,0x50,, +0.897442,0x2E,, +0.897529,0xD8,, +0.897616,0x2E,, +0.897703,0xE0,, +0.89779,0x2E,, +0.897877,0xF8,, +0.897964,0x22,, +0.898051,0x60,, +0.898138,0x22,, +0.898225,0x60,, +0.898311,0x3B,, +0.898398,0x60,, +0.898485,0x22,, +0.898572,0x60,, +0.898659,0x22,, +0.898746,0x60,, +0.898833,0x3B,, +0.89892,0x60,, +0.899007,0x2E,, +0.899094,0xE0,, +0.899181,0x2E,, +0.899268,0xE0,, +0.899355,0x56,, +0.899442,0x28,, +0.907054,0xA8,, +0.907141,0x01,, +0.907228,0x0C,, +0.907315,0x33,, +0.907402,0xE0,, +0.907489,0x2E,, +0.907576,0xD8,, +0.907662,0x2E,, +0.907749,0xE0,, +0.907836,0x2E,, +0.907923,0xF8,, +0.90801,0x22,, +0.908097,0x60,, +0.908184,0x22,, +0.908271,0x60,, +0.908358,0x3B,, +0.908445,0x60,, +0.908532,0x22,, +0.908619,0x60,, +0.908706,0x22,, +0.908793,0x60,, +0.90888,0x3B,, +0.908967,0x60,, +0.909053,0x2E,, +0.90914,0xE0,, +0.909227,0x2E,, +0.909314,0xE0,, +0.909401,0x95,, +0.909488,0x23,, +0.917101,0xA8,, +0.917188,0x01,, +0.917275,0x0C,, +0.917362,0x34,, +0.917449,0x68,, +0.917536,0x2E,, +0.917623,0xD8,, +0.91771,0x2E,, +0.917797,0xE0,, +0.917884,0x2E,, +0.917971,0xF4,, +0.918057,0x22,, +0.918144,0x60,, +0.918231,0x22,, +0.918318,0x60,, +0.918405,0x3B,, +0.918492,0x60,, +0.918579,0x22,, +0.918666,0x60,, +0.918753,0x22,, +0.91884,0x60,, +0.918927,0x3B,, +0.919014,0x60,, +0.919101,0x2E,, +0.919188,0xE0,, +0.919275,0x2E,, +0.919362,0xE0,, +0.919449,0x5F,, +0.919535,0x18,, +0.927207,0xA8,, +0.927294,0x01,, +0.927381,0x0C,, +0.927468,0x34,, +0.927555,0xF4,, +0.927642,0x2E,, +0.927729,0xD8,, +0.927816,0x2E,, +0.927903,0xE4,, +0.92799,0x2E,, +0.928077,0xF4,, +0.928164,0x22,, +0.928251,0x60,, +0.928338,0x22,, +0.928425,0x60,, +0.928511,0x3B,, +0.928598,0x60,, +0.928685,0x22,, +0.928772,0x60,, +0.928859,0x22,, +0.928946,0x60,, +0.929033,0x3B,, +0.92912,0x60,, +0.929207,0x2E,, +0.929294,0xE0,, +0.929381,0x2E,, +0.929468,0xE0,, +0.929555,0x28,, +0.929642,0xD6,, +0.937276,0xA8,, +0.937363,0x01,, +0.93745,0x0C,, +0.937537,0x35,, +0.937623,0x68,, +0.93771,0x2E,, +0.937797,0xD8,, +0.937884,0x2E,, +0.937971,0xE4,, +0.938058,0x2E,, +0.938145,0xF0,, +0.938232,0x22,, +0.938319,0x60,, +0.938406,0x22,, +0.938493,0x60,, +0.93858,0x3B,, +0.938667,0x60,, +0.938754,0x22,, +0.938841,0x60,, +0.938928,0x22,, +0.939015,0x60,, +0.939102,0x3B,, +0.939188,0x60,, +0.939275,0x2E,, +0.939362,0xE0,, +0.939449,0x2E,, +0.939536,0xE0,, +0.939623,0x63,, +0.93971,0x8C,, +0.947327,0xA8,, +0.947414,0x01,, +0.947501,0x0C,, +0.947588,0x35,, +0.947675,0xE8,, +0.947761,0x2E,, +0.947848,0xD8,, +0.947935,0x2E,, +0.948022,0xE0,, +0.948109,0x2E,, +0.948196,0xEC,, +0.948283,0x22,, +0.94837,0x60,, +0.948457,0x22,, +0.948544,0x60,, +0.948631,0x3B,, +0.948718,0x60,, +0.948805,0x22,, +0.948892,0x60,, +0.948979,0x22,, +0.949066,0x60,, +0.949153,0x3B,, +0.949239,0x60,, +0.949326,0x2E,, +0.949413,0xE0,, +0.9495,0x2E,, +0.949587,0xE0,, +0.949674,0x62,, +0.949761,0x9B,, +0.957373,0xA8,, +0.95746,0x01,, +0.957547,0x0C,, +0.957634,0x36,, +0.957721,0x68,, +0.957808,0x2E,, +0.957895,0xD8,, +0.957982,0x2E,, +0.958069,0xE0,, +0.958156,0x2E,, +0.958243,0xF0,, +0.95833,0x22,, +0.958416,0x60,, +0.958503,0x22,, +0.95859,0x60,, +0.958677,0x3B,, +0.958764,0x60,, +0.958851,0x22,, +0.958938,0x60,, +0.959025,0x22,, +0.959112,0x60,, +0.959199,0x3B,, +0.959286,0x60,, +0.959373,0x2E,, +0.95946,0xE0,, +0.959547,0x2E,, +0.959634,0xE0,, +0.959721,0x72,, +0.959808,0x96,, +0.96745,0xA8,, +0.967537,0x01,, +0.967624,0x0C,, +0.96771,0x36,, +0.967797,0xE4,, +0.967884,0x2E,, +0.967971,0xD8,, +0.968058,0x2E,, +0.968145,0xE0,, +0.968232,0x2E,, +0.968319,0xF0,, +0.968406,0x22,, +0.968493,0x60,, +0.96858,0x22,, +0.968667,0x60,, +0.968754,0x3B,, +0.968841,0x60,, +0.968928,0x22,, +0.969015,0x60,, +0.969102,0x22,, +0.969188,0x60,, +0.969275,0x3B,, +0.969362,0x60,, +0.969449,0x2E,, +0.969536,0xE0,, +0.969623,0x2E,, +0.96971,0xE0,, +0.969797,0x7D,, +0.969884,0x31,, +0.977501,0xA8,, +0.977588,0x01,, +0.977674,0x0C,, +0.977761,0x37,, +0.977848,0x68,, +0.977935,0x2E,, +0.978022,0xD8,, +0.978109,0x2E,, +0.978196,0xE0,, +0.978283,0x2E,, +0.97837,0xF0,, +0.978457,0x22,, +0.978544,0x60,, +0.978631,0x22,, +0.978718,0x60,, +0.978805,0x3B,, +0.978892,0x60,, +0.978979,0x22,, +0.979066,0x60,, +0.979153,0x22,, +0.979239,0x60,, +0.979326,0x3B,, +0.979413,0x60,, +0.9795,0x2E,, +0.979587,0xE0,, +0.979674,0x2E,, +0.979761,0xE0,, +0.979848,0x45,, +0.979935,0x95,, +0.987547,0xA8,, +0.987634,0x01,, +0.987721,0x0C,, +0.987808,0x37,, +0.987895,0xE4,, +0.987982,0x2E,, +0.988069,0xD8,, +0.988156,0x2E,, +0.988243,0xE4,, +0.98833,0x2E,, +0.988417,0xEC,, +0.988504,0x22,, +0.988591,0x60,, +0.988678,0x22,, +0.988765,0x60,, +0.988852,0x3B,, +0.988938,0x60,, +0.989025,0x22,, +0.989112,0x60,, +0.989199,0x22,, +0.989286,0x60,, +0.989373,0x3B,, +0.98946,0x60,, +0.989547,0x2E,, +0.989634,0xE0,, +0.989721,0x2E,, +0.989808,0xE0,, +0.989895,0xD8,, +0.989982,0xB4,, +0.997595,0xA8,, +0.997682,0x01,, +0.997769,0x0C,, +0.997856,0x38,, +0.997943,0x54,, +0.99803,0x2E,, +0.998117,0xD8,, +0.998203,0x2E,, +0.99829,0xE0,, +0.998377,0x2E,, +0.998464,0xEC,, +0.998551,0x22,, +0.998638,0x60,, +0.998725,0x22,, +0.998812,0x60,, +0.998899,0x3B,, +0.998986,0x60,, +0.999073,0x22,, +0.99916,0x60,, +0.999247,0x22,, +0.999334,0x60,, +0.999421,0x3B,, +0.999508,0x60,, +0.999594,0x2E,, +0.999681,0xE0,, +0.999768,0x2E,, +0.999855,0xE0,, +0.999942,0x7E,, +1.000029,0x90,, +1.007642,0xA8,, +1.007729,0x01,, +1.007816,0x0C,, +1.007903,0x38,, +1.00799,0xB8,, +1.008077,0x2E,, +1.008164,0xD8,, +1.008251,0x2E,, +1.008337,0xE4,, +1.008424,0x2E,, +1.008511,0xE8,, +1.008598,0x22,, +1.008685,0x60,, +1.008772,0x22,, +1.008859,0x60,, +1.008946,0x3B,, +1.009033,0x60,, +1.00912,0x22,, +1.009207,0x60,, +1.009294,0x22,, +1.009381,0x60,, +1.009468,0x3B,, +1.009555,0x60,, +1.009642,0x2E,, +1.009729,0xE0,, +1.009815,0x2E,, +1.009902,0xE0,, +1.009989,0xDB,, +1.010076,0x94,, +1.017689,0xA8,, +1.017776,0x01,, +1.017863,0x0C,, +1.01795,0x39,, +1.018037,0x14,, +1.018124,0x2E,, +1.018211,0xE0,, +1.018298,0x2E,, +1.018385,0xE4,, +1.018472,0x2E,, +1.018559,0xE4,, +1.018646,0x22,, +1.018733,0x60,, +1.018819,0x22,, +1.018906,0x60,, +1.018993,0x3B,, +1.01908,0x60,, +1.019167,0x22,, +1.019254,0x60,, +1.019341,0x22,, +1.019428,0x60,, +1.019515,0x3B,, +1.019602,0x60,, +1.019689,0x2E,, +1.019776,0xE0,, +1.019863,0x2E,, +1.01995,0xE0,, +1.020037,0x0B,, +1.020124,0x44,, +1.027736,0xA8,, +1.027823,0x01,, +1.02791,0x0C,, +1.027997,0x39,, +1.028084,0x68,, +1.028171,0x2E,, +1.028258,0xD4,, +1.028345,0x2E,, +1.028432,0xE0,, +1.028519,0x2E,, +1.028606,0xE4,, +1.028693,0x22,, +1.02878,0x60,, +1.028867,0x22,, +1.028954,0x60,, +1.029041,0x3B,, +1.029127,0x60,, +1.029214,0x22,, +1.029301,0x60,, +1.029388,0x22,, +1.029475,0x60,, +1.029562,0x3B,, +1.029649,0x60,, +1.029736,0x2E,, +1.029823,0xE0,, +1.02991,0x2E,, +1.029997,0xE0,, +1.030084,0xAA,, +1.030171,0xE2,, +1.037783,0xA8,, +1.03787,0x01,, +1.037957,0x0C,, +1.038044,0x39,, +1.038131,0xBC,, +1.038218,0x2E,, +1.038305,0xD8,, +1.038392,0x2E,, +1.038479,0xE0,, +1.038566,0x2E,, +1.038653,0xE8,, +1.03874,0x22,, +1.038826,0x60,, +1.038913,0x22,, +1.039,0x60,, +1.039087,0x3B,, +1.039174,0x60,, +1.039261,0x22,, +1.039348,0x60,, +1.039435,0x22,, +1.039522,0x60,, +1.039609,0x3B,, +1.039696,0x60,, +1.039783,0x2E,, +1.03987,0xE0,, +1.039957,0x2E,, +1.040044,0xE0,, +1.040131,0x20,, +1.040218,0x85,, +1.04783,0xA8,, +1.047917,0x01,, +1.048004,0x0C,, +1.048091,0x3A,, +1.048178,0x04,, +1.048265,0x2E,, +1.048352,0xD8,, +1.048439,0x2E,, +1.048526,0xE4,, +1.048613,0x2E,, +1.0487,0xE8,, +1.048787,0x22,, +1.048873,0x60,, +1.04896,0x22,, +1.049047,0x60,, +1.049134,0x3B,, +1.049221,0x60,, +1.049308,0x22,, +1.049395,0x60,, +1.049482,0x22,, +1.049569,0x60,, +1.049656,0x3B,, +1.049743,0x60,, +1.04983,0x2E,, +1.049917,0xE0,, +1.050004,0x2E,, +1.050091,0xE0,, +1.050178,0xEA,, +1.050265,0xAF,, +1.057877,0xA8,, +1.057964,0x01,, +1.058051,0x0C,, +1.058138,0x3A,, +1.058225,0x4C,, +1.058312,0x2E,, +1.058399,0xD8,, +1.058486,0x2E,, +1.058573,0xE0,, +1.05866,0x2E,, +1.058747,0xE8,, +1.058833,0x22,, +1.05892,0x60,, +1.059007,0x22,, +1.059094,0x60,, +1.059181,0x3B,, +1.059268,0x60,, +1.059355,0x22,, +1.059442,0x60,, +1.059529,0x22,, +1.059616,0x60,, +1.059703,0x3B,, +1.05979,0x60,, +1.059877,0x2E,, +1.059964,0xE0,, +1.060051,0x2E,, +1.060138,0xE0,, +1.060225,0x7B,, +1.060312,0x53,, +1.067924,0xA8,, +1.068011,0x01,, +1.068098,0x0C,, +1.068185,0x3A,, +1.068272,0x88,, +1.068359,0x2E,, +1.068446,0xD8,, +1.068533,0x2E,, +1.068619,0xE0,, +1.068706,0x2E,, +1.068793,0xE8,, +1.06888,0x22,, +1.068967,0x60,, +1.069054,0x22,, +1.069141,0x60,, +1.069228,0x3B,, +1.069315,0x60,, +1.069402,0x22,, +1.069489,0x60,, +1.069576,0x22,, +1.069663,0x60,, +1.06975,0x3B,, +1.069837,0x60,, +1.069924,0x2E,, +1.07001,0xE0,, +1.070097,0x2E,, +1.070184,0xE0,, +1.070271,0xAD,, +1.070358,0x17,, +1.077971,0xA8,, +1.078057,0x01,, +1.078144,0x0C,, +1.078231,0x3A,, +1.078318,0xC0,, +1.078405,0x2E,, +1.078492,0xD8,, +1.078579,0x2E,, +1.078666,0xE0,, +1.078753,0x2E,, +1.07884,0xE4,, +1.078927,0x22,, +1.079014,0x60,, +1.079101,0x22,, +1.079188,0x60,, +1.079275,0x3B,, +1.079362,0x60,, +1.079449,0x22,, +1.079535,0x60,, +1.079622,0x22,, +1.079709,0x60,, +1.079796,0x3B,, +1.079883,0x60,, +1.07997,0x2E,, +1.080057,0xE0,, +1.080144,0x2E,, +1.080231,0xE0,, +1.080318,0xB0,, +1.080405,0x6C,, +1.088018,0xA8,, +1.088104,0x01,, +1.088191,0x0C,, +1.088278,0x3A,, +1.088365,0xF4,, +1.088452,0x2E,, +1.088539,0xD8,, +1.088626,0x2E,, +1.088713,0xE0,, +1.0888,0x2E,, +1.088887,0xE8,, +1.088974,0x22,, +1.089061,0x60,, +1.089148,0x22,, +1.089235,0x60,, +1.089322,0x3B,, +1.089409,0x60,, +1.089496,0x22,, +1.089583,0x60,, +1.089669,0x22,, +1.089756,0x60,, +1.089843,0x3B,, +1.08993,0x60,, +1.090017,0x2E,, +1.090104,0xE0,, +1.090191,0x2E,, +1.090278,0xE0,, +1.090365,0xA0,, +1.090452,0x63,, +1.098064,0xA8,, +1.098151,0x01,, +1.098238,0x0C,, +1.098325,0x3B,, +1.098412,0x30,, +1.098499,0x2E,, +1.098586,0xD8,, +1.098673,0x2E,, +1.09876,0xE4,, +1.098847,0x2E,, +1.098933,0xE8,, +1.09902,0x22,, +1.099107,0x60,, +1.099194,0x22,, +1.099281,0x60,, +1.099368,0x3B,, +1.099455,0x60,, +1.099542,0x22,, +1.099629,0x60,, +1.099716,0x22,, +1.099803,0x60,, +1.09989,0x3B,, +1.099977,0x60,, +1.100064,0x2E,, +1.100151,0xE0,, +1.100238,0x2E,, +1.100325,0xE0,, +1.100412,0x09,, +1.100498,0x3B,, +1.108112,0xA8,, +1.108199,0x01,, +1.108286,0x0C,, +1.108372,0x3B,, +1.108459,0x60,, +1.108546,0x2E,, +1.108633,0xD8,, +1.10872,0x2E,, +1.108807,0xE4,, +1.108894,0x2E,, +1.108981,0xE8,, +1.109068,0x22,, +1.109155,0x60,, +1.109242,0x22,, +1.109329,0x60,, +1.109416,0x3B,, +1.109503,0x60,, +1.10959,0x22,, +1.109677,0x60,, +1.109764,0x22,, +1.10985,0x60,, +1.109937,0x3B,, +1.110024,0x60,, +1.110111,0x2E,, +1.110198,0xE0,, +1.110285,0x2E,, +1.110372,0xE0,, +1.110459,0xF8,, +1.110546,0x95,, +1.118158,0xA8,, +1.118245,0x01,, +1.118332,0x0C,, +1.118419,0x3B,, +1.118506,0x60,, +1.118593,0x2E,, +1.11868,0xD8,, +1.118767,0x2E,, +1.118854,0xE4,, +1.118941,0x2E,, +1.119028,0xE0,, +1.119115,0x22,, +1.119202,0x60,, +1.119289,0x22,, +1.119376,0x60,, +1.119463,0x3B,, +1.11955,0x60,, +1.119636,0x22,, +1.119723,0x60,, +1.11981,0x22,, +1.119897,0x60,, +1.119984,0x3B,, +1.120071,0x60,, +1.120158,0x2E,, +1.120245,0xE0,, +1.120332,0x2E,, +1.120419,0xE0,, +1.120506,0x7F,, +1.120593,0x85,, +1.128206,0xA8,, +1.128293,0x01,, +1.12838,0x0C,, +1.128466,0x3B,, +1.128553,0x60,, +1.12864,0x2E,, +1.128727,0xD8,, +1.128814,0x2E,, +1.128901,0xE0,, +1.128988,0x2E,, +1.129075,0xE0,, +1.129162,0x22,, +1.129249,0x60,, +1.129336,0x22,, +1.129423,0x60,, +1.12951,0x3B,, +1.129597,0x60,, +1.129684,0x22,, +1.129771,0x60,, +1.129858,0x22,, +1.129944,0x60,, +1.130031,0x3B,, +1.130118,0x60,, +1.130205,0x2E,, +1.130292,0xE0,, +1.130379,0x2E,, +1.130466,0xE0,, +1.130553,0x37,, +1.13064,0x9A,, +1.138253,0xA8,, +1.13834,0x01,, +1.138426,0x0C,, +1.138513,0x3B,, +1.1386,0x60,, +1.138687,0x2E,, +1.138774,0xD8,, +1.138861,0x2E,, +1.138948,0xE0,, +1.139035,0x2E,, +1.139122,0xE0,, +1.139209,0x22,, +1.139296,0x60,, +1.139383,0x22,, +1.13947,0x60,, +1.139557,0x3B,, +1.139644,0x60,, +1.139731,0x22,, +1.139818,0x60,, +1.139905,0x22,, +1.139991,0x60,, +1.140078,0x3B,, +1.140165,0x60,, +1.140252,0x2E,, +1.140339,0xE0,, +1.140426,0x2E,, +1.140513,0xE0,, +1.1406,0x37,, +1.140687,0x9A,, +1.1483,0xA8,, +1.148387,0x01,, +1.148474,0x0C,, +1.148561,0x3B,, +1.148648,0x60,, +1.148735,0x2E,, +1.148822,0xD8,, +1.148909,0x2E,, +1.148996,0xE4,, +1.149083,0x2E,, +1.14917,0xE0,, +1.149257,0x22,, +1.149344,0x60,, +1.149431,0x22,, +1.149517,0x60,, +1.149604,0x3B,, +1.149691,0x60,, +1.149778,0x22,, +1.149865,0x60,, +1.149952,0x22,, +1.150039,0x60,, +1.150126,0x3B,, +1.150213,0x60,, +1.1503,0x2E,, +1.150387,0xE0,, +1.150474,0x2E,, +1.150561,0xE0,, +1.150648,0x7F,, +1.150735,0x85,, +1.158347,0xA8,, +1.158434,0x01,, +1.158521,0x0C,, +1.158608,0x3B,, +1.158695,0x60,, +1.158782,0x2E,, +1.158869,0xD8,, +1.158956,0x2E,, +1.159043,0xE0,, +1.15913,0x2E,, +1.159217,0xE0,, +1.159303,0x22,, +1.15939,0x60,, +1.159477,0x22,, +1.159564,0x60,, +1.159651,0x3B,, +1.159738,0x60,, +1.159825,0x22,, +1.159912,0x60,, +1.159999,0x22,, +1.160086,0x60,, +1.160173,0x3B,, +1.16026,0x60,, +1.160347,0x2E,, +1.160434,0xE0,, +1.160521,0x2E,, +1.160608,0xE0,, +1.160695,0x37,, +1.160782,0x9A,, +1.168395,0xA8,, +1.168482,0x01,, +1.168569,0x0C,, +1.168656,0x3B,, +1.168743,0x60,, +1.168829,0x2E,, +1.168916,0xD8,, +1.169003,0x2E,, +1.16909,0xE0,, +1.169177,0x2E,, +1.169264,0xDC,, +1.169351,0x22,, +1.169438,0x60,, +1.169525,0x22,, +1.169612,0x60,, +1.169699,0x3B,, +1.169786,0x60,, +1.169873,0x22,, +1.16996,0x60,, +1.170047,0x22,, +1.170134,0x60,, +1.170221,0x3B,, +1.170307,0x60,, +1.170394,0x2E,, +1.170481,0xE0,, +1.170568,0x2E,, +1.170655,0xE0,, +1.170742,0xD1,, +1.170829,0x01,, +1.178446,0xA8,, +1.178533,0x01,, +1.17862,0x0C,, +1.178707,0x3B,, +1.178793,0x60,, +1.17888,0x2E,, +1.178967,0xD8,, +1.179054,0x2E,, +1.179141,0xE4,, +1.179228,0x2E,, +1.179315,0xDC,, +1.179402,0x22,, +1.179489,0x60,, +1.179576,0x22,, +1.179663,0x60,, +1.17975,0x3B,, +1.179837,0x60,, +1.179924,0x22,, +1.180011,0x60,, +1.180098,0x22,, +1.180185,0x60,, +1.180271,0x3B,, +1.180358,0x60,, +1.180445,0x2E,, +1.180532,0xE0,, +1.180619,0x2E,, +1.180706,0xE0,, +1.180793,0x99,, +1.18088,0x1E,, +1.188493,0xA8,, +1.18858,0x01,, +1.188667,0x0C,, +1.188754,0x3B,, +1.18884,0x60,, +1.188927,0x2E,, +1.189014,0xD8,, +1.189101,0x2E,, +1.189188,0xE4,, +1.189275,0x2E,, +1.189362,0xDC,, +1.189449,0x22,, +1.189536,0x60,, +1.189623,0x22,, +1.18971,0x60,, +1.189797,0x3B,, +1.189884,0x60,, +1.189971,0x22,, +1.190058,0x60,, +1.190145,0x22,, +1.190232,0x60,, +1.190318,0x3B,, +1.190405,0x60,, +1.190492,0x2E,, +1.190579,0xE0,, +1.190666,0x2E,, +1.190753,0xE0,, +1.19084,0x99,, +1.190927,0x1E,, +1.198539,0xA8,, +1.198626,0x01,, +1.198713,0x0C,, +1.1988,0x3B,, +1.198887,0x60,, +1.198974,0x2E,, +1.199061,0xDC,, +1.199148,0x2E,, +1.199235,0xE4,, +1.199322,0x2E,, +1.199409,0xD4,, +1.199496,0x22,, +1.199583,0x60,, +1.19967,0x22,, +1.199757,0x60,, +1.199844,0x3B,, +1.19993,0x60,, +1.200017,0x22,, +1.200104,0x60,, +1.200191,0x22,, +1.200278,0x60,, +1.200365,0x3B,, +1.200452,0x60,, +1.200539,0x2E,, +1.200626,0xE0,, +1.200713,0x2E,, +1.2008,0xE0,, +1.200887,0x79,, +1.200974,0xB5,, +1.208587,0xA8,, +1.208674,0x01,, +1.208761,0x0C,, +1.208848,0x3B,, +1.208935,0x60,, +1.209021,0x2E,, +1.209108,0xD8,, +1.209195,0x2E,, +1.209282,0xE0,, +1.209369,0x2E,, +1.209456,0xE0,, +1.209543,0x22,, +1.20963,0x60,, +1.209717,0x22,, +1.209804,0x60,, +1.209891,0x3B,, +1.209978,0x60,, +1.210065,0x22,, +1.210152,0x60,, +1.210239,0x22,, +1.210326,0x60,, +1.210413,0x3B,, +1.210499,0x60,, +1.210586,0x2E,, +1.210673,0xE0,, +1.21076,0x2E,, +1.210847,0xE0,, +1.210934,0x37,, +1.211021,0x9A,, +1.218634,0xA8,, +1.218721,0x01,, +1.218808,0x0C,, +1.218895,0x3B,, +1.218982,0x60,, +1.219069,0x2E,, +1.219156,0xD8,, +1.219243,0x2E,, +1.21933,0xE0,, +1.219416,0x2E,, +1.219503,0xE0,, +1.21959,0x22,, +1.219677,0x60,, +1.219764,0x22,, +1.219851,0x60,, +1.219938,0x3B,, +1.220025,0x60,, +1.220112,0x22,, +1.220199,0x60,, +1.220286,0x22,, +1.220373,0x60,, +1.22046,0x3B,, +1.220547,0x60,, +1.220634,0x2E,, +1.220721,0xE0,, +1.220808,0x2E,, +1.220894,0xE0,, +1.220981,0x37,, +1.221068,0x9A,, +1.228681,0xA8,, +1.228768,0x01,, +1.228855,0x0C,, +1.228942,0x3B,, +1.229029,0x60,, +1.229116,0x2E,, +1.229203,0xD8,, +1.22929,0x2E,, +1.229377,0xE4,, +1.229464,0x2E,, +1.229551,0xE0,, +1.229638,0x22,, +1.229725,0x60,, +1.229812,0x22,, +1.229898,0x60,, +1.229985,0x3B,, +1.230072,0x60,, +1.230159,0x22,, +1.230246,0x60,, +1.230333,0x22,, +1.23042,0x60,, +1.230507,0x3B,, +1.230594,0x60,, +1.230681,0x2E,, +1.230768,0xE0,, +1.230855,0x2E,, +1.230942,0xE0,, +1.231029,0x7F,, +1.231116,0x85,, +1.238728,0xA8,, +1.238815,0x01,, +1.238902,0x0C,, +1.238989,0x3B,, +1.239076,0x60,, +1.239163,0x2E,, +1.23925,0xD8,, +1.239337,0x2E,, +1.239424,0xE0,, +1.239511,0x2E,, +1.239598,0xE0,, +1.239685,0x22,, +1.239772,0x60,, +1.239859,0x22,, +1.239946,0x60,, +1.240032,0x3B,, +1.240119,0x60,, +1.240206,0x22,, +1.240293,0x60,, +1.24038,0x22,, +1.240467,0x60,, +1.240554,0x3B,, +1.240641,0x60,, +1.240728,0x2E,, +1.240815,0xE0,, +1.240902,0x2E,, +1.240989,0xE0,, +1.241076,0x37,, +1.241163,0x9A,, +1.248776,0xA8,, +1.248863,0x01,, +1.248949,0x0C,, +1.249036,0x3B,, +1.249123,0x60,, +1.24921,0x2E,, +1.249297,0xD0,, +1.249384,0x2E,, +1.249471,0xE0,, +1.249558,0x2E,, +1.249645,0xE0,, +1.249732,0x22,, +1.249819,0x60,, +1.249906,0x22,, +1.249993,0x60,, +1.25008,0x3B,, +1.250167,0x60,, +1.250254,0x22,, +1.250341,0x60,, +1.250427,0x22,, +1.250514,0x60,, +1.250601,0x3B,, +1.250688,0x60,, +1.250775,0x2E,, +1.250862,0xE0,, +1.250949,0x2E,, +1.251036,0xE0,, +1.251123,0xF8,, +1.25121,0xEC,, +1.258822,0xA8,, +1.258909,0x01,, +1.258996,0x0C,, +1.259083,0x3B,, +1.25917,0x60,, +1.259257,0x2E,, +1.259344,0xD8,, +1.259431,0x2E,, +1.259518,0xE0,, +1.259605,0x2E,, +1.259692,0xE0,, +1.259779,0x22,, +1.259866,0x60,, +1.259953,0x22,, +1.26004,0x60,, +1.260127,0x3B,, +1.260213,0x60,, +1.2603,0x22,, +1.260387,0x60,, +1.260474,0x22,, +1.260561,0x60,, +1.260648,0x3B,, +1.260735,0x60,, +1.260822,0x2E,, +1.260909,0xE0,, +1.260996,0x2E,, +1.261083,0xE0,, +1.26117,0x37,, +1.261257,0x9A,, +1.268869,0xA8,, +1.268956,0x01,, +1.269043,0x0C,, +1.26913,0x3B,, +1.269217,0x60,, +1.269304,0x2E,, +1.269391,0xD8,, +1.269478,0x2E,, +1.269565,0xE0,, +1.269652,0x2E,, +1.269739,0xE0,, +1.269826,0x22,, +1.269913,0x60,, +1.27,0x22,, +1.270087,0x60,, +1.270174,0x3B,, +1.270261,0x60,, +1.270347,0x22,, +1.270434,0x60,, +1.270521,0x22,, +1.270608,0x60,, +1.270695,0x3B,, +1.270782,0x60,, +1.270869,0x2E,, +1.270956,0xE0,, +1.271043,0x2E,, +1.27113,0xE0,, +1.271217,0x37,, +1.271304,0x9A,, +1.278936,0xA8,, +1.279023,0x01,, +1.27911,0x0C,, +1.279197,0x3B,, +1.279283,0x60,, +1.27937,0x2E,, +1.279457,0xD8,, +1.279544,0x2E,, +1.279631,0xE4,, +1.279718,0x2E,, +1.279805,0xE0,, +1.279892,0x22,, +1.279979,0x60,, +1.280066,0x22,, +1.280153,0x60,, +1.28024,0x3B,, +1.280327,0x60,, +1.280414,0x22,, +1.280501,0x60,, +1.280588,0x22,, +1.280675,0x60,, +1.280761,0x3B,, +1.280848,0x60,, +1.280935,0x2E,, +1.281022,0xE0,, +1.281109,0x2E,, +1.281196,0xE0,, +1.281283,0x7F,, +1.28137,0x85,, +1.288983,0xA8,, +1.28907,0x01,, +1.289157,0x0C,, +1.289244,0x3B,, +1.289331,0x60,, +1.289418,0x2E,, +1.289505,0xDC,, +1.289592,0x2E,, +1.289679,0xE4,, +1.289766,0x2E,, +1.289852,0xE0,, +1.289939,0x22,, +1.290026,0x60,, +1.290113,0x22,, +1.2902,0x60,, +1.290287,0x3B,, +1.290374,0x60,, +1.290461,0x22,, +1.290548,0x60,, +1.290635,0x22,, +1.290722,0x60,, +1.290809,0x3B,, +1.290896,0x60,, +1.290983,0x2E,, +1.29107,0xE0,, +1.291157,0x2E,, +1.291244,0xE0,, +1.29133,0x18,, +1.291417,0x3E,, +1.29903,0xA8,, +1.299117,0x01,, +1.299204,0x0C,, +1.299291,0x3B,, +1.299378,0x60,, +1.299464,0x2E,, +1.299551,0xD8,, +1.299638,0x2E,, +1.299725,0xE0,, +1.299812,0x2E,, +1.299899,0xE0,, +1.299986,0x22,, +1.300073,0x60,, +1.30016,0x22,, +1.300247,0x60,, +1.300334,0x3B,, +1.300421,0x60,, +1.300508,0x22,, +1.300595,0x60,, +1.300682,0x22,, +1.300769,0x60,, +1.300856,0x3B,, +1.300942,0x60,, +1.301029,0x2E,, +1.301116,0xE0,, +1.301203,0x2E,, +1.30129,0xE0,, +1.301377,0x37,, +1.301464,0x9A,, +1.309076,0xA8,, +1.309163,0x01,, +1.30925,0x0C,, +1.309337,0x3B,, +1.309424,0x60,, +1.309511,0x2E,, +1.309598,0xD8,, +1.309685,0x2E,, +1.309772,0xE0,, +1.309859,0x2E,, +1.309946,0xDC,, +1.310033,0x22,, +1.31012,0x60,, +1.310207,0x22,, +1.310294,0x60,, +1.31038,0x3B,, +1.310467,0x60,, +1.310554,0x22,, +1.310641,0x60,, +1.310728,0x22,, +1.310815,0x60,, +1.310902,0x3B,, +1.310989,0x60,, +1.311076,0x2E,, +1.311163,0xE0,, +1.31125,0x2E,, +1.311337,0xE0,, +1.311424,0xD1,, +1.311511,0x01,, +1.319123,0xA8,, +1.31921,0x01,, +1.319297,0x0C,, +1.319384,0x3B,, +1.319471,0x60,, +1.319558,0x2E,, +1.319645,0xD8,, +1.319732,0x2E,, +1.319819,0xE0,, +1.319906,0x2E,, +1.319993,0xDC,, +1.32008,0x22,, +1.320167,0x60,, +1.320254,0x22,, +1.320341,0x60,, +1.320428,0x3B,, +1.320514,0x60,, +1.320601,0x22,, +1.320688,0x60,, +1.320775,0x22,, +1.320862,0x60,, +1.320949,0x3B,, +1.321036,0x60,, +1.321123,0x2E,, +1.32121,0xE0,, +1.321297,0x2E,, +1.321384,0xE0,, +1.321471,0xD1,, +1.321558,0x01,, +1.32917,0xA8,, +1.329257,0x01,, +1.329344,0x0C,, +1.329431,0x3B,, +1.329518,0x60,, +1.329605,0x2E,, +1.329692,0xD8,, +1.329779,0x2E,, +1.329866,0xE0,, +1.329953,0x2E,, +1.33004,0xDC,, +1.330127,0x22,, +1.330214,0x60,, +1.330301,0x22,, +1.330388,0x60,, +1.330475,0x3B,, +1.330562,0x60,, +1.330648,0x22,, +1.330735,0x60,, +1.330822,0x22,, +1.330909,0x60,, +1.330996,0x3B,, +1.331083,0x60,, +1.33117,0x2E,, +1.331257,0xE0,, +1.331344,0x2E,, +1.331431,0xE0,, +1.331518,0xD1,, +1.331605,0x01,, +1.339218,0xA8,, +1.339305,0x01,, +1.339392,0x0C,, +1.339479,0x3B,, +1.339566,0x60,, +1.339652,0x2E,, +1.339739,0xD8,, +1.339826,0x2E,, +1.339913,0xE0,, +1.34,0x2E,, +1.340087,0xD8,, +1.340174,0x22,, +1.340261,0x60,, +1.340348,0x22,, +1.340435,0x60,, +1.340522,0x3B,, +1.340609,0x60,, +1.340696,0x22,, +1.340783,0x60,, +1.34087,0x22,, +1.340957,0x60,, +1.341044,0x3B,, +1.34113,0x60,, +1.341217,0x2E,, +1.341304,0xE0,, +1.341391,0x2E,, +1.341478,0xE0,, +1.341565,0x92,, +1.341652,0x89,, +1.349264,0xA8,, +1.349351,0x01,, +1.349438,0x0C,, +1.349525,0x3B,, +1.349612,0x60,, +1.349699,0x2E,, +1.349786,0xD8,, +1.349873,0x2E,, +1.34996,0xE0,, +1.350047,0x2E,, +1.350134,0xDC,, +1.350221,0x22,, +1.350308,0x60,, +1.350395,0x22,, +1.350482,0x60,, +1.350568,0x3B,, +1.350655,0x60,, +1.350742,0x22,, +1.350829,0x60,, +1.350916,0x22,, +1.351003,0x60,, +1.35109,0x3B,, +1.351177,0x60,, +1.351264,0x2E,, +1.351351,0xE0,, +1.351438,0x2E,, +1.351525,0xE0,, +1.351612,0xD1,, +1.351699,0x01,, +1.359311,0xA8,, +1.359398,0x01,, +1.359485,0x0C,, +1.359572,0x3B,, +1.359659,0x54,, +1.359746,0x2E,, +1.359833,0xD8,, +1.35992,0x2E,, +1.360007,0xE0,, +1.360094,0x2E,, +1.360181,0xD8,, +1.360268,0x22,, +1.360355,0x60,, +1.360441,0x22,, +1.360528,0x60,, +1.360615,0x3B,, +1.360702,0x60,, +1.360789,0x22,, +1.360876,0x60,, +1.360963,0x22,, +1.36105,0x60,, +1.361137,0x3B,, +1.361224,0x60,, +1.361311,0x2E,, +1.361398,0xE0,, +1.361485,0x2E,, +1.361572,0xE0,, +1.361659,0x46,, +1.361746,0x1E,, +1.369358,0xA8,, +1.369445,0x01,, +1.369532,0x0C,, +1.369619,0x3B,, +1.369706,0x34,, +1.369793,0x2E,, +1.36988,0xD8,, +1.369967,0x2E,, +1.370054,0xE0,, +1.370141,0x2E,, +1.370228,0xDC,, +1.370315,0x22,, +1.370402,0x60,, +1.370489,0x22,, +1.370576,0x60,, +1.370662,0x3B,, +1.370749,0x60,, +1.370836,0x22,, +1.370923,0x60,, +1.37101,0x22,, +1.371097,0x60,, +1.371184,0x3B,, +1.371271,0x60,, +1.371358,0x2E,, +1.371445,0xE0,, +1.371532,0x2E,, +1.371619,0xE0,, +1.371706,0xA4,, +1.371793,0xA2,, +1.379405,0xA8,, +1.379492,0x01,, +1.379579,0x0C,, +1.379666,0x3B,, +1.379753,0x00,, +1.37984,0x2E,, +1.379927,0xD8,, +1.380014,0x2E,, +1.380101,0xE4,, +1.380188,0x2E,, +1.380275,0xDC,, +1.380361,0x22,, +1.380448,0x60,, +1.380535,0x22,, +1.380622,0x60,, +1.380709,0x3B,, +1.380796,0x60,, +1.380883,0x22,, +1.38097,0x60,, +1.381057,0x22,, +1.381144,0x60,, +1.381231,0x3B,, +1.381318,0x60,, +1.381405,0x2E,, +1.381492,0xE0,, +1.381579,0x2E,, +1.381666,0xE0,, +1.381753,0x38,, +1.381839,0x2A,, +1.389452,0xA8,, +1.389539,0x01,, +1.389626,0x0C,, +1.389713,0x3A,, +1.3898,0xBC,, +1.389887,0x2E,, +1.389974,0xCC,, +1.390061,0x2E,, +1.390148,0xE0,, +1.390235,0x2E,, +1.390322,0xD8,, +1.390409,0x22,, +1.390496,0x60,, +1.390583,0x22,, +1.39067,0x60,, +1.390756,0x3B,, +1.390843,0x60,, +1.39093,0x22,, +1.391017,0x60,, +1.391104,0x22,, +1.391191,0x60,, +1.391278,0x3B,, +1.391365,0x60,, +1.391452,0x2E,, +1.391539,0xE0,, +1.391626,0x2E,, +1.391713,0xE0,, +1.3918,0xB2,, +1.391887,0xF5,, +1.399499,0xA8,, +1.399586,0x01,, +1.399673,0x0C,, +1.39976,0x3A,, +1.399847,0x68,, +1.399934,0x2E,, +1.400021,0xD8,, +1.400108,0x2E,, +1.400195,0xE0,, +1.400282,0x2E,, +1.400369,0xDC,, +1.400456,0x22,, +1.400543,0x60,, +1.400629,0x22,, +1.400716,0x60,, +1.400803,0x3B,, +1.40089,0x60,, +1.400977,0x22,, +1.401064,0x60,, +1.401151,0x22,, +1.401238,0x60,, +1.401325,0x3B,, +1.401412,0x60,, +1.401499,0x2E,, +1.401586,0xE0,, +1.401673,0x2E,, +1.40176,0xE0,, +1.401847,0xFE,, +1.401934,0x39,, +1.409546,0xA8,, +1.409633,0x01,, +1.40972,0x0C,, +1.409807,0x39,, +1.409894,0xFC,, +1.409981,0x2E,, +1.410068,0xD8,, +1.410155,0x2E,, +1.410242,0xE4,, +1.410329,0x2E,, +1.410416,0xDC,, +1.410503,0x22,, +1.41059,0x60,, +1.410677,0x22,, +1.410763,0x60,, +1.41085,0x3B,, +1.410937,0x60,, +1.411024,0x22,, +1.411111,0x60,, +1.411198,0x22,, +1.411285,0x60,, +1.411372,0x3B,, +1.411459,0x60,, +1.411546,0x2E,, +1.411633,0xE0,, +1.41172,0x2E,, +1.411807,0xE0,, +1.411894,0xC8,, +1.411981,0xC9,, +1.419593,0xA8,, +1.41968,0x01,, +1.419767,0x0C,, +1.419854,0x39,, +1.419941,0x70,, +1.420028,0x2E,, +1.420115,0xD8,, +1.420201,0x2E,, +1.420288,0xE0,, +1.420375,0x2E,, +1.420462,0xDC,, +1.420549,0x22,, +1.420636,0x60,, +1.420723,0x22,, +1.42081,0x60,, +1.420897,0x3B,, +1.420984,0x60,, +1.421071,0x22,, +1.421158,0x60,, +1.421245,0x22,, +1.421332,0x60,, +1.421419,0x3B,, +1.421506,0x60,, +1.421593,0x2E,, +1.421679,0xE0,, +1.421766,0x2E,, +1.421853,0xE0,, +1.42194,0x8F,, +1.422027,0x71,, +1.42964,0xA8,, +1.429727,0x01,, +1.429814,0x0C,, +1.429901,0x38,, +1.429988,0xC8,, +1.430075,0x2E,, +1.430162,0xD8,, +1.430249,0x2E,, +1.430336,0xE0,, +1.430423,0x2E,, +1.43051,0xD8,, +1.430596,0x22,, +1.430683,0x60,, +1.43077,0x22,, +1.430857,0x60,, +1.430944,0x3B,, +1.431031,0x60,, +1.431118,0x22,, +1.431205,0x60,, +1.431292,0x22,, +1.431379,0x60,, +1.431466,0x3B,, +1.431553,0x60,, +1.43164,0x2E,, +1.431727,0xE0,, +1.431814,0x2E,, +1.431901,0xE0,, +1.431988,0x20,, +1.432074,0xCA,, +1.439687,0xA8,, +1.439774,0x01,, +1.439861,0x0C,, +1.439948,0x38,, +1.440034,0x04,, +1.440121,0x2E,, +1.440208,0xD8,, +1.440295,0x2E,, +1.440382,0xE4,, +1.440469,0x2E,, +1.440556,0xDC,, +1.440643,0x22,, +1.44073,0x60,, +1.440817,0x22,, +1.440904,0x60,, +1.440991,0x3B,, +1.441078,0x60,, +1.441165,0x22,, +1.441252,0x60,, +1.441339,0x22,, +1.441426,0x60,, +1.441513,0x3B,, +1.441599,0x60,, +1.441686,0x2E,, +1.441773,0xE0,, +1.44186,0x2E,, +1.441947,0xE0,, +1.442034,0xE5,, +1.442121,0x22,, +1.449743,0xA8,, +1.44983,0x01,, +1.449917,0x0C,, +1.450004,0x37,, +1.450091,0x24,, +1.450178,0x2E,, +1.450265,0xD8,, +1.450352,0x2E,, +1.450439,0xE4,, +1.450526,0x2E,, +1.450613,0xDC,, +1.450699,0x22,, +1.450786,0x60,, +1.450873,0x22,, +1.45096,0x60,, +1.451047,0x3B,, +1.451134,0x60,, +1.451221,0x22,, +1.451308,0x60,, +1.451395,0x22,, +1.451482,0x60,, +1.451569,0x3B,, +1.451656,0x60,, +1.451743,0x2E,, +1.45183,0xE0,, +1.451917,0x2E,, +1.452004,0xE0,, +1.452091,0xA8,, +1.452178,0xFE,, +1.459781,0xA8,, +1.459868,0x01,, +1.459954,0x0C,, +1.460041,0x36,, +1.460128,0x70,, +1.460215,0x2E,, +1.460302,0xD8,, +1.460389,0x2E,, +1.460476,0xE0,, +1.460563,0x2E,, +1.46065,0xD8,, +1.460737,0x22,, +1.460824,0x60,, +1.460911,0x22,, +1.460998,0x60,, +1.461085,0x3B,, +1.461172,0x60,, +1.461259,0x22,, +1.461346,0x60,, +1.461432,0x22,, +1.461519,0x60,, +1.461606,0x3B,, +1.461693,0x60,, +1.46178,0x2E,, +1.461867,0xE0,, +1.461954,0x2E,, +1.462041,0xE0,, +1.462128,0xE1,, +1.462215,0xC9,, +1.469828,0xA8,, +1.469915,0x01,, +1.470002,0x0C,, +1.470089,0x35,, +1.470176,0xB4,, +1.470263,0x2E,, +1.47035,0xD8,, +1.470437,0x2E,, +1.470523,0xE4,, +1.47061,0x2E,, +1.470697,0xDC,, +1.470784,0x22,, +1.470871,0x60,, +1.470958,0x22,, +1.471045,0x60,, +1.471132,0x3B,, +1.471219,0x60,, +1.471306,0x22,, +1.471393,0x60,, +1.47148,0x22,, +1.471567,0x60,, +1.471654,0x3B,, +1.471741,0x60,, +1.471828,0x2E,, +1.471915,0xE0,, +1.472001,0x2E,, +1.472088,0xE0,, +1.472175,0x65,, +1.472262,0x1F,, +1.479874,0xA8,, +1.479961,0x01,, +1.480048,0x0C,, +1.480135,0x35,, +1.480222,0x10,, +1.480309,0x2E,, +1.480396,0xD8,, +1.480483,0x2E,, +1.48057,0xE0,, +1.480657,0x2E,, +1.480744,0xD8,, +1.480831,0x22,, +1.480918,0x60,, +1.481005,0x22,, +1.481091,0x60,, +1.481178,0x3B,, +1.481265,0x60,, +1.481352,0x22,, +1.481439,0x60,, +1.481526,0x22,, +1.481613,0x60,, +1.4817,0x3B,, +1.481787,0x60,, +1.481874,0x2E,, +1.481961,0xE0,, +1.482048,0x2E,, +1.482135,0xE0,, +1.482222,0x19,, +1.482309,0xF8,, +1.489921,0xA8,, +1.490008,0x01,, +1.490095,0x0C,, +1.490182,0x34,, +1.490269,0x44,, +1.490356,0x2E,, +1.490443,0xD8,, +1.49053,0x2E,, +1.490617,0xE0,, +1.490704,0x2E,, +1.490791,0xD8,, +1.490878,0x22,, +1.490965,0x60,, +1.491052,0x22,, +1.491139,0x60,, +1.491225,0x3B,, +1.491312,0x60,, +1.491399,0x22,, +1.491486,0x60,, +1.491573,0x22,, +1.49166,0x60,, +1.491747,0x3B,, +1.491834,0x60,, +1.491921,0x2E,, +1.492008,0xE0,, +1.492095,0x2E,, +1.492182,0xE0,, +1.492269,0x5B,, +1.492356,0x58,, +1.499989,0xA8,, +1.500076,0x01,, +1.500162,0x0C,, +1.500249,0x33,, +1.500336,0x90,, +1.500423,0x2E,, +1.50051,0xD8,, +1.500597,0x2E,, +1.500684,0xE0,, +1.500771,0x2E,, +1.500858,0xD8,, +1.500945,0x22,, +1.501032,0x60,, +1.501119,0x22,, +1.501206,0x60,, +1.501293,0x3B,, +1.50138,0x60,, +1.501467,0x22,, +1.501554,0x60,, +1.50164,0x22,, +1.501727,0x60,, +1.501814,0x3B,, +1.501901,0x60,, +1.501988,0x2E,, +1.502075,0xE0,, +1.502162,0x2E,, +1.502249,0xE0,, +1.502336,0x38,, +1.502423,0x63,, +1.510036,0xA8,, +1.510123,0x01,, +1.51021,0x0C,, +1.510297,0x32,, +1.510384,0xE8,, +1.510471,0x2E,, +1.510557,0xD8,, +1.510644,0x2E,, +1.510731,0xE0,, +1.510818,0x2E,, +1.510905,0xD8,, +1.510992,0x22,, +1.511079,0x60,, +1.511166,0x22,, +1.511253,0x60,, +1.51134,0x3B,, +1.511427,0x60,, +1.511514,0x22,, +1.511601,0x60,, +1.511688,0x22,, +1.511775,0x60,, +1.511862,0x3B,, +1.511948,0x60,, +1.512035,0x2E,, +1.512122,0xE0,, +1.512209,0x2E,, +1.512296,0xE0,, +1.512383,0x86,, +1.51247,0x19,, +1.520082,0xA8,, +1.520169,0x01,, +1.520256,0x0C,, +1.520343,0x32,, +1.52043,0x14,, +1.520517,0x2E,, +1.520604,0xD8,, +1.520691,0x2E,, +1.520778,0xE4,, +1.520865,0x2E,, +1.520952,0xD8,, +1.521039,0x22,, +1.521126,0x60,, +1.521213,0x22,, +1.5213,0x60,, +1.521387,0x3B,, +1.521473,0x60,, +1.52156,0x22,, +1.521647,0x60,, +1.521734,0x22,, +1.521821,0x60,, +1.521908,0x3B,, +1.521995,0x60,, +1.522082,0x2E,, +1.522169,0xE0,, +1.522256,0x2E,, +1.522343,0xE0,, +1.52243,0x50,, +1.522517,0xE3,, +1.53013,0xA8,, +1.530217,0x01,, +1.530304,0x0C,, +1.530391,0x31,, +1.530478,0x24,, +1.530565,0x2E,, +1.530652,0xD8,, +1.530738,0x2E,, +1.530825,0xE4,, +1.530912,0x2E,, +1.530999,0xD8,, +1.531086,0x22,, +1.531173,0x60,, +1.53126,0x22,, +1.531347,0x60,, +1.531434,0x3B,, +1.531521,0x60,, +1.531608,0x22,, +1.531695,0x60,, +1.531782,0x22,, +1.531869,0x60,, +1.531956,0x3B,, +1.532043,0x60,, +1.53213,0x2E,, +1.532216,0xE0,, +1.532303,0x2E,, +1.53239,0xE0,, +1.532477,0x59,, +1.532564,0x7C,, +1.540177,0xA8,, +1.540264,0x01,, +1.540351,0x0C,, +1.540438,0x30,, +1.540525,0x34,, +1.540612,0x2E,, +1.540699,0xD8,, +1.540786,0x2E,, +1.540873,0xE4,, +1.54096,0x2E,, +1.541047,0xD8,, +1.541134,0x22,, +1.541221,0x60,, +1.541308,0x22,, +1.541394,0x60,, +1.541481,0x3B,, +1.541568,0x60,, +1.541655,0x22,, +1.541742,0x60,, +1.541829,0x22,, +1.541916,0x60,, +1.542003,0x3B,, +1.54209,0x60,, +1.542177,0x2E,, +1.542264,0xE0,, +1.542351,0x2E,, +1.542438,0xE0,, +1.542525,0x5E,, +1.542612,0x09,, +1.550224,0xA8,, +1.550311,0x01,, +1.550398,0x0C,, +1.550485,0x2F,, +1.550572,0x5C,, +1.550659,0x2E,, +1.550746,0xD8,, +1.550833,0x2E,, +1.55092,0xE4,, +1.551007,0x2E,, +1.551094,0xD8,, +1.551181,0x22,, +1.551268,0x60,, +1.551354,0x22,, +1.551441,0x60,, +1.551528,0x3B,, +1.551615,0x60,, +1.551702,0x22,, +1.551789,0x60,, +1.551876,0x22,, +1.551963,0x60,, +1.55205,0x3B,, +1.552137,0x60,, +1.552224,0x2E,, +1.552311,0xE0,, +1.552398,0x2E,, +1.552485,0xE0,, +1.552572,0x8A,, +1.552659,0x65,, +1.560272,0xA8,, +1.560359,0x01,, +1.560446,0x0C,, +1.560533,0x2E,, +1.56062,0xA0,, +1.560707,0x2E,, +1.560794,0xD8,, +1.560881,0x2E,, +1.560968,0xE0,, +1.561055,0x2E,, +1.561141,0xD8,, +1.561228,0x22,, +1.561315,0x60,, +1.561402,0x22,, +1.561489,0x60,, +1.561576,0x3B,, +1.561663,0x60,, +1.56175,0x22,, +1.561837,0x60,, +1.561924,0x22,, +1.562011,0x60,, +1.562098,0x3B,, +1.562185,0x60,, +1.562272,0x2E,, +1.562359,0xE0,, +1.562446,0x2E,, +1.562533,0xE0,, +1.56262,0x6B,, +1.562706,0x9C,, +1.570319,0xA8,, +1.570406,0x01,, +1.570493,0x0C,, +1.57058,0x2D,, +1.570667,0xE4,, +1.570754,0x2E,, +1.570841,0xD8,, +1.570928,0x2E,, +1.571015,0xE4,, +1.571102,0x2E,, +1.571189,0xD8,, +1.571276,0x22,, +1.571363,0x60,, +1.57145,0x22,, +1.571537,0x60,, +1.571624,0x3B,, +1.571711,0x60,, +1.571797,0x22,, +1.571884,0x60,, +1.571971,0x22,, +1.572058,0x60,, +1.572145,0x3B,, +1.572232,0x60,, +1.572319,0x2E,, +1.572406,0xE0,, +1.572493,0x2E,, +1.57258,0xE0,, +1.572667,0x3F,, +1.572754,0x53,, +1.580366,0xA8,, +1.580453,0x01,, +1.58054,0x0C,, +1.580627,0x2D,, +1.580714,0x30,, +1.580801,0x2E,, +1.580888,0xD8,, +1.580975,0x2E,, +1.581062,0xE0,, +1.581149,0x2E,, +1.581236,0xD8,, +1.581323,0x22,, +1.58141,0x60,, +1.581497,0x22,, +1.581584,0x60,, +1.581671,0x3B,, +1.581757,0x60,, +1.581844,0x22,, +1.581931,0x60,, +1.582018,0x22,, +1.582105,0x60,, +1.582192,0x3B,, +1.582279,0x60,, +1.582366,0x2E,, +1.582453,0xE0,, +1.58254,0x2E,, +1.582627,0xE0,, +1.582714,0x91,, +1.582801,0x7E,, +1.590414,0xA8,, +1.590501,0x01,, +1.590588,0x0C,, +1.590675,0x2C,, +1.590762,0x90,, +1.590849,0x2E,, +1.590936,0xD8,, +1.591023,0x2E,, +1.591109,0xE4,, +1.591196,0x2E,, +1.591283,0xD8,, +1.59137,0x22,, +1.591457,0x60,, +1.591544,0x22,, +1.591631,0x60,, +1.591718,0x3B,, +1.591805,0x60,, +1.591892,0x22,, +1.591979,0x60,, +1.592066,0x22,, +1.592153,0x60,, +1.59224,0x3B,, +1.592327,0x60,, +1.592414,0x2E,, +1.592501,0xE0,, +1.592587,0x2E,, +1.592674,0xE0,, +1.592761,0x1D,, +1.592848,0x1F,, +1.600971,0xA8,, +1.601058,0x01,, +1.601145,0x0C,, +1.601232,0x2B,, +1.601319,0xE0,, +1.601406,0x2E,, +1.601493,0xD4,, +1.60158,0x2E,, +1.601667,0xE0,, +1.601754,0x2E,, +1.601841,0xD8,, +1.601928,0x22,, +1.602015,0x60,, +1.602102,0x22,, +1.602189,0x60,, +1.602276,0x3B,, +1.602362,0x60,, +1.602449,0x22,, +1.602536,0x60,, +1.602623,0x22,, +1.60271,0x60,, +1.602797,0x3B,, +1.602884,0x60,, +1.602971,0x2E,, +1.603058,0xE0,, +1.603145,0x2E,, +1.603232,0xE0,, +1.603319,0xE9,, +1.603406,0x86,, +1.611006,0xA8,, +1.611093,0x01,, +1.61118,0x0C,, +1.611266,0x2B,, +1.611353,0x54,, +1.61144,0x2E,, +1.611527,0xD8,, +1.611614,0x2E,, +1.611701,0xE0,, +1.611788,0x2E,, +1.611875,0xD8,, +1.611962,0x22,, +1.612049,0x60,, +1.612136,0x22,, +1.612223,0x60,, +1.61231,0x3B,, +1.612397,0x60,, +1.612484,0x22,, +1.612571,0x60,, +1.612658,0x22,, +1.612744,0x60,, +1.612831,0x3B,, +1.612918,0x60,, +1.613005,0x2E,, +1.613092,0xE0,, +1.613179,0x2E,, +1.613266,0xE0,, +1.613353,0x06,, +1.61344,0x4D,, +1.621008,0xA8,, +1.621095,0x01,, +1.621182,0x0C,, +1.621269,0x2A,, +1.621356,0xAC,, +1.621443,0x2E,, +1.62153,0xD8,, +1.621617,0x2E,, +1.621703,0xE0,, +1.62179,0x2E,, +1.621877,0xD8,, +1.621964,0x22,, +1.622051,0x60,, +1.622138,0x22,, +1.622225,0x60,, +1.622312,0x3B,, +1.622399,0x60,, +1.622486,0x22,, +1.622573,0x60,, +1.62266,0x22,, +1.622747,0x60,, +1.622834,0x3B,, +1.622921,0x60,, +1.623008,0x2E,, +1.623095,0xE0,, +1.623181,0x2E,, +1.623268,0xE0,, +1.623355,0x2B,, +1.623442,0xA6,, +1.631011,0xA8,, +1.631098,0x01,, +1.631185,0x0C,, +1.631272,0x2A,, +1.631359,0x24,, +1.631446,0x2E,, +1.631533,0xD8,, +1.631619,0x2E,, +1.631706,0xE0,, +1.631793,0x2E,, +1.63188,0xD8,, +1.631967,0x22,, +1.632054,0x60,, +1.632141,0x22,, +1.632228,0x60,, +1.632315,0x3B,, +1.632402,0x60,, +1.632489,0x22,, +1.632576,0x60,, +1.632663,0x22,, +1.63275,0x60,, +1.632837,0x3B,, +1.632924,0x60,, +1.63301,0x2E,, +1.633097,0xE0,, +1.633184,0x2E,, +1.633271,0xE0,, +1.633358,0xA0,, +1.633445,0x0C,, +1.641014,0xA8,, +1.641101,0x01,, +1.641188,0x0C,, +1.641275,0x29,, +1.641362,0x8C,, +1.641449,0x2E,, +1.641536,0xD8,, +1.641623,0x2E,, +1.64171,0xE4,, +1.641797,0x2E,, +1.641884,0xD8,, +1.641971,0x22,, +1.642057,0x60,, +1.642144,0x22,, +1.642231,0x60,, +1.642318,0x3B,, +1.642405,0x60,, +1.642492,0x22,, +1.642579,0x60,, +1.642666,0x22,, +1.642753,0x60,, +1.64284,0x3B,, +1.642927,0x60,, +1.643014,0x2E,, +1.643101,0xE0,, +1.643188,0x2E,, +1.643275,0xE0,, +1.643362,0x5A,, +1.643449,0x50,, +1.651016,0xA8,, +1.651103,0x01,, +1.65119,0x0C,, +1.651277,0x28,, +1.651364,0xEC,, +1.651451,0x2E,, +1.651538,0xD8,, +1.651625,0x2E,, +1.651712,0xE0,, +1.651799,0x2E,, +1.651886,0xD8,, +1.651973,0x22,, +1.652059,0x60,, +1.652146,0x22,, +1.652233,0x60,, +1.65232,0x3B,, +1.652407,0x60,, +1.652494,0x22,, +1.652581,0x60,, +1.652668,0x22,, +1.652755,0x60,, +1.652842,0x3B,, +1.652929,0x60,, +1.653016,0x2E,, +1.653103,0xE0,, +1.65319,0x2E,, +1.653277,0xE0,, +1.653364,0x84,, +1.653451,0x78,, +1.661019,0xA8,, +1.661106,0x01,, +1.661193,0x0C,, +1.66128,0x28,, +1.661366,0x5C,, +1.661453,0x2E,, +1.66154,0xD8,, +1.661627,0x2E,, +1.661714,0xE0,, +1.661801,0x2E,, +1.661888,0xD8,, +1.661975,0x22,, +1.662062,0x60,, +1.662149,0x22,, +1.662236,0x60,, +1.662323,0x3B,, +1.66241,0x60,, +1.662497,0x22,, +1.662584,0x60,, +1.662671,0x22,, +1.662758,0x60,, +1.662845,0x3B,, +1.662931,0x60,, +1.663018,0x2E,, +1.663105,0xE0,, +1.663192,0x2E,, +1.663279,0xE0,, +1.663366,0x47,, +1.663453,0x73,, +1.671022,0xA8,, +1.671109,0x01,, +1.671196,0x0C,, +1.671282,0x27,, +1.671369,0xD8,, +1.671456,0x2E,, +1.671543,0xD8,, +1.67163,0x2E,, +1.671717,0xE0,, +1.671804,0x2E,, +1.671891,0xD8,, +1.671978,0x22,, +1.672065,0x60,, +1.672152,0x22,, +1.672239,0x60,, +1.672326,0x3B,, +1.672413,0x60,, +1.6725,0x22,, +1.672587,0x60,, +1.672674,0x22,, +1.672761,0x60,, +1.672847,0x3B,, +1.672934,0x60,, +1.673021,0x2E,, +1.673108,0xE0,, +1.673195,0x2E,, +1.673282,0xE0,, +1.673369,0x7D,, +1.673456,0xDF,, +1.681025,0xA8,, +1.681112,0x01,, +1.681199,0x0C,, +1.681286,0x27,, +1.681373,0x54,, +1.681459,0x2E,, +1.681546,0xD8,, +1.681633,0x2E,, +1.68172,0xE4,, +1.681807,0x2E,, +1.681894,0xD8,, +1.681981,0x22,, +1.682068,0x60,, +1.682155,0x22,, +1.682242,0x60,, +1.682329,0x3B,, +1.682416,0x60,, +1.682503,0x22,, +1.68259,0x60,, +1.682677,0x22,, +1.682764,0x60,, +1.682851,0x3B,, +1.682937,0x60,, +1.683024,0x2E,, +1.683111,0xE0,, +1.683198,0x2E,, +1.683285,0xE0,, +1.683372,0x3A,, +1.683459,0x67,, +1.69104,0xA8,, +1.691127,0x01,, +1.691214,0x0C,, +1.691301,0x26,, +1.691388,0xE4,, +1.691475,0x2E,, +1.691562,0xD8,, +1.691649,0x2E,, +1.691736,0xE4,, +1.691823,0x2E,, +1.69191,0xD8,, +1.691997,0x22,, +1.692084,0x60,, +1.69217,0x22,, +1.692257,0x60,, +1.692344,0x3B,, +1.692431,0x60,, +1.692518,0x22,, +1.692605,0x60,, +1.692692,0x22,, +1.692779,0x60,, +1.692866,0x3B,, +1.692953,0x60,, +1.69304,0x2E,, +1.693127,0xE0,, +1.693214,0x2E,, +1.693301,0xE0,, +1.693388,0xCE,, +1.693475,0x6F,, +1.70103,0xA8,, +1.701117,0x01,, +1.701204,0x0C,, +1.701291,0x26,, +1.701378,0x70,, +1.701465,0x2E,, +1.701552,0xD8,, +1.701639,0x2E,, +1.701725,0xE0,, +1.701812,0x2E,, +1.701899,0xD8,, +1.701986,0x22,, +1.702073,0x60,, +1.70216,0x22,, +1.702247,0x60,, +1.702334,0x3B,, +1.702421,0x60,, +1.702508,0x22,, +1.702595,0x60,, +1.702682,0x22,, +1.702769,0x60,, +1.702856,0x3B,, +1.702943,0x60,, +1.70303,0x2E,, +1.703116,0xE0,, +1.703203,0x2E,, +1.70329,0xE0,, +1.703377,0xA1,, +1.703464,0x9A,, +1.711033,0xA8,, +1.71112,0x01,, +1.711207,0x0C,, +1.711294,0x26,, +1.711381,0x00,, +1.711468,0x2E,, +1.711555,0xD8,, +1.711642,0x2E,, +1.711729,0xE4,, +1.711815,0x2E,, +1.711902,0xD8,, +1.711989,0x22,, +1.712076,0x60,, +1.712163,0x22,, +1.71225,0x60,, +1.712337,0x3B,, +1.712424,0x60,, +1.712511,0x22,, +1.712598,0x60,, +1.712685,0x22,, +1.712772,0x60,, +1.712859,0x3B,, +1.712946,0x60,, +1.713033,0x2E,, +1.71312,0xE0,, +1.713207,0x2E,, +1.713294,0xE0,, +1.71338,0x78,, +1.713467,0xC7,, +1.721036,0xA8,, +1.721123,0x01,, +1.72121,0x0C,, +1.721297,0x25,, +1.721384,0x7C,, +1.72147,0x2E,, +1.721557,0xD8,, +1.721644,0x2E,, +1.721731,0xE4,, +1.721818,0x2E,, +1.721905,0xD8,, +1.721992,0x22,, +1.722079,0x60,, +1.722166,0x22,, +1.722253,0x60,, +1.72234,0x3B,, +1.722427,0x60,, +1.722514,0x22,, +1.722601,0x60,, +1.722688,0x22,, +1.722775,0x60,, +1.722862,0x3B,, +1.722948,0x60,, +1.723035,0x2E,, +1.723122,0xE0,, +1.723209,0x2E,, +1.723296,0xE0,, +1.723383,0x2C,, +1.72347,0xB6,, +1.731079,0xA8,, +1.731165,0x01,, +1.731252,0x0C,, +1.731339,0x24,, +1.731426,0xF4,, +1.731513,0x2E,, +1.7316,0xD8,, +1.731687,0x2E,, +1.731774,0xE0,, +1.731861,0x2E,, +1.731948,0xD8,, +1.732035,0x22,, +1.732122,0x60,, +1.732209,0x22,, +1.732296,0x60,, +1.732383,0x3B,, +1.73247,0x60,, +1.732557,0x22,, +1.732643,0x60,, +1.73273,0x22,, +1.732817,0x60,, +1.732904,0x3B,, +1.732991,0x60,, +1.733078,0x2E,, +1.733165,0xE0,, +1.733252,0x2E,, +1.733339,0xE0,, +1.733426,0xD8,, +1.733513,0x00,, +1.741233,0xA8,, +1.74132,0x01,, +1.741407,0x0C,, +1.741494,0x24,, +1.741581,0x7C,, +1.741668,0x2E,, +1.741755,0xD8,, +1.741842,0x2E,, +1.741928,0xE4,, +1.742015,0x2E,, +1.742102,0xD8,, +1.742189,0x22,, +1.742276,0x60,, +1.742363,0x22,, +1.74245,0x60,, +1.742537,0x3B,, +1.742624,0x60,, +1.742711,0x22,, +1.742798,0x60,, +1.742885,0x22,, +1.742972,0x60,, +1.743059,0x3B,, +1.743146,0x60,, +1.743233,0x2E,, +1.74332,0xE0,, +1.743406,0x2E,, +1.743493,0xE0,, +1.74358,0x1B,, +1.743667,0xB5,, +1.751215,0xA8,, +1.751302,0x01,, +1.751389,0x0C,, +1.751476,0x24,, +1.751563,0x10,, +1.751649,0x2E,, +1.751736,0xD8,, +1.751823,0x2E,, +1.75191,0xE0,, +1.751997,0x2E,, +1.752084,0xD8,, +1.752171,0x22,, +1.752258,0x60,, +1.752345,0x22,, +1.752432,0x60,, +1.752519,0x3B,, +1.752606,0x60,, +1.752693,0x22,, +1.75278,0x60,, +1.752867,0x22,, +1.752954,0x60,, +1.753041,0x3B,, +1.753127,0x60,, +1.753214,0x2E,, +1.753301,0xE0,, +1.753388,0x2E,, +1.753475,0xE0,, +1.753562,0x6E,, +1.753649,0xA8,, +1.761233,0xA8,, +1.76132,0x01,, +1.761406,0x0C,, +1.761493,0x23,, +1.76158,0xB0,, +1.761667,0x2E,, +1.761754,0xD8,, +1.761841,0x2E,, +1.761928,0xE0,, +1.762015,0x2E,, +1.762102,0xD8,, +1.762189,0x22,, +1.762276,0x60,, +1.762363,0x22,, +1.76245,0x60,, +1.762537,0x3B,, +1.762624,0x60,, +1.762711,0x22,, +1.762798,0x60,, +1.762884,0x22,, +1.762971,0x60,, +1.763058,0x3B,, +1.763145,0x60,, +1.763232,0x2E,, +1.763319,0xE0,, +1.763406,0x2E,, +1.763493,0xE0,, +1.76358,0x18,, +1.763667,0xDC,, +1.771268,0xA8,, +1.771355,0x01,, +1.771442,0x0C,, +1.771529,0x23,, +1.771616,0x60,, +1.771703,0x2E,, +1.77179,0xD8,, +1.771877,0x2E,, +1.771964,0xE0,, +1.772051,0x2E,, +1.772138,0xD8,, +1.772224,0x22,, +1.772311,0x60,, +1.772398,0x22,, +1.772485,0x60,, +1.772572,0x3B,, +1.772659,0x60,, +1.772746,0x22,, +1.772833,0x60,, +1.77292,0x22,, +1.773007,0x60,, +1.773094,0x3B,, +1.773181,0x60,, +1.773268,0x2E,, +1.773355,0xE0,, +1.773442,0x2E,, +1.773529,0xE0,, +1.773616,0x7A,, +1.773703,0xE3,, +1.781316,0xA8,, +1.781403,0x01,, +1.78149,0x0C,, +1.781577,0x23,, +1.781664,0x1C,, +1.781751,0x2E,, +1.781838,0xD8,, +1.781925,0x2E,, +1.782011,0xE4,, +1.782098,0x2E,, +1.782185,0xD8,, +1.782272,0x22,, +1.782359,0x60,, +1.782446,0x22,, +1.782533,0x60,, +1.78262,0x3B,, +1.782707,0x60,, +1.782794,0x22,, +1.782881,0x60,, +1.782968,0x22,, +1.783055,0x60,, +1.783142,0x3B,, +1.783229,0x60,, +1.783316,0x2E,, +1.783403,0xE0,, +1.783489,0x2E,, +1.783576,0xE0,, +1.783663,0x3F,, +1.78375,0x88,, +1.791408,0xA8,, +1.791495,0x01,, +1.791582,0x0C,, +1.791669,0x22,, +1.791755,0xE4,, +1.791842,0x2E,, +1.791929,0xD8,, +1.792016,0x2E,, +1.792103,0xE4,, +1.79219,0x2E,, +1.792277,0xD8,, +1.792364,0x22,, +1.792451,0x60,, +1.792538,0x22,, +1.792625,0x60,, +1.792712,0x3B,, +1.792799,0x60,, +1.792886,0x22,, +1.792973,0x60,, +1.79306,0x22,, +1.793147,0x60,, +1.793233,0x3B,, +1.79332,0x60,, +1.793407,0x2E,, +1.793494,0xE0,, +1.793581,0x2E,, +1.793668,0xE0,, +1.793755,0x12,, +1.793842,0x63,, +1.801435,0xA8,, +1.801522,0x01,, +1.801609,0x0C,, +1.801696,0x22,, +1.801783,0xA8,, +1.80187,0x2E,, +1.801957,0xD8,, +1.802044,0x2E,, +1.802131,0xE4,, +1.802218,0x2E,, +1.802305,0xD8,, +1.802391,0x22,, +1.802478,0x60,, +1.802565,0x22,, +1.802652,0x60,, +1.802739,0x3B,, +1.802826,0x60,, +1.802913,0x22,, +1.803,0x60,, +1.803087,0x22,, +1.803174,0x60,, +1.803261,0x3B,, +1.803348,0x60,, +1.803435,0x2E,, +1.803522,0xE0,, +1.803609,0x2E,, +1.803696,0xE0,, +1.803783,0x4F,, +1.803869,0x8D,, +1.811459,0xA8,, +1.811546,0x01,, +1.811633,0x0C,, +1.81172,0x22,, +1.811806,0x7C,, +1.811893,0x2E,, +1.81198,0xD8,, +1.812067,0x2E,, +1.812154,0xE0,, +1.812241,0x2E,, +1.812328,0xD8,, +1.812415,0x22,, +1.812502,0x60,, +1.812589,0x22,, +1.812676,0x60,, +1.812763,0x3B,, +1.81285,0x60,, +1.812937,0x22,, +1.813024,0x60,, +1.813111,0x22,, +1.813198,0x60,, +1.813284,0x3B,, +1.813371,0x60,, +1.813458,0x2E,, +1.813545,0xE0,, +1.813632,0x2E,, +1.813719,0xE0,, +1.813806,0xE1,, +1.813893,0xA0,, +1.821505,0xA8,, +1.821592,0x01,, +1.821679,0x0C,, +1.821766,0x22,, +1.821853,0x78,, +1.82194,0x2E,, +1.822027,0xD8,, +1.822114,0x2E,, +1.822201,0xE0,, +1.822288,0x2E,, +1.822375,0xD8,, +1.822462,0x22,, +1.822548,0x60,, +1.822635,0x22,, +1.822722,0x60,, +1.822809,0x3B,, +1.822896,0x60,, +1.822983,0x22,, +1.82307,0x60,, +1.823157,0x22,, +1.823244,0x60,, +1.823331,0x3B,, +1.823418,0x60,, +1.823505,0x2E,, +1.823592,0xE0,, +1.823679,0x2E,, +1.823766,0xE0,, +1.823853,0x65,, +1.82394,0xAD,, +1.831553,0xA8,, +1.831639,0x01,, +1.831726,0x0C,, +1.831813,0x22,, +1.8319,0x78,, +1.831987,0x2E,, +1.832074,0xD8,, +1.832161,0x2E,, +1.832248,0xE0,, +1.832335,0x2E,, +1.832422,0xD8,, +1.832509,0x22,, +1.832596,0x60,, +1.832683,0x22,, +1.83277,0x60,, +1.832857,0x3B,, +1.832944,0x60,, +1.833031,0x22,, +1.833117,0x60,, +1.833204,0x22,, +1.833291,0x60,, +1.833378,0x3B,, +1.833465,0x60,, +1.833552,0x2E,, +1.833639,0xE0,, +1.833726,0x2E,, +1.833813,0xE0,, +1.8339,0x65,, +1.833987,0xAD,, +1.841601,0xA8,, +1.841688,0x01,, +1.841775,0x0C,, +1.841862,0x22,, +1.841949,0x78,, +1.842036,0x2E,, +1.842123,0xD8,, +1.84221,0x2E,, +1.842297,0xE0,, +1.842384,0x2E,, +1.84247,0xD8,, +1.842557,0x22,, +1.842644,0x60,, +1.842731,0x22,, +1.842818,0x60,, +1.842905,0x3B,, +1.842992,0x60,, +1.843079,0x22,, +1.843166,0x60,, +1.843253,0x22,, +1.84334,0x60,, +1.843427,0x3B,, +1.843514,0x60,, +1.843601,0x2E,, +1.843688,0xE0,, +1.843775,0x2E,, +1.843862,0xE0,, +1.843948,0x65,, +1.844035,0xAD,, +1.851647,0xA8,, +1.851734,0x01,, +1.851821,0x0C,, +1.851908,0x22,, +1.851995,0x78,, +1.852082,0x2E,, +1.852169,0xD8,, +1.852256,0x2E,, +1.852343,0xE0,, +1.85243,0x2E,, +1.852516,0xD8,, +1.852603,0x22,, +1.85269,0x60,, +1.852777,0x22,, +1.852864,0x60,, +1.852951,0x3B,, +1.853038,0x60,, +1.853125,0x22,, +1.853212,0x60,, +1.853299,0x22,, +1.853386,0x60,, +1.853473,0x3B,, +1.85356,0x60,, +1.853647,0x2E,, +1.853734,0xE0,, +1.853821,0x2E,, +1.853908,0xE0,, +1.853995,0x65,, +1.854081,0xAD,, +1.861695,0xA8,, +1.861782,0x01,, +1.861869,0x0C,, +1.861956,0x22,, +1.862043,0x78,, +1.862129,0x2E,, +1.862216,0xD8,, +1.862303,0x2E,, +1.86239,0xE0,, +1.862477,0x2E,, +1.862564,0xD8,, +1.862651,0x22,, +1.862738,0x60,, +1.862825,0x22,, +1.862912,0x60,, +1.862999,0x3B,, +1.863086,0x60,, +1.863173,0x22,, +1.86326,0x60,, +1.863347,0x22,, +1.863434,0x60,, +1.863521,0x3B,, +1.863607,0x60,, +1.863694,0x2E,, +1.863781,0xE0,, +1.863868,0x2E,, +1.863955,0xE0,, +1.864042,0x65,, +1.864129,0xAD,, +1.871742,0xA8,, +1.871829,0x01,, +1.871916,0x0C,, +1.872003,0x22,, +1.87209,0x78,, +1.872177,0x2E,, +1.872264,0xD8,, +1.872351,0x2E,, +1.872438,0xE4,, +1.872525,0x2E,, +1.872612,0xD8,, +1.872699,0x22,, +1.872786,0x60,, +1.872872,0x22,, +1.872959,0x60,, +1.873046,0x3B,, +1.873133,0x60,, +1.87322,0x22,, +1.873307,0x60,, +1.873394,0x22,, +1.873481,0x60,, +1.873568,0x3B,, +1.873655,0x60,, +1.873742,0x2E,, +1.873829,0xE0,, +1.873916,0x2E,, +1.874003,0xE0,, +1.87409,0x2D,, +1.874177,0xB2,, +1.881789,0xA8,, +1.881876,0x01,, +1.881963,0x0C,, +1.88205,0x22,, +1.882137,0x78,, +1.882224,0x2E,, +1.882311,0xD8,, +1.882398,0x2E,, +1.882485,0xE0,, +1.882572,0x2E,, +1.882659,0xD8,, +1.882746,0x22,, +1.882833,0x60,, +1.882919,0x22,, +1.883006,0x60,, +1.883093,0x3B,, +1.88318,0x60,, +1.883267,0x22,, +1.883354,0x60,, +1.883441,0x22,, +1.883528,0x60,, +1.883615,0x3B,, +1.883702,0x60,, +1.883789,0x2E,, +1.883876,0xE0,, +1.883963,0x2E,, +1.88405,0xE0,, +1.884137,0x65,, +1.884224,0xAD,, +1.891837,0xA8,, +1.891924,0x01,, +1.892011,0x0C,, +1.892098,0x22,, +1.892185,0x78,, +1.892271,0x2E,, +1.892358,0xD8,, +1.892445,0x2E,, +1.892532,0xE0,, +1.892619,0x2E,, +1.892706,0xD8,, +1.892793,0x22,, +1.89288,0x60,, +1.892967,0x22,, +1.893054,0x60,, +1.893141,0x3B,, +1.893228,0x60,, +1.893315,0x22,, +1.893402,0x60,, +1.893489,0x22,, +1.893576,0x60,, +1.893663,0x3B,, +1.893749,0x60,, +1.893836,0x2E,, +1.893923,0xE0,, +1.89401,0x2E,, +1.894097,0xE0,, +1.894184,0x65,, +1.894271,0xAD,, +1.901884,0xA8,, +1.901971,0x01,, +1.902058,0x0C,, +1.902145,0x22,, +1.902232,0x78,, +1.902319,0x2E,, +1.902406,0xD8,, +1.902493,0x2E,, +1.90258,0xE0,, +1.902667,0x2E,, +1.902754,0xD8,, +1.902841,0x22,, +1.902928,0x60,, +1.903015,0x22,, +1.903102,0x60,, +1.903188,0x3B,, +1.903275,0x60,, +1.903362,0x22,, +1.903449,0x60,, +1.903536,0x22,, +1.903623,0x60,, +1.90371,0x3B,, +1.903797,0x60,, +1.903884,0x2E,, +1.903971,0xE0,, +1.904058,0x2E,, +1.904145,0xE0,, +1.904232,0x65,, +1.904319,0xAD,, +1.911932,0xA8,, +1.912018,0x01,, +1.912105,0x0C,, +1.912192,0x22,, +1.912279,0x78,, +1.912366,0x2E,, +1.912453,0xD8,, +1.91254,0x2E,, +1.912627,0xE4,, +1.912714,0x2E,, +1.912801,0xD8,, +1.912888,0x22,, +1.912975,0x60,, +1.913062,0x22,, +1.913149,0x60,, +1.913236,0x3B,, +1.913323,0x60,, +1.91341,0x22,, +1.913496,0x60,, +1.913583,0x22,, +1.91367,0x60,, +1.913757,0x3B,, +1.913844,0x60,, +1.913931,0x2E,, +1.914018,0xE0,, +1.914105,0x2E,, +1.914192,0xE0,, +1.914279,0x2D,, +1.914366,0xB2,, +1.921979,0xA8,, +1.922066,0x01,, +1.922153,0x0C,, +1.92224,0x22,, +1.922327,0x78,, +1.922414,0x2E,, +1.922501,0xD8,, +1.922588,0x2E,, +1.922674,0xE4,, +1.922761,0x2E,, +1.922848,0xD8,, +1.922935,0x22,, +1.923022,0x60,, +1.923109,0x22,, +1.923196,0x60,, +1.923283,0x3B,, +1.92337,0x60,, +1.923457,0x22,, +1.923544,0x60,, +1.923631,0x22,, +1.923718,0x60,, +1.923805,0x3B,, +1.923892,0x60,, +1.923979,0x2E,, +1.924066,0xE0,, +1.924152,0x2E,, +1.924239,0xE0,, +1.924326,0x2D,, +1.924413,0xB2,, +1.932026,0xA8,, +1.932113,0x01,, +1.9322,0x0C,, +1.932287,0x22,, +1.932374,0x78,, +1.932461,0x2E,, +1.932548,0xD8,, +1.932635,0x2E,, +1.932722,0xE0,, +1.932809,0x2E,, +1.932896,0xD8,, +1.932983,0x22,, +1.93307,0x60,, +1.933157,0x22,, +1.933244,0x60,, +1.933331,0x3B,, +1.933418,0x60,, +1.933504,0x22,, +1.933591,0x60,, +1.933678,0x22,, +1.933765,0x60,, +1.933852,0x3B,, +1.933939,0x60,, +1.934026,0x2E,, +1.934113,0xE0,, +1.9342,0x2E,, +1.934287,0xE0,, +1.934374,0x65,, +1.934461,0xAD,, +1.942074,0xA8,, +1.942161,0x01,, +1.942248,0x0C,, +1.942335,0x22,, +1.942422,0x78,, +1.942509,0x2E,, +1.942595,0xD8,, +1.942682,0x2E,, +1.942769,0xE0,, +1.942856,0x2E,, +1.942943,0xD8,, +1.94303,0x22,, +1.943117,0x60,, +1.943204,0x22,, +1.943291,0x60,, +1.943378,0x3B,, +1.943465,0x60,, +1.943552,0x22,, +1.943639,0x60,, +1.943726,0x22,, +1.943813,0x60,, +1.9439,0x3B,, +1.943987,0x60,, +1.944073,0x2E,, +1.94416,0xE0,, +1.944247,0x2E,, +1.944334,0xE0,, +1.944421,0x65,, +1.944508,0xAD,, +1.952122,0xA8,, +1.952209,0x01,, +1.952296,0x0C,, +1.952383,0x22,, +1.95247,0x78,, +1.952557,0x2E,, +1.952644,0xD8,, +1.952731,0x2E,, +1.952818,0xE0,, +1.952905,0x2E,, +1.952991,0xD8,, +1.953078,0x22,, +1.953165,0x60,, +1.953252,0x22,, +1.953339,0x60,, +1.953426,0x3B,, +1.953513,0x60,, +1.9536,0x22,, +1.953687,0x60,, +1.953774,0x22,, +1.953861,0x60,, +1.953948,0x3B,, +1.954035,0x60,, +1.954122,0x2E,, +1.954209,0xE0,, +1.954296,0x2E,, +1.954383,0xE0,, +1.95447,0x65,, +1.954556,0xAD,, +1.96217,0xA8,, +1.962257,0x01,, +1.962344,0x0C,, +1.962431,0x22,, +1.962518,0x78,, +1.962605,0x2E,, +1.962692,0xD8,, +1.962779,0x2E,, +1.962866,0xE0,, +1.962952,0x2E,, +1.963039,0xD8,, +1.963126,0x22,, +1.963213,0x60,, +1.9633,0x22,, +1.963387,0x60,, +1.963474,0x3B,, +1.963561,0x60,, +1.963648,0x22,, +1.963735,0x60,, +1.963822,0x22,, +1.963909,0x60,, +1.963996,0x3B,, +1.964083,0x60,, +1.96417,0x2E,, +1.964257,0xE0,, +1.964344,0x2E,, +1.96443,0xE0,, +1.964517,0x65,, +1.964604,0xAD,, +1.972217,0xA8,, +1.972304,0x01,, +1.972391,0x0C,, +1.972478,0x22,, +1.972565,0x78,, +1.972652,0x2E,, +1.972739,0xD8,, +1.972826,0x2E,, +1.972913,0xE0,, +1.973,0x2E,, +1.973087,0xD8,, +1.973173,0x22,, +1.97326,0x60,, +1.973347,0x22,, +1.973434,0x60,, +1.973521,0x3B,, +1.973608,0x60,, +1.973695,0x22,, +1.973782,0x60,, +1.973869,0x22,, +1.973956,0x60,, +1.974043,0x3B,, +1.97413,0x60,, +1.974217,0x2E,, +1.974304,0xE0,, +1.974391,0x2E,, +1.974478,0xE0,, +1.974565,0x65,, +1.974651,0xAD,, +1.982264,0xA8,, +1.982351,0x01,, +1.982438,0x0C,, +1.982525,0x22,, +1.982612,0x78,, +1.982699,0x2E,, +1.982786,0xD8,, +1.982873,0x2E,, +1.98296,0xE0,, +1.983047,0x2E,, +1.983134,0xD8,, +1.983221,0x22,, +1.983308,0x60,, +1.983395,0x22,, +1.983482,0x60,, +1.983569,0x3B,, +1.983656,0x60,, +1.983742,0x22,, +1.983829,0x60,, +1.983916,0x22,, +1.984003,0x60,, +1.98409,0x3B,, +1.984177,0x60,, +1.984264,0x2E,, +1.984351,0xE0,, +1.984438,0x2E,, +1.984525,0xE0,, +1.984612,0x65,, +1.984699,0xAD,, +1.992323,0xA8,, +1.99241,0x01,, +1.992497,0x0C,, +1.992584,0x22,, +1.992671,0x78,, +1.992758,0x2E,, +1.992845,0xD8,, +1.992932,0x2E,, +1.993019,0xE0,, +1.993106,0x2E,, +1.993193,0xD8,, +1.99328,0x22,, +1.993367,0x60,, +1.993454,0x22,, +1.99354,0x60,, +1.993627,0x3B,, +1.993714,0x60,, +1.993801,0x22,, +1.993888,0x60,, +1.993975,0x22,, +1.994062,0x60,, +1.994149,0x3B,, +1.994236,0x60,, +1.994323,0x2E,, +1.99441,0xE0,, +1.994497,0x2E,, +1.994584,0xE0,, +1.994671,0x65,, +1.994758,0xAD,, +2.002371,0xA8,, +2.002458,0x01,, +2.002545,0x0C,, +2.002632,0x22,, +2.002719,0x78,, +2.002806,0x2E,, +2.002892,0xD8,, +2.002979,0x2E,, +2.003066,0xE0,, +2.003153,0x2E,, +2.00324,0xD8,, +2.003327,0x22,, +2.003414,0x60,, +2.003501,0x22,, +2.003588,0x60,, +2.003675,0x3B,, +2.003762,0x60,, +2.003849,0x22,, +2.003936,0x60,, +2.004023,0x22,, +2.00411,0x60,, +2.004197,0x3B,, +2.004283,0x60,, +2.00437,0x2E,, +2.004457,0xE0,, +2.004544,0x2E,, +2.004631,0xE0,, +2.004718,0x65,, +2.004805,0xAD,, +2.012419,0xA8,, +2.012506,0x01,, +2.012592,0x0C,, +2.012679,0x22,, +2.012766,0x78,, +2.012853,0x2E,, +2.01294,0xD8,, +2.013027,0x2E,, +2.013114,0xE0,, +2.013201,0x2E,, +2.013288,0xD8,, +2.013375,0x22,, +2.013462,0x60,, +2.013549,0x22,, +2.013636,0x60,, +2.013723,0x3B,, +2.01381,0x60,, +2.013897,0x22,, +2.013984,0x60,, +2.01407,0x22,, +2.014157,0x60,, +2.014244,0x3B,, +2.014331,0x60,, +2.014418,0x2E,, +2.014505,0xE0,, +2.014592,0x2E,, +2.014679,0xE0,, +2.014766,0x65,, +2.014853,0xAD,, +2.022466,0xA8,, +2.022553,0x01,, +2.02264,0x0C,, +2.022727,0x22,, +2.022814,0x78,, +2.022901,0x2E,, +2.022988,0xD8,, +2.023074,0x2E,, +2.023161,0xE4,, +2.023248,0x2E,, +2.023335,0xD8,, +2.023422,0x22,, +2.023509,0x60,, +2.023596,0x22,, +2.023683,0x60,, +2.02377,0x3B,, +2.023857,0x60,, +2.023944,0x22,, +2.024031,0x60,, +2.024118,0x22,, +2.024205,0x60,, +2.024292,0x3B,, +2.024379,0x60,, +2.024466,0x2E,, +2.024552,0xE0,, +2.024639,0x2E,, +2.024726,0xE0,, +2.024813,0x2D,, +2.0249,0xB2,, +2.032513,0xA8,, +2.0326,0x01,, +2.032687,0x0C,, +2.032774,0x22,, +2.032861,0x94,, +2.032948,0x2E,, +2.033035,0xD8,, +2.033122,0x2E,, +2.033209,0xE0,, +2.033295,0x2E,, +2.033382,0xD4,, +2.033469,0x22,, +2.033556,0x60,, +2.033643,0x22,, +2.03373,0x60,, +2.033817,0x3B,, +2.033904,0x60,, +2.033991,0x22,, +2.034078,0x60,, +2.034165,0x22,, +2.034252,0x60,, +2.034339,0x3B,, +2.034426,0x60,, +2.034513,0x2E,, +2.0346,0xE0,, +2.034687,0x2E,, +2.034774,0xE0,, +2.03486,0x0F,, +2.034947,0xA6,, +2.04256,0xA8,, +2.042647,0x01,, +2.042734,0x0C,, +2.042821,0x22,, +2.042908,0xE0,, +2.042995,0x2E,, +2.043082,0xD8,, +2.043169,0x2E,, +2.043256,0xE0,, +2.043343,0x2E,, +2.04343,0xD4,, +2.043517,0x22,, +2.043604,0x60,, +2.043691,0x22,, +2.043778,0x60,, +2.043864,0x3B,, +2.043951,0x60,, +2.044038,0x22,, +2.044125,0x60,, +2.044212,0x22,, +2.044299,0x60,, +2.044386,0x3B,, +2.044473,0x60,, +2.04456,0x2E,, +2.044647,0xE0,, +2.044734,0x2E,, +2.044821,0xE0,, +2.044908,0x1A,, +2.044995,0xE9,, +2.052608,0xA8,, +2.052695,0x01,, +2.052782,0x0C,, +2.052869,0x23,, +2.052956,0x50,, +2.053043,0x2E,, +2.053129,0xD8,, +2.053216,0x2E,, +2.053303,0xE0,, +2.05339,0x2E,, +2.053477,0xD4,, +2.053564,0x22,, +2.053651,0x60,, +2.053738,0x22,, +2.053825,0x60,, +2.053912,0x3B,, +2.053999,0x60,, +2.054086,0x22,, +2.054173,0x60,, +2.05426,0x22,, +2.054347,0x60,, +2.054434,0x3B,, +2.054521,0x60,, +2.054607,0x2E,, +2.054694,0xE0,, +2.054781,0x2E,, +2.054868,0xE0,, +2.054955,0xEE,, +2.055042,0xE1,, +2.062656,0xA8,, +2.062743,0x01,, +2.06283,0x0C,, +2.062916,0x23,, +2.063003,0xDC,, +2.06309,0x2E,, +2.063177,0xD8,, +2.063264,0x2E,, +2.063351,0xE0,, +2.063438,0x2E,, +2.063525,0xD4,, +2.063612,0x22,, +2.063699,0x60,, +2.063786,0x22,, +2.063873,0x60,, +2.06396,0x3B,, +2.064047,0x60,, +2.064134,0x22,, +2.064221,0x60,, +2.064308,0x22,, +2.064394,0x60,, +2.064481,0x3B,, +2.064568,0x60,, +2.064655,0x2E,, +2.064742,0xE0,, +2.064829,0x2E,, +2.064916,0xE0,, +2.065003,0xE1,, +2.06509,0x46,, +2.072703,0xA8,, +2.07279,0x01,, +2.072877,0x0C,, +2.072964,0x24,, +2.073051,0x5C,, +2.073138,0x2E,, +2.073225,0xD8,, +2.073312,0x2E,, +2.073398,0xE0,, +2.073485,0x2E,, +2.073572,0xD4,, +2.073659,0x22,, +2.073746,0x60,, +2.073833,0x22,, +2.07392,0x60,, +2.074007,0x3B,, +2.074094,0x60,, +2.074181,0x22,, +2.074268,0x60,, +2.074355,0x22,, +2.074442,0x60,, +2.074529,0x3B,, +2.074616,0x60,, +2.074703,0x2E,, +2.074789,0xE0,, +2.074876,0x2E,, +2.074963,0xE0,, +2.07505,0xF7,, +2.075137,0xDE,, +2.08275,0xA8,, +2.082837,0x01,, +2.082924,0x0C,, +2.083011,0x24,, +2.083098,0xDC,, +2.083185,0x2E,, +2.083272,0xD8,, +2.083359,0x2E,, +2.083445,0xE0,, +2.083532,0x2E,, +2.083619,0xD4,, +2.083706,0x22,, +2.083793,0x60,, +2.08388,0x22,, +2.083967,0x60,, +2.084054,0x3B,, +2.084141,0x60,, +2.084228,0x22,, +2.084315,0x60,, +2.084402,0x22,, +2.084489,0x60,, +2.084576,0x3B,, +2.084663,0x60,, +2.08475,0x2E,, +2.084837,0xE0,, +2.084923,0x2E,, +2.08501,0xE0,, +2.085097,0x64,, +2.085184,0x4F,, +2.092797,0xA8,, +2.092884,0x01,, +2.092971,0x0C,, +2.093058,0x25,, +2.093145,0x6C,, +2.093232,0x2E,, +2.093319,0xD8,, +2.093406,0x2E,, +2.093493,0xE0,, +2.09358,0x2E,, +2.093667,0xD4,, +2.093754,0x22,, +2.093841,0x60,, +2.093928,0x22,, +2.094014,0x60,, +2.094101,0x3B,, +2.094188,0x60,, +2.094275,0x22,, +2.094362,0x60,, +2.094449,0x22,, +2.094536,0x60,, +2.094623,0x3B,, +2.09471,0x60,, +2.094797,0x2E,, +2.094884,0xE0,, +2.094971,0x2E,, +2.095058,0xE0,, +2.095145,0x90,, +2.095232,0x47,, +2.102844,0xA8,, +2.102931,0x01,, +2.103018,0x0C,, +2.103105,0x25,, +2.103192,0xF0,, +2.103279,0x2E,, +2.103366,0xD8,, +2.103452,0x2E,, +2.103539,0xE4,, +2.103626,0x2E,, +2.103713,0xD8,, +2.1038,0x22,, +2.103887,0x60,, +2.103974,0x22,, +2.104061,0x60,, +2.104148,0x3B,, +2.104235,0x60,, +2.104322,0x22,, +2.104409,0x60,, +2.104496,0x22,, +2.104583,0x60,, +2.10467,0x3B,, +2.104757,0x60,, +2.104844,0x2E,, +2.10493,0xE0,, +2.105017,0x2E,, +2.105104,0xE0,, +2.105191,0x23,, +2.105278,0x11,, +2.112892,0xA8,, +2.112979,0x01,, +2.113066,0x0C,, +2.113153,0x26,, +2.11324,0x74,, +2.113326,0x2E,, +2.113413,0xD8,, +2.1135,0x2E,, +2.113587,0xE0,, +2.113674,0x2E,, +2.113761,0xD8,, +2.113848,0x22,, +2.113935,0x60,, +2.114022,0x22,, +2.114109,0x60,, +2.114196,0x3B,, +2.114283,0x60,, +2.11437,0x22,, +2.114457,0x60,, +2.114544,0x22,, +2.114631,0x60,, +2.114718,0x3B,, +2.114804,0x60,, +2.114891,0x2E,, +2.114978,0xE0,, +2.115065,0x2E,, +2.115152,0xE0,, +2.115239,0x25,, +2.115326,0x97,, +2.122939,0xA8,, +2.123026,0x01,, +2.123113,0x0C,, +2.1232,0x26,, +2.123286,0xFC,, +2.123373,0x2E,, +2.12346,0xD8,, +2.123547,0x2E,, +2.123634,0xE0,, +2.123721,0x2E,, +2.123808,0xD8,, +2.123895,0x22,, +2.123982,0x60,, +2.124069,0x22,, +2.124156,0x60,, +2.124243,0x3B,, +2.12433,0x60,, +2.124417,0x22,, +2.124504,0x60,, +2.124591,0x22,, +2.124678,0x60,, +2.124764,0x3B,, +2.124851,0x60,, +2.124938,0x2E,, +2.125025,0xE0,, +2.125112,0x2E,, +2.125199,0xE0,, +2.125286,0xAE,, +2.125373,0x3D,, +2.132987,0xA8,, +2.133074,0x01,, +2.133161,0x0C,, +2.133248,0x27,, +2.133335,0x80,, +2.133422,0x2E,, +2.133509,0xD8,, +2.133596,0x2E,, +2.133683,0xE4,, +2.13377,0x2E,, +2.133857,0xD4,, +2.133943,0x22,, +2.13403,0x60,, +2.134117,0x22,, +2.134204,0x60,, +2.134291,0x3B,, +2.134378,0x60,, +2.134465,0x22,, +2.134552,0x60,, +2.134639,0x22,, +2.134726,0x60,, +2.134813,0x3B,, +2.1349,0x60,, +2.134987,0x2E,, +2.135074,0xE0,, +2.135161,0x2E,, +2.135248,0xE0,, +2.135335,0x18,, +2.135421,0xCD,, +2.143034,0xA8,, +2.143121,0x01,, +2.143208,0x0C,, +2.143295,0x28,, +2.143382,0x1C,, +2.143469,0x2E,, +2.143556,0xD8,, +2.143643,0x2E,, +2.143729,0xE4,, +2.143816,0x2E,, +2.143903,0xD8,, +2.14399,0x22,, +2.144077,0x60,, +2.144164,0x22,, +2.144251,0x60,, +2.144338,0x3B,, +2.144425,0x60,, +2.144512,0x22,, +2.144599,0x60,, +2.144686,0x22,, +2.144773,0x60,, +2.14486,0x3B,, +2.144947,0x60,, +2.145034,0x2E,, +2.145121,0xE0,, +2.145208,0x2E,, +2.145294,0xE0,, +2.145381,0xCE,, +2.145468,0xB4,, +2.153101,0xA8,, +2.153188,0x01,, +2.153275,0x0C,, +2.153362,0x28,, +2.153449,0xAC,, +2.153536,0x2E,, +2.153623,0xD8,, +2.15371,0x2E,, +2.153797,0xE4,, +2.153884,0x2E,, +2.153971,0xD8,, +2.154057,0x22,, +2.154144,0x60,, +2.154231,0x22,, +2.154318,0x60,, +2.154405,0x3B,, +2.154492,0x60,, +2.154579,0x22,, +2.154666,0x60,, +2.154753,0x22,, +2.15484,0x60,, +2.154927,0x3B,, +2.155014,0x60,, +2.155101,0x2E,, +2.155188,0xE0,, +2.155275,0x2E,, +2.155362,0xE0,, +2.155449,0x0D,, +2.155535,0xBF,, +2.163131,0xA8,, +2.163218,0x01,, +2.163305,0x0C,, +2.163392,0x29,, +2.163479,0x50,, +2.163566,0x2E,, +2.163652,0xD8,, +2.163739,0x2E,, +2.163826,0xE0,, +2.163913,0x2E,, +2.164,0xD8,, +2.164087,0x22,, +2.164174,0x60,, +2.164261,0x22,, +2.164348,0x60,, +2.164435,0x3B,, +2.164522,0x60,, +2.164609,0x22,, +2.164696,0x60,, +2.164783,0x22,, +2.16487,0x60,, +2.164957,0x3B,, +2.165043,0x60,, +2.16513,0x2E,, +2.165217,0xE0,, +2.165304,0x2E,, +2.165391,0xE0,, +2.165478,0xEC,, +2.165565,0x46,, +2.173198,0xA8,, +2.173285,0x01,, +2.173371,0x0C,, +2.173458,0x2A,, +2.173545,0x00,, +2.173632,0x2E,, +2.173719,0xD8,, +2.173806,0x2E,, +2.173893,0xE0,, +2.17398,0x2E,, +2.174067,0xD8,, +2.174154,0x22,, +2.174241,0x60,, +2.174328,0x22,, +2.174415,0x60,, +2.174502,0x3B,, +2.174589,0x60,, +2.174676,0x22,, +2.174763,0x60,, +2.174849,0x22,, +2.174936,0x60,, +2.175023,0x3B,, +2.17511,0x60,, +2.175197,0x2E,, +2.175284,0xE0,, +2.175371,0x2E,, +2.175458,0xE0,, +2.175545,0x44,, +2.175632,0xED,, +2.183244,0xA8,, +2.183331,0x01,, +2.183418,0x0C,, +2.183505,0x2A,, +2.183592,0x8C,, +2.183679,0x2E,, +2.183766,0xD8,, +2.183853,0x2E,, +2.18394,0xE0,, +2.184027,0x2E,, +2.184114,0xD8,, +2.1842,0x22,, +2.184287,0x60,, +2.184374,0x22,, +2.184461,0x60,, +2.184548,0x3B,, +2.184635,0x60,, +2.184722,0x22,, +2.184809,0x60,, +2.184896,0x22,, +2.184983,0x60,, +2.18507,0x3B,, +2.185157,0x60,, +2.185244,0x2E,, +2.185331,0xE0,, +2.185418,0x2E,, +2.185505,0xE0,, +2.185592,0x4B,, +2.185678,0x4A,, +2.193291,0xA8,, +2.193378,0x01,, +2.193465,0x0C,, +2.193552,0x2B,, +2.193639,0x24,, +2.193726,0x2E,, +2.193813,0xD8,, +2.1939,0x2E,, +2.193987,0xE0,, +2.194074,0x2E,, +2.194161,0xD8,, +2.194248,0x22,, +2.194335,0x60,, +2.194421,0x22,, +2.194508,0x60,, +2.194595,0x3B,, +2.194682,0x60,, +2.194769,0x22,, +2.194856,0x60,, +2.194943,0x22,, +2.19503,0x60,, +2.195117,0x3B,, +2.195204,0x60,, +2.195291,0x2E,, +2.195378,0xE0,, +2.195465,0x2E,, +2.195552,0xE0,, +2.195639,0x97,, +2.195726,0x0F,, +2.20334,0xA8,, +2.203427,0x01,, +2.203513,0x0C,, +2.2036,0x2B,, +2.203687,0xC4,, +2.203774,0x2E,, +2.203861,0xD8,, +2.203948,0x2E,, +2.204035,0xE4,, +2.204122,0x2E,, +2.204209,0xD8,, +2.204296,0x22,, +2.204383,0x60,, +2.20447,0x22,, +2.204557,0x60,, +2.204644,0x3B,, +2.204731,0x60,, +2.204818,0x22,, +2.204905,0x60,, +2.204991,0x22,, +2.205078,0x60,, +2.205165,0x3B,, +2.205252,0x60,, +2.205339,0x2E,, +2.205426,0xE0,, +2.205513,0x2E,, +2.2056,0xE0,, +2.205687,0xED,, +2.205774,0xB5,, +2.213388,0xA8,, +2.213475,0x01,, +2.213562,0x0C,, +2.213649,0x2C,, +2.213736,0x58,, +2.213823,0x2E,, +2.21391,0xD8,, +2.213997,0x2E,, +2.214083,0xE0,, +2.21417,0x2E,, +2.214257,0xD8,, +2.214344,0x22,, +2.214431,0x60,, +2.214518,0x22,, +2.214605,0x60,, +2.214692,0x3B,, +2.214779,0x60,, +2.214866,0x22,, +2.214953,0x60,, +2.21504,0x22,, +2.215127,0x60,, +2.215214,0x3B,, +2.215301,0x60,, +2.215388,0x2E,, +2.215475,0xE0,, +2.215562,0x2E,, +2.215648,0xE0,, +2.215735,0x1F,, +2.215822,0x72,, +2.223436,0xA8,, +2.223523,0x01,, +2.22361,0x0C,, +2.223696,0x2D,, +2.223783,0x04,, +2.22387,0x2E,, +2.223957,0xD8,, +2.224044,0x2E,, +2.224131,0xE0,, +2.224218,0x2E,, +2.224305,0xD8,, +2.224392,0x22,, +2.224479,0x60,, +2.224566,0x22,, +2.224653,0x60,, +2.22474,0x3B,, +2.224827,0x60,, +2.224914,0x22,, +2.225001,0x60,, +2.225088,0x22,, +2.225174,0x60,, +2.225261,0x3B,, +2.225348,0x60,, +2.225435,0x2E,, +2.225522,0xE0,, +2.225609,0x2E,, +2.225696,0xE0,, +2.225783,0x45,, +2.22587,0xE9,, +2.233483,0xA8,, +2.23357,0x01,, +2.233657,0x0C,, +2.233744,0x2D,, +2.233831,0x98,, +2.233918,0x2E,, +2.234005,0xD8,, +2.234092,0x2E,, +2.234179,0xE4,, +2.234266,0x2E,, +2.234353,0xD8,, +2.23444,0x22,, +2.234527,0x60,, +2.234614,0x22,, +2.234701,0x60,, +2.234788,0x3B,, +2.234875,0x60,, +2.234961,0x22,, +2.235048,0x60,, +2.235135,0x22,, +2.235222,0x60,, +2.235309,0x3B,, +2.235396,0x60,, +2.235483,0x2E,, +2.23557,0xE0,, +2.235657,0x2E,, +2.235744,0xE0,, +2.235831,0x32,, +2.235918,0x27,, +2.243531,0xA8,, +2.243618,0x01,, +2.243705,0x0C,, +2.243792,0x2E,, +2.243879,0x54,, +2.243966,0x2E,, +2.244053,0xD8,, +2.24414,0x2E,, +2.244227,0xE0,, +2.244314,0x2E,, +2.244401,0xD8,, +2.244487,0x22,, +2.244574,0x60,, +2.244661,0x22,, +2.244748,0x60,, +2.244835,0x3B,, +2.244922,0x60,, +2.245009,0x22,, +2.245096,0x60,, +2.245183,0x22,, +2.24527,0x60,, +2.245357,0x3B,, +2.245444,0x60,, +2.245531,0x2E,, +2.245618,0xE0,, +2.245705,0x2E,, +2.245792,0xE0,, +2.245879,0xED,, +2.245966,0x42,, +2.253579,0xA8,, +2.253665,0x01,, +2.253752,0x0C,, +2.253839,0x2E,, +2.253926,0xF4,, +2.254013,0x2E,, +2.2541,0xD8,, +2.254187,0x2E,, +2.254274,0xE0,, +2.254361,0x2E,, +2.254448,0xDC,, +2.254535,0x22,, +2.254622,0x60,, +2.254709,0x22,, +2.254796,0x60,, +2.254883,0x3B,, +2.25497,0x60,, +2.255057,0x22,, +2.255143,0x60,, +2.25523,0x22,, +2.255317,0x60,, +2.255404,0x3B,, +2.255491,0x60,, +2.255578,0x2E,, +2.255665,0xE0,, +2.255752,0x2E,, +2.255839,0xE0,, +2.255926,0x5D,, +2.256013,0xB7,, +2.263627,0xA8,, +2.263714,0x01,, +2.263801,0x0C,, +2.263887,0x2F,, +2.263974,0xB0,, +2.264061,0x2E,, +2.264148,0xD8,, +2.264235,0x2E,, +2.264322,0xE0,, +2.264409,0x2E,, +2.264496,0xD8,, +2.264583,0x22,, +2.26467,0x60,, +2.264757,0x22,, +2.264844,0x60,, +2.264931,0x3B,, +2.265018,0x60,, +2.265105,0x22,, +2.265192,0x60,, +2.265279,0x22,, +2.265365,0x60,, +2.265452,0x3B,, +2.265539,0x60,, +2.265626,0x2E,, +2.265713,0xE0,, +2.2658,0x2E,, +2.265887,0xE0,, +2.265974,0x6C,, +2.266061,0xE9,, +2.273674,0xA8,, +2.273761,0x01,, +2.273848,0x0C,, +2.273935,0x30,, +2.274022,0x6C,, +2.274109,0x2E,, +2.274196,0xD8,, +2.274283,0x2E,, +2.27437,0xE4,, +2.274457,0x2E,, +2.274544,0xD8,, +2.274631,0x22,, +2.274718,0x60,, +2.274805,0x22,, +2.274892,0x60,, +2.274979,0x3B,, +2.275066,0x60,, +2.275153,0x22,, +2.275239,0x60,, +2.275326,0x22,, +2.275413,0x60,, +2.2755,0x3B,, +2.275587,0x60,, +2.275674,0x2E,, +2.275761,0xE0,, +2.275848,0x2E,, +2.275935,0xE0,, +2.276022,0xB7,, +2.276109,0x9C,, +2.28421,0xA8,, +2.284296,0x01,, +2.284383,0x0C,, +2.28447,0x31,, +2.284557,0x30,, +2.284644,0x2E,, +2.284731,0xD8,, +2.284818,0x2E,, +2.284905,0xE0,, +2.284992,0x2E,, +2.285079,0xDC,, +2.285166,0x22,, +2.285253,0x60,, +2.28534,0x22,, +2.285427,0x60,, +2.285514,0x3B,, +2.285601,0x60,, +2.285688,0x22,, +2.285774,0x60,, +2.285861,0x22,, +2.285948,0x60,, +2.286035,0x3B,, +2.286122,0x60,, +2.286209,0x2E,, +2.286296,0xE0,, +2.286383,0x2E,, +2.28647,0xE0,, +2.286557,0xE6,, +2.286644,0x90,, +2.294258,0xA8,, +2.294345,0x01,, +2.294432,0x0C,, +2.294519,0x31,, +2.294606,0xF0,, +2.294693,0x2E,, +2.29478,0xDC,, +2.294867,0x2E,, +2.294954,0xE0,, +2.295041,0x2E,, +2.295127,0xD8,, +2.295214,0x22,, +2.295301,0x60,, +2.295388,0x22,, +2.295475,0x60,, +2.295562,0x3B,, +2.295649,0x60,, +2.295736,0x22,, +2.295823,0x60,, +2.29591,0x22,, +2.295997,0x60,, +2.296084,0x3B,, +2.296171,0x60,, +2.296258,0x2E,, +2.296345,0xE0,, +2.296432,0x2E,, +2.296519,0xE0,, +2.296606,0x90,, +2.296692,0xEA,, +2.30437,0xA8,, +2.304457,0x01,, +2.304544,0x0C,, +2.304631,0x32,, +2.304718,0xA4,, +2.304805,0x2E,, +2.304891,0xD8,, +2.304978,0x2E,, +2.305065,0xE0,, +2.305152,0x2E,, +2.305239,0xD8,, +2.305326,0x22,, +2.305413,0x60,, +2.3055,0x22,, +2.305587,0x60,, +2.305674,0x3B,, +2.305761,0x60,, +2.305848,0x22,, +2.305935,0x60,, +2.306022,0x22,, +2.306109,0x60,, +2.306196,0x3B,, +2.306283,0x60,, +2.306369,0x2E,, +2.306456,0xE0,, +2.306543,0x2E,, +2.30663,0xE0,, +2.306717,0xDB,, +2.306804,0xF7,, +2.314417,0xA8,, +2.314504,0x01,, +2.314591,0x0C,, +2.314678,0x33,, +2.314765,0x3C,, +2.314852,0x2E,, +2.314939,0xD8,, +2.315026,0x2E,, +2.315113,0xE0,, +2.3152,0x2E,, +2.315287,0xD8,, +2.315374,0x22,, +2.315461,0x60,, +2.315548,0x22,, +2.315635,0x60,, +2.315722,0x3B,, +2.315809,0x60,, +2.315895,0x22,, +2.315982,0x60,, +2.316069,0x22,, +2.316156,0x60,, +2.316243,0x3B,, +2.31633,0x60,, +2.316417,0x2E,, +2.316504,0xE0,, +2.316591,0x2E,, +2.316678,0xE0,, +2.316765,0x57,, +2.316852,0x28,, +2.324465,0xA8,, +2.324552,0x01,, +2.324639,0x0C,, +2.324726,0x33,, +2.324813,0xDC,, +2.3249,0x2E,, +2.324987,0xD8,, +2.325074,0x2E,, +2.325161,0xE0,, +2.325247,0x2E,, +2.325334,0xD8,, +2.325421,0x22,, +2.325508,0x60,, +2.325595,0x22,, +2.325682,0x60,, +2.325769,0x3B,, +2.325856,0x60,, +2.325943,0x22,, +2.32603,0x60,, +2.326117,0x22,, +2.326204,0x60,, +2.326291,0x3B,, +2.326378,0x60,, +2.326465,0x2E,, +2.326552,0xE0,, +2.326639,0x2E,, +2.326725,0xE0,, +2.326812,0x65,, +2.326899,0x8D,, +2.334613,0xA8,, +2.3347,0x01,, +2.334787,0x0C,, +2.334874,0x34,, +2.334961,0x84,, +2.335048,0x2E,, +2.335135,0xD8,, +2.335222,0x2E,, +2.335309,0xE4,, +2.335396,0x2E,, +2.335483,0xD8,, +2.33557,0x22,, +2.335657,0x60,, +2.335744,0x22,, +2.335831,0x60,, +2.335917,0x3B,, +2.336004,0x60,, +2.336091,0x22,, +2.336178,0x60,, +2.336265,0x22,, +2.336352,0x60,, +2.336439,0x3B,, +2.336526,0x60,, +2.336613,0x2E,, +2.3367,0xE0,, +2.336787,0x2E,, +2.336874,0xE0,, +2.336961,0x41,, +2.337048,0x0E,, +2.344661,0xA8,, +2.344748,0x01,, +2.344834,0x0C,, +2.344921,0x35,, +2.345008,0x24,, +2.345095,0x2E,, +2.345182,0xD8,, +2.345269,0x2E,, +2.345356,0xE0,, +2.345443,0x2E,, +2.34553,0xDC,, +2.345617,0x22,, +2.345704,0x60,, +2.345791,0x22,, +2.345878,0x60,, +2.345965,0x3B,, +2.346052,0x60,, +2.346139,0x22,, +2.346226,0x60,, +2.346312,0x22,, +2.346399,0x60,, +2.346486,0x3B,, +2.346573,0x60,, +2.34666,0x2E,, +2.346747,0xE0,, +2.346834,0x2E,, +2.346921,0xE0,, +2.347008,0x8E,, +2.347095,0xE7,, +2.354708,0xA8,, +2.354795,0x01,, +2.354882,0x0C,, +2.354969,0x35,, +2.355056,0xC0,, +2.355143,0x2E,, +2.35523,0xD8,, +2.355317,0x2E,, +2.355403,0xE0,, +2.35549,0x2E,, +2.355577,0xDC,, +2.355664,0x22,, +2.355751,0x60,, +2.355838,0x22,, +2.355925,0x60,, +2.356012,0x3B,, +2.356099,0x60,, +2.356186,0x22,, +2.356273,0x60,, +2.35636,0x22,, +2.356447,0x60,, +2.356534,0x3B,, +2.356621,0x60,, +2.356708,0x2E,, +2.356794,0xE0,, +2.356881,0x2E,, +2.356968,0xE0,, +2.357055,0x38,, +2.357142,0x4F,, +2.364787,0xA8,, +2.364874,0x01,, +2.364961,0x0C,, +2.365048,0x36,, +2.365135,0x64,, +2.365222,0x2E,, +2.365309,0xDC,, +2.365396,0x2E,, +2.365483,0xE0,, +2.36557,0x2E,, +2.365657,0xDC,, +2.365744,0x22,, +2.365831,0x60,, +2.365918,0x22,, +2.366005,0x60,, +2.366091,0x3B,, +2.366178,0x60,, +2.366265,0x22,, +2.366352,0x60,, +2.366439,0x22,, +2.366526,0x60,, +2.366613,0x3B,, +2.3667,0x60,, +2.366787,0x2E,, +2.366874,0xE0,, +2.366961,0x2E,, +2.367048,0xE0,, +2.367135,0x71,, +2.367222,0x81,, +2.374835,0xA8,, +2.374922,0x01,, +2.375009,0x0C,, +2.375096,0x37,, +2.375183,0x08,, +2.37527,0x2E,, +2.375356,0xD8,, +2.375443,0x2E,, +2.37553,0xE0,, +2.375617,0x2E,, +2.375704,0xDC,, +2.375791,0x22,, +2.375878,0x60,, +2.375965,0x22,, +2.376052,0x60,, +2.376139,0x3B,, +2.376226,0x60,, +2.376313,0x22,, +2.3764,0x60,, +2.376487,0x22,, +2.376574,0x60,, +2.376661,0x3B,, +2.376748,0x60,, +2.376834,0x2E,, +2.376921,0xE0,, +2.377008,0x2E,, +2.377095,0xE0,, +2.377182,0x1C,, +2.377269,0x3B,, +2.384887,0xA8,, +2.384974,0x01,, +2.385061,0x0C,, +2.385148,0x37,, +2.385235,0xB0,, +2.385322,0x2E,, +2.385409,0xD8,, +2.385496,0x2E,, +2.385583,0xE0,, +2.38567,0x2E,, +2.385757,0xD8,, +2.385844,0x22,, +2.385931,0x60,, +2.386018,0x22,, +2.386104,0x60,, +2.386191,0x3B,, +2.386278,0x60,, +2.386365,0x22,, +2.386452,0x60,, +2.386539,0x22,, +2.386626,0x60,, +2.386713,0x3B,, +2.3868,0x60,, +2.386887,0x2E,, +2.386974,0xE0,, +2.387061,0x2E,, +2.387148,0xE0,, +2.387235,0x84,, +2.387322,0x83,, +2.394935,0xA8,, +2.395022,0x01,, +2.395109,0x0C,, +2.395196,0x38,, +2.395282,0x54,, +2.395369,0x2E,, +2.395456,0xDC,, +2.395543,0x2E,, +2.39563,0xE0,, +2.395717,0x2E,, +2.395804,0xDC,, +2.395891,0x22,, +2.395978,0x60,, +2.396065,0x22,, +2.396152,0x60,, +2.396239,0x3B,, +2.396326,0x60,, +2.396413,0x22,, +2.3965,0x60,, +2.396587,0x22,, +2.396674,0x60,, +2.39676,0x3B,, +2.396847,0x60,, +2.396934,0x2E,, +2.397021,0xE0,, +2.397108,0x2E,, +2.397195,0xE0,, +2.397282,0x3B,, +2.397369,0x28,, +2.404982,0xA8,, +2.405069,0x01,, +2.405156,0x0C,, +2.405243,0x38,, +2.40533,0xE0,, +2.405417,0x2E,, +2.405504,0xD8,, +2.405591,0x2E,, +2.405677,0xE4,, +2.405764,0x2E,, +2.405851,0xDC,, +2.405938,0x22,, +2.406025,0x60,, +2.406112,0x22,, +2.406199,0x60,, +2.406286,0x3B,, +2.406373,0x60,, +2.40646,0x22,, +2.406547,0x60,, +2.406634,0x22,, +2.406721,0x60,, +2.406808,0x3B,, +2.406895,0x60,, +2.406982,0x2E,, +2.407069,0xE0,, +2.407155,0x2E,, +2.407242,0xE0,, +2.407329,0x53,, +2.407416,0x8A,, +2.415029,0xA8,, +2.415116,0x01,, +2.415203,0x0C,, +2.41529,0x39,, +2.415377,0x68,, +2.415464,0x2E,, +2.415551,0xD8,, +2.415638,0x2E,, +2.415725,0xE4,, +2.415812,0x2E,, +2.415899,0xDC,, +2.415986,0x22,, +2.416073,0x60,, +2.41616,0x22,, +2.416247,0x60,, +2.416333,0x3B,, +2.41642,0x60,, +2.416507,0x22,, +2.416594,0x60,, +2.416681,0x22,, +2.416768,0x60,, +2.416855,0x3B,, +2.416942,0x60,, +2.417029,0x2E,, +2.417116,0xE0,, +2.417203,0x2E,, +2.41729,0xE0,, +2.417377,0xEF,, +2.417464,0x23,, +2.425077,0xA8,, +2.425164,0x01,, +2.425251,0x0C,, +2.425338,0x39,, +2.425425,0xE0,, +2.425512,0x2E,, +2.425599,0xD8,, +2.425686,0x2E,, +2.425772,0xE0,, +2.425859,0x2E,, +2.425946,0xDC,, +2.426033,0x22,, +2.42612,0x60,, +2.426207,0x22,, +2.426294,0x60,, +2.426381,0x3B,, +2.426468,0x60,, +2.426555,0x22,, +2.426642,0x60,, +2.426729,0x22,, +2.426816,0x60,, +2.426903,0x3B,, +2.42699,0x60,, +2.427077,0x2E,, +2.427164,0xE0,, +2.42725,0x2E,, +2.427337,0xE0,, +2.427424,0x2C,, +2.427511,0x96,, +2.435124,0xA8,, +2.435211,0x01,, +2.435298,0x0C,, +2.435385,0x3A,, +2.435472,0x5C,, +2.435559,0x2E,, +2.435646,0xD8,, +2.435733,0x2E,, +2.43582,0xE0,, +2.435907,0x2E,, +2.435994,0xDC,, +2.436081,0x22,, +2.436168,0x60,, +2.436255,0x22,, +2.436341,0x60,, +2.436428,0x3B,, +2.436515,0x60,, +2.436602,0x22,, +2.436689,0x60,, +2.436776,0x22,, +2.436863,0x60,, +2.43695,0x3B,, +2.437037,0x60,, +2.437124,0x2E,, +2.437211,0xE0,, +2.437298,0x2E,, +2.437385,0xE0,, +2.437472,0x2A,, +2.437559,0xAE,, +2.445172,0xA8,, +2.445259,0x01,, +2.445346,0x0C,, +2.445433,0x3A,, +2.445519,0xC8,, +2.445606,0x2E,, +2.445693,0xD8,, +2.44578,0x2E,, +2.445867,0xE0,, +2.445954,0x2E,, +2.446041,0xDC,, +2.446128,0x22,, +2.446215,0x60,, +2.446302,0x22,, +2.446389,0x60,, +2.446476,0x3B,, +2.446563,0x60,, +2.44665,0x22,, +2.446737,0x60,, +2.446824,0x22,, +2.446911,0x60,, +2.446997,0x3B,, +2.447084,0x60,, +2.447171,0x2E,, +2.447258,0xE0,, +2.447345,0x2E,, +2.447432,0xE0,, +2.447519,0x0D,, +2.447606,0x44,, +2.455219,0xA8,, +2.455306,0x01,, +2.455393,0x0C,, +2.45548,0x3B,, +2.455567,0x34,, +2.455654,0x2E,, +2.455741,0xD8,, +2.455828,0x2E,, +2.455915,0xE0,, +2.456001,0x2E,, +2.456088,0xD8,, +2.456175,0x22,, +2.456262,0x60,, +2.456349,0x22,, +2.456436,0x60,, +2.456523,0x3B,, +2.45661,0x60,, +2.456697,0x22,, +2.456784,0x60,, +2.456871,0x22,, +2.456958,0x60,, +2.457045,0x3B,, +2.457132,0x60,, +2.457219,0x2E,, +2.457306,0xE0,, +2.457393,0x2E,, +2.457479,0xE0,, +2.457566,0xE7,, +2.457653,0x2A,, +2.465267,0xA8,, +2.465354,0x01,, +2.465441,0x0C,, +2.465528,0x3B,, +2.465615,0x60,, +2.465702,0x2E,, +2.465788,0xD8,, +2.465875,0x2E,, +2.465962,0xE0,, +2.466049,0x2E,, +2.466136,0xDC,, +2.466223,0x22,, +2.46631,0x60,, +2.466397,0x22,, +2.466484,0x60,, +2.466571,0x3B,, +2.466658,0x60,, +2.466745,0x22,, +2.466832,0x60,, +2.466919,0x22,, +2.467006,0x60,, +2.467093,0x3B,, +2.46718,0x60,, +2.467266,0x2E,, +2.467353,0xE0,, +2.46744,0x2E,, +2.467527,0xE0,, +2.467614,0xD1,, +2.467701,0x01,, +2.475314,0xA8,, +2.475401,0x01,, +2.475488,0x0C,, +2.475575,0x3B,, +2.475662,0x60,, +2.475749,0x2E,, +2.475836,0xD8,, +2.475923,0x2E,, +2.47601,0xE0,, +2.476097,0x2E,, +2.476184,0xE0,, +2.476271,0x22,, +2.476358,0x60,, +2.476445,0x22,, +2.476532,0x60,, +2.476619,0x3B,, +2.476706,0x60,, +2.476793,0x22,, +2.476879,0x60,, +2.476966,0x22,, +2.477053,0x60,, +2.47714,0x3B,, +2.477227,0x60,, +2.477314,0x2E,, +2.477401,0xE0,, +2.477488,0x2E,, +2.477575,0xE0,, +2.477662,0x37,, +2.477749,0x9A,, +2.485361,0xA8,, +2.485448,0x01,, +2.485535,0x0C,, +2.485622,0x3B,, +2.485709,0x60,, +2.485796,0x2E,, +2.485883,0xD8,, +2.48597,0x2E,, +2.486057,0xE0,, +2.486143,0x2E,, +2.48623,0xDC,, +2.486317,0x22,, +2.486404,0x60,, +2.486491,0x22,, +2.486578,0x60,, +2.486665,0x3B,, +2.486752,0x60,, +2.486839,0x22,, +2.486926,0x60,, +2.487013,0x22,, +2.4871,0x60,, +2.487187,0x3B,, +2.487274,0x60,, +2.487361,0x2E,, +2.487448,0xE0,, +2.487535,0x2E,, +2.487622,0xE0,, +2.487708,0xD1,, +2.487795,0x01,, +2.495409,0xA8,, +2.495496,0x01,, +2.495583,0x0C,, +2.49567,0x3B,, +2.495757,0x60,, +2.495844,0x2E,, +2.49593,0xD8,, +2.496017,0x2E,, +2.496104,0xE4,, +2.496191,0x2E,, +2.496278,0xD8,, +2.496365,0x22,, +2.496452,0x60,, +2.496539,0x22,, +2.496626,0x60,, +2.496713,0x3B,, +2.4968,0x60,, +2.496887,0x22,, +2.496974,0x60,, +2.497061,0x22,, +2.497148,0x60,, +2.497235,0x3B,, +2.497321,0x60,, +2.497408,0x2E,, +2.497495,0xE0,, +2.497582,0x2E,, +2.497669,0xE0,, +2.497756,0xDA,, +2.497843,0x96,, +2.505456,0xA8,, +2.505543,0x01,, +2.50563,0x0C,, +2.505717,0x3B,, +2.505804,0x60,, +2.505891,0x2E,, +2.505978,0xD8,, +2.506065,0x2E,, +2.506151,0xE0,, +2.506238,0x2E,, +2.506325,0xD8,, +2.506412,0x22,, +2.506499,0x60,, +2.506586,0x22,, +2.506673,0x60,, +2.50676,0x3B,, +2.506847,0x60,, +2.506934,0x22,, +2.507021,0x60,, +2.507108,0x22,, +2.507195,0x60,, +2.507282,0x3B,, +2.507369,0x60,, +2.507456,0x2E,, +2.507543,0xE0,, +2.507629,0x2E,, +2.507716,0xE0,, +2.507803,0x92,, +2.50789,0x89,, +2.515503,0xA8,, +2.51559,0x01,, +2.515677,0x0C,, +2.515764,0x3B,, +2.515851,0x60,, +2.515938,0x2E,, +2.516025,0xD8,, +2.516112,0x2E,, +2.516199,0xE4,, +2.516286,0x2E,, +2.516373,0xD4,, +2.51646,0x22,, +2.516547,0x60,, +2.516634,0x22,, +2.51672,0x60,, +2.516807,0x3B,, +2.516894,0x60,, +2.516981,0x22,, +2.517068,0x60,, +2.517155,0x22,, +2.517242,0x60,, +2.517329,0x3B,, +2.517416,0x60,, +2.517503,0x2E,, +2.51759,0xE0,, +2.517677,0x2E,, +2.517764,0xE0,, +2.517851,0x1E,, +2.517938,0x0E,, +2.525551,0xA8,, +2.525638,0x01,, +2.525725,0x0C,, +2.525812,0x3B,, +2.525899,0x60,, +2.525986,0x2E,, +2.526073,0xD8,, +2.52616,0x2E,, +2.526247,0xE0,, +2.526334,0x2E,, +2.526421,0xD4,, +2.526507,0x22,, +2.526594,0x60,, +2.526681,0x22,, +2.526768,0x60,, +2.526855,0x3B,, +2.526942,0x60,, +2.527029,0x22,, +2.527116,0x60,, +2.527203,0x22,, +2.52729,0x60,, +2.527377,0x3B,, +2.527464,0x60,, +2.527551,0x2E,, +2.527638,0xE0,, +2.527725,0x2E,, +2.527812,0xE0,, +2.527899,0x56,, +2.527985,0x11,, +2.535599,0xA8,, +2.535685,0x01,, +2.535772,0x0C,, +2.535859,0x3B,, +2.535946,0x60,, +2.536033,0x2E,, +2.53612,0xD8,, +2.536207,0x2E,, +2.536294,0xE0,, +2.536381,0x2E,, +2.536468,0xD0,, +2.536555,0x22,, +2.536642,0x60,, +2.536729,0x22,, +2.536816,0x60,, +2.536903,0x3B,, +2.53699,0x60,, +2.537077,0x22,, +2.537163,0x60,, +2.53725,0x22,, +2.537337,0x60,, +2.537424,0x3B,, +2.537511,0x60,, +2.537598,0x2E,, +2.537685,0xE0,, +2.537772,0x2E,, +2.537859,0xE0,, +2.537946,0x15,, +2.538033,0x99,, +2.545646,0xA8,, +2.545733,0x01,, +2.54582,0x0C,, +2.545907,0x3B,, +2.545994,0x60,, +2.546081,0x2E,, +2.546168,0xD8,, +2.546255,0x2E,, +2.546342,0xE4,, +2.546429,0x2E,, +2.546515,0xD0,, +2.546602,0x22,, +2.546689,0x60,, +2.546776,0x22,, +2.546863,0x60,, +2.54695,0x3B,, +2.547037,0x60,, +2.547124,0x22,, +2.547211,0x60,, +2.547298,0x22,, +2.547385,0x60,, +2.547472,0x3B,, +2.547559,0x60,, +2.547646,0x2E,, +2.547733,0xE0,, +2.54782,0x2E,, +2.547907,0xE0,, +2.547993,0x5D,, +2.54808,0x86,, +2.555693,0xA8,, +2.55578,0x01,, +2.555867,0x0C,, +2.555954,0x3B,, +2.556041,0x60,, +2.556128,0x2E,, +2.556215,0xD8,, +2.556302,0x2E,, +2.556389,0xE0,, +2.556476,0x2E,, +2.556563,0xD0,, +2.556649,0x22,, +2.556736,0x60,, +2.556823,0x22,, +2.55691,0x60,, +2.556997,0x3B,, +2.557084,0x60,, +2.557171,0x22,, +2.557258,0x60,, +2.557345,0x22,, +2.557432,0x60,, +2.557519,0x3B,, +2.557606,0x60,, +2.557693,0x2E,, +2.55778,0xE0,, +2.557867,0x2E,, +2.557954,0xE0,, +2.558041,0x15,, +2.558128,0x99,, +2.565741,0xA8,, +2.565828,0x01,, +2.565915,0x0C,, +2.566002,0x3B,, +2.566089,0x60,, +2.566176,0x2E,, +2.566263,0xD8,, +2.56635,0x2E,, +2.566436,0xE0,, +2.566523,0x2E,, +2.56661,0xD0,, +2.566697,0x22,, +2.566784,0x60,, +2.566871,0x22,, +2.566958,0x60,, +2.567045,0x3B,, +2.567132,0x60,, +2.567219,0x22,, +2.567306,0x60,, +2.567393,0x22,, +2.56748,0x60,, +2.567567,0x3B,, +2.567654,0x60,, +2.567741,0x2E,, +2.567828,0xE0,, +2.567915,0x2E,, +2.568001,0xE0,, +2.568088,0x15,, +2.568175,0x99,, +2.575788,0xA8,, +2.575875,0x01,, +2.575962,0x0C,, +2.576049,0x3B,, +2.576136,0x60,, +2.576223,0x2E,, +2.57631,0xD8,, +2.576397,0x2E,, +2.576484,0xE0,, +2.576571,0x2E,, +2.576658,0xD4,, +2.576745,0x22,, +2.576832,0x60,, +2.576919,0x22,, +2.577006,0x60,, +2.577093,0x3B,, +2.577179,0x60,, +2.577266,0x22,, +2.577353,0x60,, +2.57744,0x22,, +2.577527,0x60,, +2.577614,0x3B,, +2.577701,0x60,, +2.577788,0x2E,, +2.577875,0xE0,, +2.577962,0x2E,, +2.578049,0xE0,, +2.578136,0x56,, +2.578223,0x11,, +2.585836,0xA8,, +2.585923,0x01,, +2.58601,0x0C,, +2.586097,0x3B,, +2.586184,0x60,, +2.586271,0x2E,, +2.586357,0xD8,, +2.586444,0x2E,, +2.586531,0xE4,, +2.586618,0x2E,, +2.586705,0xD8,, +2.586792,0x22,, +2.586879,0x60,, +2.586966,0x22,, +2.587053,0x60,, +2.58714,0x3B,, +2.587227,0x60,, +2.587314,0x22,, +2.587401,0x60,, +2.587488,0x22,, +2.587575,0x60,, +2.587662,0x3B,, +2.587749,0x60,, +2.587835,0x2E,, +2.587922,0xE0,, +2.588009,0x2E,, +2.588096,0xE0,, +2.588183,0xDA,, +2.58827,0x96,, +2.595883,0xA8,, +2.59597,0x01,, +2.596057,0x0C,, +2.596144,0x3B,, +2.596231,0x60,, +2.596318,0x2E,, +2.596405,0xD8,, +2.596492,0x2E,, +2.596579,0xE0,, +2.596666,0x2E,, +2.596753,0xD8,, +2.59684,0x22,, +2.596926,0x60,, +2.597013,0x22,, +2.5971,0x60,, +2.597187,0x3B,, +2.597274,0x60,, +2.597361,0x22,, +2.597448,0x60,, +2.597535,0x22,, +2.597622,0x60,, +2.597709,0x3B,, +2.597796,0x60,, +2.597883,0x2E,, +2.59797,0xE0,, +2.598057,0x2E,, +2.598144,0xE0,, +2.598231,0x92,, +2.598318,0x89,, +2.60593,0xA8,, +2.606017,0x01,, +2.606104,0x0C,, +2.606191,0x3B,, +2.606278,0x60,, +2.606365,0x2E,, +2.606452,0xD8,, +2.606539,0x2E,, +2.606626,0xE4,, +2.606713,0x2E,, +2.6068,0xD8,, +2.606887,0x22,, +2.606974,0x60,, +2.60706,0x22,, +2.607147,0x60,, +2.607234,0x3B,, +2.607321,0x60,, +2.607408,0x22,, +2.607495,0x60,, +2.607582,0x22,, +2.607669,0x60,, +2.607756,0x3B,, +2.607843,0x60,, +2.60793,0x2E,, +2.608017,0xE0,, +2.608104,0x2E,, +2.608191,0xE0,, +2.608278,0xDA,, +2.608365,0x96,, +2.615978,0xA8,, +2.616065,0x01,, +2.616152,0x0C,, +2.616239,0x3B,, +2.616326,0x60,, +2.616413,0x2E,, +2.6165,0xD8,, +2.616587,0x2E,, +2.616674,0xE0,, +2.616761,0x2E,, +2.616848,0xD8,, +2.616935,0x22,, +2.617022,0x60,, +2.617109,0x22,, +2.617195,0x60,, +2.617282,0x3B,, +2.617369,0x60,, +2.617456,0x22,, +2.617543,0x60,, +2.61763,0x22,, +2.617717,0x60,, +2.617804,0x3B,, +2.617891,0x60,, +2.617978,0x2E,, +2.618065,0xE0,, +2.618152,0x2E,, +2.618239,0xE0,, +2.618326,0x92,, +2.618413,0x89,, +2.626025,0xA8,, +2.626112,0x01,, +2.626199,0x0C,, +2.626286,0x3B,, +2.626373,0x60,, +2.62646,0x2E,, +2.626547,0xD8,, +2.626634,0x2E,, +2.626721,0xE4,, +2.626808,0x2E,, +2.626895,0xD8,, +2.626982,0x22,, +2.627069,0x60,, +2.627155,0x22,, +2.627242,0x60,, +2.627329,0x3B,, +2.627416,0x60,, +2.627503,0x22,, +2.62759,0x60,, +2.627677,0x22,, +2.627764,0x60,, +2.627851,0x3B,, +2.627938,0x60,, +2.628025,0x2E,, +2.628112,0xE0,, +2.628199,0x2E,, +2.628286,0xE0,, +2.628373,0xDA,, +2.62846,0x96,, +2.636074,0xA8,, +2.636161,0x01,, +2.636248,0x0C,, +2.636335,0x3B,, +2.636422,0x60,, +2.636509,0x2E,, +2.636596,0xD8,, +2.636682,0x2E,, +2.636769,0xE0,, +2.636856,0x2E,, +2.636943,0xD8,, +2.63703,0x22,, +2.637117,0x60,, +2.637204,0x22,, +2.637291,0x60,, +2.637378,0x3B,, +2.637465,0x60,, +2.637552,0x22,, +2.637639,0x60,, +2.637726,0x22,, +2.637813,0x60,, +2.6379,0x3B,, +2.637987,0x60,, +2.638074,0x2E,, +2.63816,0xE0,, +2.638247,0x2E,, +2.638334,0xE0,, +2.638421,0x92,, +2.638508,0x89,, +2.646122,0xA8,, +2.646209,0x01,, +2.646295,0x0C,, +2.646382,0x3B,, +2.646469,0x60,, +2.646556,0x2E,, +2.646643,0xD8,, +2.64673,0x2E,, +2.646817,0xE4,, +2.646904,0x2E,, +2.646991,0xDC,, +2.647078,0x22,, +2.647165,0x60,, +2.647252,0x22,, +2.647339,0x60,, +2.647426,0x3B,, +2.647513,0x60,, +2.6476,0x22,, +2.647687,0x60,, +2.647774,0x22,, +2.64786,0x60,, +2.647947,0x3B,, +2.648034,0x60,, +2.648121,0x2E,, +2.648208,0xE0,, +2.648295,0x2E,, +2.648382,0xE0,, +2.648469,0x99,, +2.648556,0x1E,, +2.656169,0xA8,, +2.656256,0x01,, +2.656343,0x0C,, +2.65643,0x3B,, +2.656517,0x60,, +2.656604,0x2E,, +2.656691,0xD8,, +2.656778,0x2E,, +2.656865,0xE0,, +2.656952,0x2E,, +2.657039,0xD8,, +2.657126,0x22,, +2.657213,0x60,, +2.6573,0x22,, +2.657387,0x60,, +2.657473,0x3B,, +2.65756,0x60,, +2.657647,0x22,, +2.657734,0x60,, +2.657821,0x22,, +2.657908,0x60,, +2.657995,0x3B,, +2.658082,0x60,, +2.658169,0x2E,, +2.658256,0xE0,, +2.658343,0x2E,, +2.65843,0xE0,, +2.658517,0x92,, +2.658604,0x89,, +2.666217,0xA8,, +2.666304,0x01,, +2.666391,0x0C,, +2.666478,0x3B,, +2.666565,0x60,, +2.666652,0x2E,, +2.666739,0xD8,, +2.666826,0x2E,, +2.666913,0xE0,, +2.667,0x2E,, +2.667086,0xDC,, +2.667173,0x22,, +2.66726,0x60,, +2.667347,0x22,, +2.667434,0x60,, +2.667521,0x3B,, +2.667608,0x60,, +2.667695,0x22,, +2.667782,0x60,, +2.667869,0x22,, +2.667956,0x60,, +2.668043,0x3B,, +2.66813,0x60,, +2.668217,0x2E,, +2.668304,0xE0,, +2.668391,0x2E,, +2.668478,0xE0,, +2.668564,0xD1,, +2.668651,0x01,, +2.676284,0xA8,, +2.676371,0x01,, +2.676458,0x0C,, +2.676545,0x3B,, +2.676632,0x60,, +2.676719,0x2E,, +2.676806,0xD8,, +2.676893,0x2E,, +2.67698,0xE0,, +2.677067,0x2E,, +2.677154,0xDC,, +2.677241,0x22,, +2.677327,0x60,, +2.677414,0x22,, +2.677501,0x60,, +2.677588,0x3B,, +2.677675,0x60,, +2.677762,0x22,, +2.677849,0x60,, +2.677936,0x22,, +2.678023,0x60,, +2.67811,0x3B,, +2.678197,0x60,, +2.678284,0x2E,, +2.678371,0xE0,, +2.678458,0x2E,, +2.678545,0xE0,, +2.678632,0xD1,, +2.678719,0x01,, +2.686331,0xA8,, +2.686418,0x01,, +2.686505,0x0C,, +2.686592,0x3B,, +2.686679,0x60,, +2.686766,0x2E,, +2.686853,0xD8,, +2.68694,0x2E,, +2.687027,0xE4,, +2.687114,0x2E,, +2.687201,0xDC,, +2.687288,0x22,, +2.687374,0x60,, +2.687461,0x22,, +2.687548,0x60,, +2.687635,0x3B,, +2.687722,0x60,, +2.687809,0x22,, +2.687896,0x60,, +2.687983,0x22,, +2.68807,0x60,, +2.688157,0x3B,, +2.688244,0x60,, +2.688331,0x2E,, +2.688418,0xE0,, +2.688505,0x2E,, +2.688592,0xE0,, +2.688679,0x99,, +2.688766,0x1E,, +2.696379,0xA8,, +2.696466,0x01,, +2.696553,0x0C,, +2.69664,0x3B,, +2.696727,0x58,, +2.696814,0x2E,, +2.696901,0xD8,, +2.696988,0x2E,, +2.697075,0xE0,, +2.697161,0x2E,, +2.697248,0xDC,, +2.697335,0x22,, +2.697422,0x60,, +2.697509,0x22,, +2.697596,0x60,, +2.697683,0x3B,, +2.69777,0x60,, +2.697857,0x22,, +2.697944,0x60,, +2.698031,0x22,, +2.698118,0x60,, +2.698205,0x3B,, +2.698292,0x60,, +2.698379,0x2E,, +2.698466,0xE0,, +2.698553,0x2E,, +2.698639,0xE0,, +2.698726,0x99,, +2.698813,0xA0,, +2.706426,0xA8,, +2.706513,0x01,, +2.7066,0x0C,, +2.706687,0x3B,, +2.706774,0x40,, +2.706861,0x2E,, +2.706948,0xD8,, +2.707035,0x2E,, +2.707122,0xE4,, +2.707209,0x2E,, +2.707296,0xDC,, +2.707383,0x22,, +2.707469,0x60,, +2.707556,0x22,, +2.707643,0x60,, +2.70773,0x3B,, +2.707817,0x60,, +2.707904,0x22,, +2.707991,0x60,, +2.708078,0x22,, +2.708165,0x60,, +2.708252,0x3B,, +2.708339,0x60,, +2.708426,0x2E,, +2.708513,0xE0,, +2.7086,0x2E,, +2.708687,0xE0,, +2.708774,0xF9,, +2.708861,0xF2,, +2.716474,0xA8,, +2.716561,0x01,, +2.716648,0x0C,, +2.716734,0x3B,, +2.716821,0x28,, +2.716908,0x2E,, +2.716995,0xD8,, +2.717082,0x2E,, +2.717169,0xE4,, +2.717256,0x2E,, +2.717343,0xD4,, +2.71743,0x22,, +2.717517,0x60,, +2.717604,0x22,, +2.717691,0x60,, +2.717778,0x3B,, +2.717865,0x60,, +2.717952,0x22,, +2.718039,0x60,, +2.718126,0x22,, +2.718212,0x60,, +2.718299,0x3B,, +2.718386,0x60,, +2.718473,0x2E,, +2.71856,0xE0,, +2.718647,0x2E,, +2.718734,0xE0,, +2.718821,0xC7,, +2.718908,0xED,, +2.726523,0xA8,, +2.72661,0x01,, +2.726697,0x0C,, +2.726784,0x3A,, +2.726871,0xFC,, +2.726958,0x2E,, +2.727044,0xD8,, +2.727131,0x2E,, +2.727218,0xE0,, +2.727305,0x2E,, +2.727392,0xDC,, +2.727479,0x22,, +2.727566,0x60,, +2.727653,0x22,, +2.72774,0x60,, +2.727827,0x3B,, +2.727914,0x60,, +2.728001,0x22,, +2.728088,0x60,, +2.728175,0x22,, +2.728262,0x60,, +2.728349,0x3B,, +2.728436,0x60,, +2.728522,0x2E,, +2.728609,0xE0,, +2.728696,0x2E,, +2.728783,0xE0,, +2.72887,0xD9,, +2.728957,0xD3,, +2.736571,0xA8,, +2.736658,0x01,, +2.736745,0x0C,, +2.736832,0x3A,, +2.736919,0xCC,, +2.737005,0x2E,, +2.737092,0xD8,, +2.737179,0x2E,, +2.737266,0xE0,, +2.737353,0x2E,, +2.73744,0xDC,, +2.737527,0x22,, +2.737614,0x60,, +2.737701,0x22,, +2.737788,0x60,, +2.737875,0x3B,, +2.737962,0x60,, +2.738049,0x22,, +2.738136,0x60,, +2.738223,0x22,, +2.73831,0x60,, +2.738397,0x3B,, +2.738483,0x60,, +2.73857,0x2E,, +2.738657,0xE0,, +2.738744,0x2E,, +2.738831,0xE0,, +2.738918,0x89,, +2.739005,0x49,, +2.74662,0xA8,, +2.746707,0x01,, +2.746794,0x0C,, +2.746881,0x3A,, +2.746968,0x88,, +2.747055,0x2E,, +2.747142,0xD8,, +2.747228,0x2E,, +2.747315,0xD4,, +2.747402,0x2E,, +2.747489,0xDC,, +2.747576,0x22,, +2.747663,0x60,, +2.74775,0x22,, +2.747837,0x60,, +2.747924,0x3B,, +2.748011,0x60,, +2.748098,0x22,, +2.748185,0x60,, +2.748272,0x22,, +2.748359,0x60,, +2.748446,0x3B,, +2.748533,0x60,, +2.74862,0x2E,, +2.748706,0xE0,, +2.748793,0x2E,, +2.74888,0xE0,, +2.748967,0xD4,, +2.749054,0x64,, +2.756665,0xA8,, +2.756752,0x01,, +2.756839,0x0C,, +2.756926,0x3A,, +2.757013,0x20,, +2.7571,0x2E,, +2.757187,0xD8,, +2.757274,0x2E,, +2.757361,0xE4,, +2.757447,0x2E,, +2.757534,0xDC,, +2.757621,0x22,, +2.757708,0x60,, +2.757795,0x22,, +2.757882,0x60,, +2.757969,0x3B,, +2.758056,0x60,, +2.758143,0x22,, +2.75823,0x60,, +2.758317,0x22,, +2.758404,0x60,, +2.758491,0x3B,, +2.758578,0x60,, +2.758665,0x2E,, +2.758752,0xE0,, +2.758839,0x2E,, +2.758925,0xE0,, +2.759012,0x6F,, +2.759099,0xC5,, +2.766713,0xA8,, +2.7668,0x01,, +2.766887,0x0C,, +2.766974,0x39,, +2.76706,0x9C,, +2.767147,0x2E,, +2.767234,0xD8,, +2.767321,0x2E,, +2.767408,0xE0,, +2.767495,0x2E,, +2.767582,0xDC,, +2.767669,0x22,, +2.767756,0x60,, +2.767843,0x22,, +2.76793,0x60,, +2.768017,0x3B,, +2.768104,0x60,, +2.768191,0x22,, +2.768278,0x60,, +2.768365,0x22,, +2.768452,0x60,, +2.768538,0x3B,, +2.768625,0x60,, +2.768712,0x2E,, +2.768799,0xE0,, +2.768886,0x2E,, +2.768973,0xE0,, +2.76906,0x21,, +2.769147,0xE2,, +2.77676,0xA8,, +2.776847,0x01,, +2.776934,0x0C,, +2.777021,0x38,, +2.777108,0xF4,, +2.777195,0x2E,, +2.777282,0xD8,, +2.777369,0x2E,, +2.777456,0xE0,, +2.777543,0x2E,, +2.77763,0xE0,, +2.777717,0x22,, +2.777804,0x60,, +2.777891,0x22,, +2.777978,0x60,, +2.778065,0x3B,, +2.778151,0x60,, +2.778238,0x22,, +2.778325,0x60,, +2.778412,0x22,, +2.778499,0x60,, +2.778586,0x3B,, +2.778673,0x60,, +2.77876,0x2E,, +2.778847,0xE0,, +2.778934,0x2E,, +2.779021,0xE0,, +2.779108,0x49,, +2.779195,0x75,, +2.786808,0xA8,, +2.786895,0x01,, +2.786982,0x0C,, +2.787069,0x38,, +2.787156,0x50,, +2.787243,0x2E,, +2.78733,0xD8,, +2.787417,0x2E,, +2.787503,0xE0,, +2.78759,0x2E,, +2.787677,0xDC,, +2.787764,0x22,, +2.787851,0x60,, +2.787938,0x22,, +2.788025,0x60,, +2.788112,0x3B,, +2.788199,0x60,, +2.788286,0x22,, +2.788373,0x60,, +2.78846,0x22,, +2.788547,0x60,, +2.788634,0x3B,, +2.788721,0x60,, +2.788808,0x2E,, +2.788895,0xE0,, +2.788981,0x2E,, +2.789068,0xE0,, +2.789155,0xD8,, +2.789242,0x9E,, +2.797375,0xA8,, +2.797462,0x01,, +2.797549,0x0C,, +2.797636,0x37,, +2.797723,0xB4,, +2.79781,0x2E,, +2.797897,0xD8,, +2.797984,0x2E,, +2.798071,0xE0,, +2.798158,0x2E,, +2.798245,0xDC,, +2.798332,0x22,, +2.798419,0x60,, +2.798506,0x22,, +2.798593,0x60,, +2.798679,0x3B,, +2.798766,0x60,, +2.798853,0x22,, +2.79894,0x60,, +2.799027,0x22,, +2.799114,0x60,, +2.799201,0x3B,, +2.799288,0x60,, +2.799375,0x2E,, +2.799462,0xE0,, +2.799549,0x2E,, +2.799636,0xE0,, +2.799723,0x43,, +2.79981,0x06,, +2.807413,0xA8,, +2.8075,0x01,, +2.807587,0x0C,, +2.807674,0x37,, +2.807761,0x10,, +2.807848,0x2E,, +2.807935,0xD8,, +2.808022,0x2E,, +2.808109,0xE4,, +2.808196,0x2E,, +2.808283,0xDC,, +2.80837,0x22,, +2.808457,0x60,, +2.808544,0x22,, +2.80863,0x60,, +2.808717,0x3B,, +2.808804,0x60,, +2.808891,0x22,, +2.808978,0x60,, +2.809065,0x22,, +2.809152,0x60,, +2.809239,0x3B,, +2.809326,0x60,, +2.809413,0x2E,, +2.8095,0xE0,, +2.809587,0x2E,, +2.809674,0xE0,, +2.809761,0x7C,, +2.809848,0x69,, +2.81746,0xA8,, +2.817547,0x01,, +2.817634,0x0C,, +2.817721,0x36,, +2.817807,0x80,, +2.817894,0x2E,, +2.817981,0xD8,, +2.818068,0x2E,, +2.818155,0xE8,, +2.818242,0x2E,, +2.818329,0xDC,, +2.818416,0x22,, +2.818503,0x60,, +2.81859,0x22,, +2.818677,0x60,, +2.818764,0x3B,, +2.818851,0x60,, +2.818938,0x22,, +2.819025,0x60,, +2.819112,0x22,, +2.819199,0x60,, +2.819285,0x3B,, +2.819372,0x60,, +2.819459,0x2E,, +2.819546,0xE0,, +2.819633,0x2E,, +2.81972,0xE0,, +2.819807,0x30,, +2.819894,0xAC,, +2.827556,0xA8,, +2.827643,0x01,, +2.82773,0x0C,, +2.827816,0x35,, +2.827903,0xDC,, +2.82799,0x2E,, +2.828077,0xD8,, +2.828164,0x2E,, +2.828251,0xE4,, +2.828338,0x2E,, +2.828425,0xDC,, +2.828512,0x22,, +2.828599,0x60,, +2.828686,0x22,, +2.828773,0x60,, +2.82886,0x3B,, +2.828947,0x60,, +2.829034,0x22,, +2.829121,0x60,, +2.829208,0x22,, +2.829294,0x60,, +2.829381,0x3B,, +2.829468,0x60,, +2.829555,0x2E,, +2.829642,0xE0,, +2.829729,0x2E,, +2.829816,0xE0,, +2.829903,0xDC,, +2.82999,0x10,, +2.837602,0xA8,, +2.837689,0x01,, +2.837776,0x0C,, +2.837863,0x35,, +2.83795,0x3C,, +2.838037,0x2E,, +2.838124,0xD4,, +2.838211,0x2E,, +2.838298,0xE0,, +2.838385,0x2E,, +2.838472,0xDC,, +2.838559,0x22,, +2.838646,0x60,, +2.838733,0x22,, +2.838819,0x60,, +2.838906,0x3B,, +2.838993,0x60,, +2.83908,0x22,, +2.839167,0x60,, +2.839254,0x22,, +2.839341,0x60,, +2.839428,0x3B,, +2.839515,0x60,, +2.839602,0x2E,, +2.839689,0xE0,, +2.839776,0x2E,, +2.839863,0xE0,, +2.83995,0x0E,, +2.840037,0x67,, +2.84765,0xA8,, +2.847737,0x01,, +2.847823,0x0C,, +2.84791,0x34,, +2.847997,0xA0,, +2.848084,0x2E,, +2.848171,0xD8,, +2.848258,0x2E,, +2.848345,0xE0,, +2.848432,0x2E,, +2.848519,0xDC,, +2.848606,0x22,, +2.848693,0x60,, +2.84878,0x22,, +2.848867,0x60,, +2.848954,0x3B,, +2.849041,0x60,, +2.849128,0x22,, +2.849215,0x60,, +2.849301,0x22,, +2.849388,0x60,, +2.849475,0x3B,, +2.849562,0x60,, +2.849649,0x2E,, +2.849736,0xE0,, +2.849823,0x2E,, +2.84991,0xE0,, +2.849997,0xAE,, +2.850084,0x78,, +2.857766,0xA8,, +2.857853,0x01,, +2.85794,0x0C,, +2.858027,0x33,, +2.858114,0xF8,, +2.858201,0x2E,, +2.858288,0xD8,, +2.858375,0x2E,, +2.858462,0xE0,, +2.858549,0x2E,, +2.858636,0xDC,, +2.858723,0x22,, +2.85881,0x60,, +2.858897,0x22,, +2.858983,0x60,, +2.85907,0x3B,, +2.859157,0x60,, +2.859244,0x22,, +2.859331,0x60,, +2.859418,0x22,, +2.859505,0x60,, +2.859592,0x3B,, +2.859679,0x60,, +2.859766,0x2E,, +2.859853,0xE0,, +2.85994,0x2E,, +2.860027,0xE0,, +2.860114,0xC2,, +2.860201,0xE4,, +2.867813,0xA8,, +2.8679,0x01,, +2.867987,0x0C,, +2.868074,0x33,, +2.868161,0x60,, +2.868248,0x2E,, +2.868335,0xD8,, +2.868422,0x2E,, +2.868509,0xE0,, +2.868596,0x2E,, +2.868683,0xDC,, +2.86877,0x22,, +2.868857,0x60,, +2.868944,0x22,, +2.86903,0x60,, +2.869117,0x3B,, +2.869204,0x60,, +2.869291,0x22,, +2.869378,0x60,, +2.869465,0x22,, +2.869552,0x60,, +2.869639,0x3B,, +2.869726,0x60,, +2.869813,0x2E,, +2.8699,0xE0,, +2.869987,0x2E,, +2.870074,0xE0,, +2.870161,0x79,, +2.870248,0x38,, +2.87786,0xA8,, +2.877947,0x01,, +2.878034,0x0C,, +2.878121,0x32,, +2.878208,0xDC,, +2.878295,0x2E,, +2.878382,0xD8,, +2.878469,0x2E,, +2.878556,0xE0,, +2.878643,0x2E,, +2.87873,0xDC,, +2.878817,0x22,, +2.878904,0x60,, +2.87899,0x22,, +2.879077,0x60,, +2.879164,0x3B,, +2.879251,0x60,, +2.879338,0x22,, +2.879425,0x60,, +2.879512,0x22,, +2.879599,0x60,, +2.879686,0x3B,, +2.879773,0x60,, +2.87986,0x2E,, +2.879947,0xE0,, +2.880034,0x2E,, +2.880121,0xE0,, +2.880208,0x11,, +2.880295,0x06,, +2.887907,0xA8,, +2.887994,0x01,, +2.888081,0x0C,, +2.888168,0x32,, +2.888255,0x28,, +2.888342,0x2E,, +2.888429,0xD8,, +2.888516,0x2E,, +2.888603,0xE0,, +2.88869,0x2E,, +2.888777,0xDC,, +2.888864,0x22,, +2.888951,0x60,, +2.889038,0x22,, +2.889125,0x60,, +2.889212,0x3B,, +2.889298,0x60,, +2.889385,0x22,, +2.889472,0x60,, +2.889559,0x22,, +2.889646,0x60,, +2.889733,0x3B,, +2.88982,0x60,, +2.889907,0x2E,, +2.889994,0xE0,, +2.890081,0x2E,, +2.890168,0xE0,, +2.890255,0x97,, +2.890342,0xD8,, +2.897954,0xA8,, +2.898041,0x01,, +2.898128,0x0C,, +2.898215,0x31,, +2.898302,0x54,, +2.898389,0x2E,, +2.898476,0xD8,, +2.898563,0x2E,, +2.89865,0xE4,, +2.898737,0x2E,, +2.898824,0xDC,, +2.89891,0x22,, +2.898997,0x60,, +2.899084,0x22,, +2.899171,0x60,, +2.899258,0x3B,, +2.899345,0x60,, +2.899432,0x22,, +2.899519,0x60,, +2.899606,0x22,, +2.899693,0x60,, +2.89978,0x3B,, +2.899867,0x60,, +2.899954,0x2E,, +2.900041,0xE0,, +2.900128,0x2E,, +2.900215,0xE0,, +2.900302,0x8B,, +2.900389,0xB6,, +2.908001,0xA8,, +2.908088,0x01,, +2.908175,0x0C,, +2.908262,0x30,, +2.908349,0x84,, +2.908436,0x2E,, +2.908523,0xD8,, +2.90861,0x2E,, +2.908697,0xE4,, +2.908784,0x2E,, +2.908871,0xDC,, +2.908958,0x22,, +2.909045,0x60,, +2.909132,0x22,, +2.909219,0x60,, +2.909306,0x3B,, +2.909392,0x60,, +2.909479,0x22,, +2.909566,0x60,, +2.909653,0x22,, +2.90974,0x60,, +2.909827,0x3B,, +2.909914,0x60,, +2.910001,0x2E,, +2.910088,0xE0,, +2.910175,0x2E,, +2.910262,0xE0,, +2.910349,0xDE,, +2.910436,0x8A,, +2.918048,0xA8,, +2.918135,0x01,, +2.918222,0x0C,, +2.918309,0x2F,, +2.918396,0xC4,, +2.918483,0x2E,, +2.91857,0xD8,, +2.918657,0x2E,, +2.918744,0xE0,, +2.918831,0x2E,, +2.918918,0xDC,, +2.919005,0x22,, +2.919092,0x60,, +2.919179,0x22,, +2.919266,0x60,, +2.919352,0x3B,, +2.919439,0x60,, +2.919526,0x22,, +2.919613,0x60,, +2.9197,0x22,, +2.919787,0x60,, +2.919874,0x3B,, +2.919961,0x60,, +2.920048,0x2E,, +2.920135,0xE0,, +2.920222,0x2E,, +2.920309,0xE0,, +2.920396,0x3A,, +2.920483,0x2E,, +2.928096,0xA8,, +2.928183,0x01,, +2.92827,0x0C,, +2.928357,0x2F,, +2.928443,0x10,, +2.92853,0x2E,, +2.928617,0xD8,, +2.928704,0x2E,, +2.928791,0xE4,, +2.928878,0x2E,, +2.928965,0xDC,, +2.929052,0x22,, +2.929139,0x60,, +2.929226,0x22,, +2.929313,0x60,, +2.9294,0x3B,, +2.929487,0x60,, +2.929574,0x22,, +2.929661,0x60,, +2.929748,0x22,, +2.929835,0x60,, +2.929922,0x3B,, +2.930008,0x60,, +2.930095,0x2E,, +2.930182,0xE0,, +2.930269,0x2E,, +2.930356,0xE0,, +2.930443,0x94,, +2.93053,0x03,, +2.938143,0xA8,, +2.93823,0x01,, +2.938317,0x0C,, +2.938404,0x2E,, +2.938491,0x70,, +2.938577,0x2E,, +2.938664,0xD8,, +2.938751,0x2E,, +2.938838,0xE0,, +2.938925,0x2E,, +2.939012,0xDC,, +2.939099,0x22,, +2.939186,0x60,, +2.939273,0x22,, +2.93936,0x60,, +2.939447,0x3B,, +2.939534,0x60,, +2.939621,0x22,, +2.939708,0x60,, +2.939795,0x22,, +2.939882,0x60,, +2.939969,0x3B,, +2.940055,0x60,, +2.940142,0x2E,, +2.940229,0xE0,, +2.940316,0x2E,, +2.940403,0xE0,, +2.94049,0x4A,, +2.940577,0x2B,, +2.94819,0xA8,, +2.948277,0x01,, +2.948364,0x0C,, +2.948451,0x2D,, +2.948538,0xB4,, +2.948625,0x2E,, +2.948712,0xD8,, +2.948798,0x2E,, +2.948885,0xE4,, +2.948972,0x2E,, +2.949059,0xD8,, +2.949146,0x22,, +2.949233,0x60,, +2.94932,0x22,, +2.949407,0x60,, +2.949494,0x3B,, +2.949581,0x60,, +2.949668,0x22,, +2.949755,0x60,, +2.949842,0x22,, +2.949929,0x60,, +2.950016,0x3B,, +2.950103,0x60,, +2.95019,0x2E,, +2.950276,0xE0,, +2.950363,0x2E,, +2.95045,0xE0,, +2.950537,0xCE,, +2.950624,0xFD,, +2.958237,0xA8,, +2.958324,0x01,, +2.958411,0x0C,, +2.958498,0x2D,, +2.958585,0x14,, +2.958672,0x2E,, +2.958759,0xD8,, +2.958845,0x2E,, +2.958932,0xE0,, +2.959019,0x2E,, +2.959106,0xDC,, +2.959193,0x22,, +2.95928,0x60,, +2.959367,0x22,, +2.959454,0x60,, +2.959541,0x3B,, +2.959628,0x60,, +2.959715,0x22,, +2.959802,0x60,, +2.959889,0x22,, +2.959976,0x60,, +2.960063,0x3B,, +2.96015,0x60,, +2.960237,0x2E,, +2.960323,0xE0,, +2.96041,0x2E,, +2.960497,0xE0,, +2.960584,0x36,, +2.960671,0x17,, +2.968284,0xA8,, +2.96837,0x01,, +2.968457,0x0C,, +2.968544,0x2C,, +2.968631,0x58,, +2.968718,0x2E,, +2.968805,0xD8,, +2.968892,0x2E,, +2.968979,0xE0,, +2.969066,0x2E,, +2.969153,0xDC,, +2.96924,0x22,, +2.969327,0x60,, +2.969414,0x22,, +2.969501,0x60,, +2.969588,0x3B,, +2.969675,0x60,, +2.969762,0x22,, +2.969848,0x60,, +2.969935,0x22,, +2.970022,0x60,, +2.970109,0x3B,, +2.970196,0x60,, +2.970283,0x2E,, +2.97037,0xE0,, +2.970457,0x2E,, +2.970544,0xE0,, +2.970631,0x5C,, +2.970718,0xFA,, +2.978331,0xA8,, +2.978418,0x01,, +2.978504,0x0C,, +2.978591,0x2B,, +2.978678,0xBC,, +2.978765,0x2E,, +2.978852,0xD8,, +2.978939,0x2E,, +2.979026,0xE0,, +2.979113,0x2E,, +2.9792,0xDC,, +2.979287,0x22,, +2.979374,0x60,, +2.979461,0x22,, +2.979548,0x60,, +2.979635,0x3B,, +2.979722,0x60,, +2.979809,0x22,, +2.979896,0x60,, +2.979982,0x22,, +2.980069,0x60,, +2.980156,0x3B,, +2.980243,0x60,, +2.98033,0x2E,, +2.980417,0xE0,, +2.980504,0x2E,, +2.980591,0xE0,, +2.980678,0x6F,, +2.980765,0x5B,, +2.988377,0xA8,, +2.988464,0x01,, +2.988551,0x0C,, +2.988638,0x2B,, +2.988725,0x0C,, +2.988812,0x2E,, +2.988899,0xD8,, +2.988986,0x2E,, +2.989072,0xE0,, +2.989159,0x2E,, +2.989246,0xDC,, +2.989333,0x22,, +2.98942,0x60,, +2.989507,0x22,, +2.989594,0x60,, +2.989681,0x3B,, +2.989768,0x60,, +2.989855,0x22,, +2.989942,0x60,, +2.990029,0x22,, +2.990116,0x60,, +2.990203,0x3B,, +2.99029,0x60,, +2.990377,0x2E,, +2.990464,0xE0,, +2.99055,0x2E,, +2.990637,0xE0,, +2.990724,0xAC,, +2.990811,0x50,, +2.998424,0xA8,, +2.998511,0x01,, +2.998598,0x0C,, +2.998685,0x2A,, +2.998772,0x68,, +2.998859,0x2E,, +2.998946,0xD8,, +2.999033,0x2E,, +2.99912,0xE0,, +2.999206,0x2E,, +2.999293,0xDC,, +2.99938,0x22,, +2.999467,0x60,, +2.999554,0x22,, +2.999641,0x60,, +2.999728,0x3B,, +2.999815,0x60,, +2.999902,0x22,, +2.999989,0x60,, +3.000076,0x22,, +3.000163,0x60,, +3.00025,0x3B,, +3.000337,0x60,, +3.000424,0x2E,, +3.000511,0xE0,, +3.000598,0x2E,, +3.000685,0xE0,, +3.000771,0xBE,, +3.000858,0x6A,, +3.008471,0xA8,, +3.008558,0x01,, +3.008645,0x0C,, +3.008732,0x29,, +3.008819,0xB0,, +3.008906,0x2E,, +3.008993,0xD8,, +3.00908,0x2E,, +3.009167,0xE0,, +3.009254,0x2E,, +3.009341,0xDC,, +3.009428,0x22,, +3.009515,0x60,, +3.009601,0x22,, +3.009688,0x60,, +3.009775,0x3B,, +3.009862,0x60,, +3.009949,0x22,, +3.010036,0x60,, +3.010123,0x22,, +3.01021,0x60,, +3.010297,0x3B,, +3.010384,0x60,, +3.010471,0x2E,, +3.010558,0xE0,, +3.010645,0x2E,, +3.010732,0xE0,, +3.010819,0x9D,, +3.010906,0x6B,, +3.018518,0xA8,, +3.018605,0x01,, +3.018692,0x0C,, +3.018779,0x28,, +3.018866,0xDC,, +3.018953,0x2E,, +3.01904,0xD8,, +3.019127,0x2E,, +3.019214,0xE0,, +3.019301,0x2E,, +3.019388,0xDC,, +3.019475,0x22,, +3.019561,0x60,, +3.019648,0x22,, +3.019735,0x60,, +3.019822,0x3B,, +3.019909,0x60,, +3.019996,0x22,, +3.020083,0x60,, +3.02017,0x22,, +3.020257,0x60,, +3.020344,0x3B,, +3.020431,0x60,, +3.020518,0x2E,, +3.020605,0xE0,, +3.020692,0x2E,, +3.020779,0xE0,, +3.020866,0x97,, +3.020953,0x6A,, +3.028576,0xA8,, +3.028663,0x01,, +3.02875,0x0C,, +3.028837,0x28,, +3.028924,0x34,, +3.029011,0x2E,, +3.029098,0xDC,, +3.029185,0x2E,, +3.029272,0xE0,, +3.029359,0x2E,, +3.029446,0xD8,, +3.029533,0x22,, +3.02962,0x60,, +3.029706,0x22,, +3.029793,0x60,, +3.02988,0x3B,, +3.029967,0x60,, +3.030054,0x22,, +3.030141,0x60,, +3.030228,0x22,, +3.030315,0x60,, +3.030402,0x3B,, +3.030489,0x60,, +3.030576,0x2E,, +3.030663,0xE0,, +3.03075,0x2E,, +3.030837,0xE0,, +3.030924,0x99,, +3.031011,0xC7,, +3.038623,0xA8,, +3.03871,0x01,, +3.038797,0x0C,, +3.038884,0x27,, +3.038971,0x80,, +3.039058,0x2E,, +3.039145,0xD8,, +3.039232,0x2E,, +3.039319,0xE0,, +3.039406,0x2E,, +3.039493,0xD8,, +3.039579,0x22,, +3.039666,0x60,, +3.039753,0x22,, +3.03984,0x60,, +3.039927,0x3B,, +3.040014,0x60,, +3.040101,0x22,, +3.040188,0x60,, +3.040275,0x22,, +3.040362,0x60,, +3.040449,0x3B,, +3.040536,0x60,, +3.040623,0x2E,, +3.04071,0xE0,, +3.040797,0x2E,, +3.040884,0xE0,, +3.040971,0x94,, +3.041058,0x4A,, +3.04867,0xA8,, +3.048757,0x01,, +3.048844,0x0C,, +3.048931,0x26,, +3.049018,0xE0,, +3.049105,0x2E,, +3.049192,0xD8,, +3.049279,0x2E,, +3.049366,0xE0,, +3.049453,0x2E,, +3.04954,0xD8,, +3.049627,0x22,, +3.049714,0x60,, +3.049801,0x22,, +3.049888,0x60,, +3.049974,0x3B,, +3.050061,0x60,, +3.050148,0x22,, +3.050235,0x60,, +3.050322,0x22,, +3.050409,0x60,, +3.050496,0x3B,, +3.050583,0x60,, +3.05067,0x2E,, +3.050757,0xE0,, +3.050844,0x2E,, +3.050931,0xE0,, +3.051018,0x02,, +3.051105,0x7D,, +3.058717,0xA8,, +3.058804,0x01,, +3.058891,0x0C,, +3.058978,0x26,, +3.059065,0x4C,, +3.059152,0x2E,, +3.059239,0xD8,, +3.059326,0x2E,, +3.059413,0xE4,, +3.059499,0x2E,, +3.059586,0xD8,, +3.059673,0x22,, +3.05976,0x60,, +3.059847,0x22,, +3.059934,0x60,, +3.060021,0x3B,, +3.060108,0x60,, +3.060195,0x22,, +3.060282,0x60,, +3.060369,0x22,, +3.060456,0x60,, +3.060543,0x3B,, +3.06063,0x60,, +3.060717,0x2E,, +3.060804,0xE0,, +3.060891,0x2E,, +3.060978,0xE0,, +3.061064,0x25,, +3.061151,0x29,, +3.068764,0xA8,, +3.068851,0x01,, +3.068938,0x0C,, +3.069025,0x25,, +3.069112,0xD0,, +3.069199,0x2E,, +3.069286,0xD8,, +3.069373,0x2E,, +3.069459,0xE4,, +3.069546,0x2E,, +3.069633,0xD8,, +3.06972,0x22,, +3.069807,0x60,, +3.069894,0x22,, +3.069981,0x60,, +3.070068,0x3B,, +3.070155,0x60,, +3.070242,0x22,, +3.070329,0x60,, +3.070416,0x22,, +3.070503,0x60,, +3.07059,0x3B,, +3.070677,0x60,, +3.070764,0x2E,, +3.07085,0xE0,, +3.070937,0x2E,, +3.071024,0xE0,, +3.071111,0x43,, +3.071198,0xFD,, +3.078811,0xA8,, +3.078898,0x01,, +3.078985,0x0C,, +3.079072,0x25,, +3.079159,0x58,, +3.079246,0x2E,, +3.079333,0xD8,, +3.07942,0x2E,, +3.079507,0xE4,, +3.079594,0x2E,, +3.079681,0xD8,, +3.079768,0x22,, +3.079855,0x60,, +3.079942,0x22,, +3.080029,0x60,, +3.080116,0x3B,, +3.080203,0x60,, +3.080289,0x22,, +3.080376,0x60,, +3.080463,0x22,, +3.08055,0x60,, +3.080637,0x3B,, +3.080724,0x60,, +3.080811,0x2E,, +3.080898,0xE0,, +3.080985,0x2E,, +3.081072,0xE0,, +3.081159,0xC8,, +3.081246,0x57,, +3.088859,0xA8,, +3.088946,0x01,, +3.089033,0x0C,, +3.08912,0x24,, +3.089207,0xD0,, +3.089294,0x2E,, +3.089381,0xD8,, +3.089468,0x2E,, +3.089555,0xE4,, +3.089642,0x2E,, +3.089729,0xD8,, +3.089816,0x22,, +3.089903,0x60,, +3.089989,0x22,, +3.090076,0x60,, +3.090163,0x3B,, +3.09025,0x60,, +3.090337,0x22,, +3.090424,0x60,, +3.090511,0x22,, +3.090598,0x60,, +3.090685,0x3B,, +3.090772,0x60,, +3.090859,0x2E,, +3.090946,0xE0,, +3.091033,0x2E,, +3.09112,0xE0,, +3.091207,0x74,, +3.091294,0xFE,, +3.098906,0xA8,, +3.098992,0x01,, +3.099079,0x0C,, +3.099166,0x24,, +3.099253,0x4C,, +3.09934,0x2E,, +3.099427,0xD8,, +3.099514,0x2E,, +3.099601,0xE0,, +3.099688,0x2E,, +3.099775,0xDC,, +3.099862,0x22,, +3.099949,0x60,, +3.100036,0x22,, +3.100123,0x60,, +3.10021,0x3B,, +3.100297,0x60,, +3.100384,0x22,, +3.100471,0x60,, +3.100557,0x22,, +3.100644,0x60,, +3.100731,0x3B,, +3.100818,0x60,, +3.100905,0x2E,, +3.100992,0xE0,, +3.101079,0x2E,, +3.101166,0xE0,, +3.101253,0x40,, +3.10134,0xB8,, +3.108953,0xA8,, +3.10904,0x01,, +3.109127,0x0C,, +3.109214,0x23,, +3.109301,0xDC,, +3.109388,0x2E,, +3.109475,0xD8,, +3.109561,0x2E,, +3.109648,0xE0,, +3.109735,0x2E,, +3.109822,0xD8,, +3.109909,0x22,, +3.109996,0x60,, +3.110083,0x22,, +3.11017,0x60,, +3.110257,0x3B,, +3.110344,0x60,, +3.110431,0x22,, +3.110518,0x60,, +3.110605,0x22,, +3.110692,0x60,, +3.110779,0x3B,, +3.110866,0x60,, +3.110952,0x2E,, +3.111039,0xE0,, +3.111126,0x2E,, +3.111213,0xE0,, +3.1113,0x25,, +3.111387,0xDE,, +3.119,0xA8,, +3.119087,0x01,, +3.119174,0x0C,, +3.119261,0x23,, +3.119348,0x68,, +3.119435,0x2E,, +3.119522,0xD8,, +3.119609,0x2E,, +3.119695,0xE4,, +3.119782,0x2E,, +3.119869,0xDC,, +3.119956,0x22,, +3.120043,0x60,, +3.12013,0x22,, +3.120217,0x60,, +3.120304,0x3B,, +3.120391,0x60,, +3.120478,0x22,, +3.120565,0x60,, +3.120652,0x22,, +3.120739,0x60,, +3.120826,0x3B,, +3.120913,0x60,, +3.121,0x2E,, +3.121087,0xE0,, +3.121173,0x2E,, +3.12126,0xE0,, +3.121347,0x69,, +3.121434,0x4F,, +3.129047,0xA8,, +3.129133,0x01,, +3.12922,0x0C,, +3.129307,0x23,, +3.129394,0x04,, +3.129481,0x2E,, +3.129568,0xDC,, +3.129655,0x2E,, +3.129742,0xE0,, +3.129829,0x2E,, +3.129916,0xD8,, +3.130003,0x22,, +3.13009,0x60,, +3.130177,0x22,, +3.130264,0x60,, +3.130351,0x3B,, +3.130438,0x60,, +3.130525,0x22,, +3.130611,0x60,, +3.130698,0x22,, +3.130785,0x60,, +3.130872,0x3B,, +3.130959,0x60,, +3.131046,0x2E,, +3.131133,0xE0,, +3.13122,0x2E,, +3.131307,0xE0,, +3.131394,0x38,, +3.131481,0x61,, +3.139094,0xA8,, +3.139181,0x01,, +3.139268,0x0C,, +3.139355,0x22,, +3.139441,0xAC,, +3.139528,0x2E,, +3.139615,0xD8,, +3.139702,0x2E,, +3.139789,0xE4,, +3.139876,0x2E,, +3.139963,0xD8,, +3.14005,0x22,, +3.140137,0x60,, +3.140224,0x22,, +3.140311,0x60,, +3.140398,0x3B,, +3.140485,0x60,, +3.140572,0x22,, +3.140659,0x60,, +3.140746,0x22,, +3.140833,0x60,, +3.140919,0x3B,, +3.141006,0x60,, +3.141093,0x2E,, +3.14118,0xE0,, +3.141267,0x2E,, +3.141354,0xE0,, +3.141441,0xCB,, +3.141528,0x80,, +3.14914,0xA8,, +3.149227,0x01,, +3.149314,0x0C,, +3.149401,0x22,, +3.149488,0x78,, +3.149575,0x2E,, +3.149662,0xD8,, +3.149749,0x2E,, +3.149836,0xE0,, +3.149923,0x2E,, +3.15001,0xD8,, +3.150097,0x22,, +3.150184,0x60,, +3.150271,0x22,, +3.150358,0x60,, +3.150444,0x3B,, +3.150531,0x60,, +3.150618,0x22,, +3.150705,0x60,, +3.150792,0x22,, +3.150879,0x60,, +3.150966,0x3B,, +3.151053,0x60,, +3.15114,0x2E,, +3.151227,0xE0,, +3.151314,0x2E,, +3.151401,0xE0,, +3.151488,0x65,, +3.151575,0xAD,, +3.159188,0xA8,, +3.159275,0x01,, +3.159362,0x0C,, +3.159449,0x22,, +3.159536,0x78,, +3.159623,0x2E,, +3.159709,0xD8,, +3.159796,0x2E,, +3.159883,0xE0,, +3.15997,0x2E,, +3.160057,0xDC,, +3.160144,0x22,, +3.160231,0x60,, +3.160318,0x22,, +3.160405,0x60,, +3.160492,0x3B,, +3.160579,0x60,, +3.160666,0x22,, +3.160753,0x60,, +3.16084,0x22,, +3.160927,0x60,, +3.161014,0x3B,, +3.1611,0x60,, +3.161187,0x2E,, +3.161274,0xE0,, +3.161361,0x2E,, +3.161448,0xE0,, +3.161535,0x26,, +3.161622,0x25,, +3.169235,0xA8,, +3.169321,0x01,, +3.169408,0x0C,, +3.169495,0x22,, +3.169582,0x78,, +3.169669,0x2E,, +3.169756,0xD8,, +3.169843,0x2E,, +3.16993,0xE0,, +3.170017,0x2E,, +3.170104,0xD4,, +3.170191,0x22,, +3.170278,0x60,, +3.170365,0x22,, +3.170452,0x60,, +3.170539,0x3B,, +3.170626,0x60,, +3.170713,0x22,, +3.170799,0x60,, +3.170886,0x22,, +3.170973,0x60,, +3.17106,0x3B,, +3.171147,0x60,, +3.171234,0x2E,, +3.171321,0xE0,, +3.171408,0x2E,, +3.171495,0xE0,, +3.171582,0xA1,, +3.171669,0x35,, +3.179281,0xA8,, +3.179368,0x01,, +3.179455,0x0C,, +3.179542,0x22,, +3.179629,0x74,, +3.179716,0x2E,, +3.179803,0xD8,, +3.17989,0x2E,, +3.179977,0xE0,, +3.180064,0x2E,, +3.180151,0xD8,, +3.180238,0x22,, +3.180325,0x60,, +3.180412,0x22,, +3.180499,0x60,, +3.180586,0x3B,, +3.180673,0x60,, +3.180759,0x22,, +3.180846,0x60,, +3.180933,0x22,, +3.18102,0x60,, +3.181107,0x3B,, +3.181194,0x60,, +3.181281,0x2E,, +3.181368,0xE0,, +3.181455,0x2E,, +3.181542,0xE0,, +3.181629,0xF9,, +3.181716,0x9B,, +3.189328,0xA8,, +3.189415,0x01,, +3.189502,0x0C,, +3.189589,0x22,, +3.189676,0x78,, +3.189763,0x2E,, +3.18985,0xD8,, +3.189937,0x2E,, +3.190024,0xE4,, +3.190111,0x2E,, +3.190198,0xD8,, +3.190285,0x22,, +3.190372,0x60,, +3.190459,0x22,, +3.190546,0x60,, +3.190633,0x3B,, +3.19072,0x60,, +3.190806,0x22,, +3.190893,0x60,, +3.19098,0x22,, +3.191067,0x60,, +3.191154,0x3B,, +3.191241,0x60,, +3.191328,0x2E,, +3.191415,0xE0,, +3.191502,0x2E,, +3.191589,0xE0,, +3.191676,0x2D,, +3.191763,0xB2,, +3.199375,0xA8,, +3.199462,0x01,, +3.199549,0x0C,, +3.199636,0x22,, +3.199723,0x78,, +3.19981,0x2E,, +3.199897,0xD8,, +3.199984,0x2E,, +3.200071,0xE0,, +3.200158,0x2E,, +3.200245,0xD8,, +3.200332,0x22,, +3.200419,0x60,, +3.200505,0x22,, +3.200592,0x60,, +3.200679,0x3B,, +3.200766,0x60,, +3.200853,0x22,, +3.20094,0x60,, +3.201027,0x22,, +3.201114,0x60,, +3.201201,0x3B,, +3.201288,0x60,, +3.201375,0x2E,, +3.201462,0xE0,, +3.201549,0x2E,, +3.201636,0xE0,, +3.201723,0x65,, +3.20181,0xAD,, +3.209442,0xA8,, +3.209529,0x01,, +3.209616,0x0C,, +3.209703,0x22,, +3.20979,0x78,, +3.209877,0x2E,, +3.209964,0xD8,, +3.210051,0x2E,, +3.210138,0xE4,, +3.210225,0x2E,, +3.210312,0xD8,, +3.210399,0x22,, +3.210486,0x60,, +3.210573,0x22,, +3.210659,0x60,, +3.210746,0x3B,, +3.210833,0x60,, +3.21092,0x22,, +3.211007,0x60,, +3.211094,0x22,, +3.211181,0x60,, +3.211268,0x3B,, +3.211355,0x60,, +3.211442,0x2E,, +3.211529,0xE0,, +3.211616,0x2E,, +3.211703,0xE0,, +3.21179,0x2D,, +3.211877,0xB2,, +3.219489,0xA8,, +3.219576,0x01,, +3.219663,0x0C,, +3.21975,0x22,, +3.219837,0x78,, +3.219924,0x2E,, +3.220011,0xD8,, +3.220098,0x2E,, +3.220185,0xE8,, +3.220272,0x2E,, +3.220359,0xD8,, +3.220446,0x22,, +3.220533,0x60,, +3.220619,0x22,, +3.220706,0x60,, +3.220793,0x3B,, +3.22088,0x60,, +3.220967,0x22,, +3.221054,0x60,, +3.221141,0x22,, +3.221228,0x60,, +3.221315,0x3B,, +3.221402,0x60,, +3.221489,0x2E,, +3.221576,0xE0,, +3.221663,0x2E,, +3.22175,0xE0,, +3.221837,0xF5,, +3.221924,0x93,, +3.229535,0xA8,, +3.229622,0x01,, +3.229709,0x0C,, +3.229796,0x22,, +3.229883,0x6C,, +3.22997,0x2E,, +3.230057,0xD8,, +3.230144,0x2E,, +3.230231,0xE0,, +3.230318,0x2E,, +3.230405,0xD8,, +3.230492,0x22,, +3.230579,0x60,, +3.230666,0x22,, +3.230753,0x60,, +3.23084,0x3B,, +3.230926,0x60,, +3.231013,0x22,, +3.2311,0x60,, +3.231187,0x22,, +3.231274,0x60,, +3.231361,0x3B,, +3.231448,0x60,, +3.231535,0x2E,, +3.231622,0xE0,, +3.231709,0x2E,, +3.231796,0xE0,, +3.231883,0xD1,, +3.23197,0xD6,, +3.239582,0xA8,, +3.239669,0x01,, +3.239756,0x0C,, +3.239843,0x22,, +3.23993,0x74,, +3.240017,0x2E,, +3.240104,0xD8,, +3.240191,0x2E,, +3.240278,0xE4,, +3.240365,0x2E,, +3.240452,0xD8,, +3.240539,0x22,, +3.240626,0x60,, +3.240713,0x22,, +3.2408,0x60,, +3.240887,0x3B,, +3.240974,0x60,, +3.241061,0x22,, +3.241147,0x60,, +3.241234,0x22,, +3.241321,0x60,, +3.241408,0x3B,, +3.241495,0x60,, +3.241582,0x2E,, +3.241669,0xE0,, +3.241756,0x2E,, +3.241843,0xE0,, +3.24193,0xB1,, +3.242017,0x84,, +3.24963,0xA8,, +3.249717,0x01,, +3.249804,0x0C,, +3.249891,0x22,, +3.249978,0x74,, +3.250065,0x2E,, +3.250152,0xD8,, +3.250239,0x2E,, +3.250326,0xE0,, +3.250413,0x2E,, +3.2505,0xD8,, +3.250587,0x22,, +3.250674,0x60,, +3.250761,0x22,, +3.250848,0x60,, +3.250935,0x3B,, +3.251021,0x60,, +3.251108,0x22,, +3.251195,0x60,, +3.251282,0x22,, +3.251369,0x60,, +3.251456,0x3B,, +3.251543,0x60,, +3.25163,0x2E,, +3.251717,0xE0,, +3.251804,0x2E,, +3.251891,0xE0,, +3.251978,0xF9,, +3.252065,0x9B,, +3.259677,0xA8,, +3.259764,0x01,, +3.259851,0x0C,, +3.259938,0x22,, +3.260025,0x78,, +3.260112,0x2E,, +3.260199,0xD8,, +3.260286,0x2E,, +3.260373,0xE0,, +3.26046,0x2E,, +3.260547,0xD8,, +3.260634,0x22,, +3.260721,0x60,, +3.260807,0x22,, +3.260894,0x60,, +3.260981,0x3B,, +3.261068,0x60,, +3.261155,0x22,, +3.261242,0x60,, +3.261329,0x22,, +3.261416,0x60,, +3.261503,0x3B,, +3.26159,0x60,, +3.261677,0x2E,, +3.261764,0xE0,, +3.261851,0x2E,, +3.261938,0xE0,, +3.262025,0x65,, +3.262112,0xAD,, +3.269724,0xA8,, +3.269811,0x01,, +3.269898,0x0C,, +3.269985,0x22,, +3.270072,0x74,, +3.270159,0x2E,, +3.270246,0xD8,, +3.270333,0x2E,, +3.27042,0xE4,, +3.270506,0x2E,, +3.270593,0xD8,, +3.27068,0x22,, +3.270767,0x60,, +3.270854,0x22,, +3.270941,0x60,, +3.271028,0x3B,, +3.271115,0x60,, +3.271202,0x22,, +3.271289,0x60,, +3.271376,0x22,, +3.271463,0x60,, +3.27155,0x3B,, +3.271637,0x60,, +3.271724,0x2E,, +3.271811,0xE0,, +3.271898,0x2E,, +3.271984,0xE0,, +3.272071,0xB1,, +3.272158,0x84,, +3.279772,0xA8,, +3.279859,0x01,, +3.279946,0x0C,, +3.280033,0x22,, +3.28012,0x74,, +3.280206,0x2E,, +3.280293,0xD8,, +3.28038,0x2E,, +3.280467,0xE0,, +3.280554,0x2E,, +3.280641,0xD8,, +3.280728,0x22,, +3.280815,0x60,, +3.280902,0x22,, +3.280989,0x60,, +3.281076,0x3B,, +3.281163,0x60,, +3.28125,0x22,, +3.281337,0x60,, +3.281424,0x22,, +3.281511,0x60,, +3.281598,0x3B,, +3.281684,0x60,, +3.281771,0x2E,, +3.281858,0xE0,, +3.281945,0x2E,, +3.282032,0xE0,, +3.282119,0xF9,, +3.282206,0x9B,, +3.289818,0xA8,, +3.289905,0x01,, +3.289992,0x0C,, +3.290079,0x22,, +3.290166,0x78,, +3.290253,0x2E,, +3.29034,0xD8,, +3.290427,0x2E,, +3.290514,0xE4,, +3.290601,0x2E,, +3.290687,0xD8,, +3.290774,0x22,, +3.290861,0x60,, +3.290948,0x22,, +3.291035,0x60,, +3.291122,0x3B,, +3.291209,0x60,, +3.291296,0x22,, +3.291383,0x60,, +3.29147,0x22,, +3.291557,0x60,, +3.291644,0x3B,, +3.291731,0x60,, +3.291818,0x2E,, +3.291905,0xE0,, +3.291992,0x2E,, +3.292079,0xE0,, +3.292165,0x2D,, +3.292252,0xB2,, +3.299865,0xA8,, +3.299952,0x01,, +3.300039,0x0C,, +3.300126,0x22,, +3.300212,0x74,, +3.300299,0x2E,, +3.300386,0xD8,, +3.300473,0x2E,, +3.30056,0xE0,, +3.300647,0x2E,, +3.300734,0xD8,, +3.300821,0x22,, +3.300908,0x60,, +3.300995,0x22,, +3.301082,0x60,, +3.301169,0x3B,, +3.301256,0x60,, +3.301343,0x22,, +3.30143,0x60,, +3.301517,0x22,, +3.301604,0x60,, +3.30169,0x3B,, +3.301777,0x60,, +3.301864,0x2E,, +3.301951,0xE0,, +3.302038,0x2E,, +3.302125,0xE0,, +3.302212,0xF9,, +3.302299,0x9B,, +3.309912,0xA8,, +3.309999,0x01,, +3.310086,0x0C,, +3.310173,0x22,, +3.31026,0x74,, +3.310347,0x2E,, +3.310434,0xD8,, +3.310521,0x2E,, +3.310608,0xE4,, +3.310695,0x2E,, +3.310781,0xD8,, +3.310868,0x22,, +3.310955,0x60,, +3.311042,0x22,, +3.311129,0x60,, +3.311216,0x3B,, +3.311303,0x60,, +3.31139,0x22,, +3.311477,0x60,, +3.311564,0x22,, +3.311651,0x60,, +3.311738,0x3B,, +3.311825,0x60,, +3.311912,0x2E,, +3.311999,0xE0,, +3.312086,0x2E,, +3.312173,0xE0,, +3.31226,0xB1,, +3.312346,0x84,, +3.319959,0xA8,, +3.320046,0x01,, +3.320133,0x0C,, +3.32022,0x22,, +3.320307,0x78,, +3.320394,0x2E,, +3.320481,0xD8,, +3.320568,0x2E,, +3.320655,0xE4,, +3.320742,0x2E,, +3.320829,0xD8,, +3.320916,0x22,, +3.321002,0x60,, +3.321089,0x22,, +3.321176,0x60,, +3.321263,0x3B,, +3.32135,0x60,, +3.321437,0x22,, +3.321524,0x60,, +3.321611,0x22,, +3.321698,0x60,, +3.321785,0x3B,, +3.321872,0x60,, +3.321959,0x2E,, +3.322046,0xE0,, +3.322133,0x2E,, +3.32222,0xE0,, +3.322307,0x2D,, +3.322394,0xB2,, +3.330006,0xA8,, +3.330093,0x01,, +3.33018,0x0C,, +3.330267,0x22,, +3.330354,0x74,, +3.330441,0x2E,, +3.330527,0xD8,, +3.330614,0x2E,, +3.330701,0xE4,, +3.330788,0x2E,, +3.330875,0xD8,, +3.330962,0x22,, +3.331049,0x60,, +3.331136,0x22,, +3.331223,0x60,, +3.33131,0x3B,, +3.331397,0x60,, +3.331484,0x22,, +3.331571,0x60,, +3.331658,0x22,, +3.331745,0x60,, +3.331832,0x3B,, +3.331919,0x60,, +3.332005,0x2E,, +3.332092,0xE0,, +3.332179,0x2E,, +3.332266,0xE0,, +3.332353,0xB1,, +3.33244,0x84,, +3.340053,0xA8,, +3.34014,0x01,, +3.340227,0x0C,, +3.340314,0x22,, +3.340401,0x78,, +3.340488,0x2E,, +3.340575,0xD8,, +3.340662,0x2E,, +3.340749,0xE4,, +3.340836,0x2E,, +3.340923,0xD8,, +3.34101,0x22,, +3.341097,0x60,, +3.341183,0x22,, +3.34127,0x60,, +3.341357,0x3B,, +3.341444,0x60,, +3.341531,0x22,, +3.341618,0x60,, +3.341705,0x22,, +3.341792,0x60,, +3.341879,0x3B,, +3.341966,0x60,, +3.342053,0x2E,, +3.34214,0xE0,, +3.342227,0x2E,, +3.342314,0xE0,, +3.342401,0x2D,, +3.342488,0xB2,, +3.3501,0xA8,, +3.350187,0x01,, +3.350274,0x0C,, +3.350361,0x22,, +3.350448,0x74,, +3.350534,0x2E,, +3.350621,0xD8,, +3.350708,0x2E,, +3.350795,0xE0,, +3.350882,0x2E,, +3.350969,0xD8,, +3.351056,0x22,, +3.351143,0x60,, +3.35123,0x22,, +3.351317,0x60,, +3.351404,0x3B,, +3.351491,0x60,, +3.351578,0x22,, +3.351665,0x60,, +3.351752,0x22,, +3.351839,0x60,, +3.351926,0x3B,, +3.352012,0x60,, +3.352099,0x2E,, +3.352186,0xE0,, +3.352273,0x2E,, +3.35236,0xE0,, +3.352447,0xF9,, +3.352534,0x9B,, +3.360147,0xA8,, +3.360234,0x01,, +3.360321,0x0C,, +3.360408,0x22,, +3.360494,0x74,, +3.360581,0x2E,, +3.360668,0xD8,, +3.360755,0x2E,, +3.360842,0xE4,, +3.360929,0x2E,, +3.361016,0xD8,, +3.361103,0x22,, +3.36119,0x60,, +3.361277,0x22,, +3.361364,0x60,, +3.361451,0x3B,, +3.361538,0x60,, +3.361625,0x22,, +3.361712,0x60,, +3.361799,0x22,, +3.361886,0x60,, +3.361972,0x3B,, +3.362059,0x60,, +3.362146,0x2E,, +3.362233,0xE0,, +3.36232,0x2E,, +3.362407,0xE0,, +3.362494,0xB1,, +3.362581,0x84,, +3.370194,0xA8,, +3.370281,0x01,, +3.370368,0x0C,, +3.370455,0x22,, +3.370542,0x74,, +3.370629,0x2E,, +3.370716,0xD8,, +3.370802,0x2E,, +3.370889,0xE0,, +3.370976,0x2E,, +3.371063,0xD8,, +3.37115,0x22,, +3.371237,0x60,, +3.371324,0x22,, +3.371411,0x60,, +3.371498,0x3B,, +3.371585,0x60,, +3.371672,0x22,, +3.371759,0x60,, +3.371846,0x22,, +3.371933,0x60,, +3.37202,0x3B,, +3.372107,0x60,, +3.372193,0x2E,, +3.37228,0xE0,, +3.372367,0x2E,, +3.372454,0xE0,, +3.372541,0xF9,, +3.372628,0x9B,, +3.380241,0xA8,, +3.380328,0x01,, +3.380415,0x0C,, +3.380502,0x22,, +3.380589,0x74,, +3.380676,0x2E,, +3.380763,0xD8,, +3.380849,0x2E,, +3.380936,0xE4,, +3.381023,0x2E,, +3.38111,0xD8,, +3.381197,0x22,, +3.381284,0x60,, +3.381371,0x22,, +3.381458,0x60,, +3.381545,0x3B,, +3.381632,0x60,, +3.381719,0x22,, +3.381806,0x60,, +3.381893,0x22,, +3.38198,0x60,, +3.382067,0x3B,, +3.382154,0x60,, +3.382241,0x2E,, +3.382327,0xE0,, +3.382414,0x2E,, +3.382501,0xE0,, +3.382588,0xB1,, +3.382675,0x84,, +3.390288,0xA8,, +3.390375,0x01,, +3.390462,0x0C,, +3.390549,0x22,, +3.390636,0x78,, +3.390723,0x2E,, +3.39081,0xD8,, +3.390897,0x2E,, +3.390983,0xE4,, +3.39107,0x2E,, +3.391157,0xD8,, +3.391244,0x22,, +3.391331,0x60,, +3.391418,0x22,, +3.391505,0x60,, +3.391592,0x3B,, +3.391679,0x60,, +3.391766,0x22,, +3.391853,0x60,, +3.39194,0x22,, +3.392027,0x60,, +3.392114,0x3B,, +3.392201,0x60,, +3.392288,0x2E,, +3.392375,0xE0,, +3.392462,0x2E,, +3.392548,0xE0,, +3.392635,0x2D,, +3.392722,0xB2,, +3.400335,0xA8,, +3.400422,0x01,, +3.400509,0x0C,, +3.400596,0x22,, +3.400683,0x78,, +3.40077,0x2E,, +3.400857,0xD8,, +3.400944,0x2E,, +3.40103,0xE0,, +3.401117,0x2E,, +3.401204,0xD8,, +3.401291,0x22,, +3.401378,0x60,, +3.401465,0x22,, +3.401552,0x60,, +3.401639,0x3B,, +3.401726,0x60,, +3.401813,0x22,, +3.4019,0x60,, +3.401987,0x22,, +3.402074,0x60,, +3.402161,0x3B,, +3.402248,0x60,, +3.402335,0x2E,, +3.402422,0xE0,, +3.402508,0x2E,, +3.402595,0xE0,, +3.402682,0x65,, +3.402769,0xAD,, +3.410381,0xA8,, +3.410468,0x01,, +3.410555,0x0C,, +3.410642,0x22,, +3.410729,0x78,, +3.410816,0x2E,, +3.410903,0xD8,, +3.41099,0x2E,, +3.411077,0xE4,, +3.411164,0x2E,, +3.411251,0xD8,, +3.411338,0x22,, +3.411425,0x60,, +3.411512,0x22,, +3.411599,0x60,, +3.411685,0x3B,, +3.411772,0x60,, +3.411859,0x22,, +3.411946,0x60,, +3.412033,0x22,, +3.41212,0x60,, +3.412207,0x3B,, +3.412294,0x60,, +3.412381,0x2E,, +3.412468,0xE0,, +3.412555,0x2E,, +3.412642,0xE0,, +3.412729,0x2D,, +3.412816,0xB2,, +3.420429,0xA8,, +3.420516,0x01,, +3.420602,0x0C,, +3.420689,0x22,, +3.420776,0x8C,, +3.420863,0x2E,, +3.42095,0xD8,, +3.421037,0x2E,, +3.421124,0xE4,, +3.421211,0x2E,, +3.421298,0xD8,, +3.421385,0x22,, +3.421472,0x60,, +3.421559,0x22,, +3.421646,0x60,, +3.421733,0x3B,, +3.42182,0x60,, +3.421907,0x22,, +3.421994,0x60,, +3.42208,0x22,, +3.422167,0x60,, +3.422254,0x3B,, +3.422341,0x60,, +3.422428,0x2E,, +3.422515,0xE0,, +3.422602,0x2E,, +3.422689,0xE0,, +3.422776,0xAB,, +3.422863,0x6C,, +3.430496,0xA8,, +3.430583,0x01,, +3.43067,0x0C,, +3.430757,0x22,, +3.430844,0xD4,, +3.430931,0x2E,, +3.431018,0xD8,, +3.431105,0x2E,, +3.431192,0xE0,, +3.431279,0x2E,, +3.431366,0xDC,, +3.431452,0x22,, +3.431539,0x60,, +3.431626,0x22,, +3.431713,0x60,, +3.4318,0x3B,, +3.431887,0x60,, +3.431974,0x22,, +3.432061,0x60,, +3.432148,0x22,, +3.432235,0x60,, +3.432322,0x3B,, +3.432409,0x60,, +3.432496,0x2E,, +3.432583,0xE0,, +3.43267,0x2E,, +3.432757,0xE0,, +3.432844,0x49,, +3.432931,0x6E,, +3.440543,0xA8,, +3.44063,0x01,, +3.440717,0x0C,, +3.440804,0x23,, +3.440891,0x4C,, +3.440978,0x2E,, +3.441065,0xD8,, +3.441151,0x2E,, +3.441238,0xE0,, +3.441325,0x2E,, +3.441412,0xD8,, +3.441499,0x22,, +3.441586,0x60,, +3.441673,0x22,, +3.44176,0x60,, +3.441847,0x3B,, +3.441934,0x60,, +3.442021,0x22,, +3.442108,0x60,, +3.442195,0x22,, +3.442282,0x60,, +3.442369,0x3B,, +3.442456,0x60,, +3.442542,0x2E,, +3.442629,0xE0,, +3.442716,0x2E,, +3.442803,0xE0,, +3.44289,0x86,, +3.442977,0x39,, +3.45059,0xA8,, +3.450677,0x01,, +3.450764,0x0C,, +3.450851,0x23,, +3.450938,0xE0,, +3.451025,0x2E,, +3.451112,0xD8,, +3.451199,0x2E,, +3.451286,0xE4,, +3.451373,0x2E,, +3.45146,0xD8,, +3.451547,0x22,, +3.451634,0x60,, +3.451721,0x22,, +3.451808,0x60,, +3.451895,0x3B,, +3.451981,0x60,, +3.452068,0x22,, +3.452155,0x60,, +3.452242,0x22,, +3.452329,0x60,, +3.452416,0x3B,, +3.452503,0x60,, +3.45259,0x2E,, +3.452677,0xE0,, +3.452764,0x2E,, +3.452851,0xE0,, +3.452938,0xA1,, +3.453025,0x6D,, +3.460637,0xA8,, +3.460724,0x01,, +3.460811,0x0C,, +3.460898,0x24,, +3.460985,0x58,, +3.461072,0x2E,, +3.461159,0xD8,, +3.461246,0x2E,, +3.461333,0xE0,, +3.46142,0x2E,, +3.461507,0xDC,, +3.461594,0x22,, +3.461681,0x60,, +3.461768,0x22,, +3.461855,0x60,, +3.461941,0x3B,, +3.462028,0x60,, +3.462115,0x22,, +3.462202,0x60,, +3.462289,0x22,, +3.462376,0x60,, +3.462463,0x3B,, +3.46255,0x60,, +3.462637,0x2E,, +3.462724,0xE0,, +3.462811,0x2E,, +3.462898,0xE0,, +3.462985,0xF4,, +3.463072,0xC3,, +3.470685,0xA8,, +3.470772,0x01,, +3.470858,0x0C,, +3.470945,0x24,, +3.471032,0xD8,, +3.471119,0x2E,, +3.471206,0xD8,, +3.471293,0x2E,, +3.47138,0xE0,, +3.471467,0x2E,, +3.471554,0xD8,, +3.471641,0x22,, +3.471728,0x60,, +3.471815,0x22,, +3.471902,0x60,, +3.471989,0x3B,, +3.472076,0x60,, +3.472163,0x22,, +3.47225,0x60,, +3.472336,0x22,, +3.472423,0x60,, +3.47251,0x3B,, +3.472597,0x60,, +3.472684,0x2E,, +3.472771,0xE0,, +3.472858,0x2E,, +3.472945,0xE0,, +3.473032,0x24,, +3.473119,0xDA,, +3.480732,0xA8,, +3.480819,0x01,, +3.480906,0x0C,, +3.480993,0x25,, +3.48108,0x5C,, +3.481167,0x2E,, +3.481254,0xD8,, +3.48134,0x2E,, +3.481427,0xE0,, +3.481514,0x2E,, +3.481601,0xD8,, +3.481688,0x22,, +3.481775,0x60,, +3.481862,0x22,, +3.481949,0x60,, +3.482036,0x3B,, +3.482123,0x60,, +3.48221,0x22,, +3.482297,0x60,, +3.482384,0x22,, +3.482471,0x60,, +3.482558,0x3B,, +3.482645,0x60,, +3.482732,0x2E,, +3.482818,0xE0,, +3.482905,0x2E,, +3.482992,0xE0,, +3.483079,0x04,, +3.483166,0x45,, +3.490779,0xA8,, +3.490866,0x01,, +3.490953,0x0C,, +3.49104,0x25,, +3.491127,0xDC,, +3.491214,0x2E,, +3.491301,0xD8,, +3.491388,0x2E,, +3.491475,0xE0,, +3.491562,0x2E,, +3.491649,0xDC,, +3.491736,0x22,, +3.491823,0x60,, +3.49191,0x22,, +3.491997,0x60,, +3.492084,0x3B,, +3.49217,0x60,, +3.492257,0x22,, +3.492344,0x60,, +3.492431,0x22,, +3.492518,0x60,, +3.492605,0x3B,, +3.492692,0x60,, +3.492779,0x2E,, +3.492866,0xE0,, +3.492953,0x2E,, +3.49304,0xE0,, +3.493127,0xD4,, +3.493214,0x5C,, +3.500827,0xA8,, +3.500914,0x01,, +3.501001,0x0C,, +3.501088,0x26,, +3.501175,0x50,, +3.501262,0x2E,, +3.501349,0xD8,, +3.501436,0x2E,, +3.501523,0xE4,, +3.50161,0x2E,, +3.501697,0xD8,, +3.501784,0x22,, +3.501871,0x60,, +3.501958,0x22,, +3.502045,0x60,, +3.502131,0x3B,, +3.502218,0x60,, +3.502305,0x22,, +3.502392,0x60,, +3.502479,0x22,, +3.502566,0x60,, +3.502653,0x3B,, +3.50274,0x60,, +3.502827,0x2E,, +3.502914,0xE0,, +3.503001,0x2E,, +3.503088,0xE0,, +3.503175,0x89,, +3.503262,0x69,, +3.510875,0xA8,, +3.510962,0x01,, +3.511049,0x0C,, +3.511135,0x26,, +3.511222,0xD8,, +3.511309,0x2E,, +3.511396,0xD8,, +3.511483,0x2E,, +3.51157,0xE0,, +3.511657,0x2E,, +3.511744,0xDC,, +3.511831,0x22,, +3.511918,0x60,, +3.512005,0x22,, +3.512092,0x60,, +3.512179,0x3B,, +3.512266,0x60,, +3.512353,0x22,, +3.51244,0x60,, +3.512527,0x22,, +3.512613,0x60,, +3.5127,0x3B,, +3.512787,0x60,, +3.512874,0x2E,, +3.512961,0xE0,, +3.513048,0x2E,, +3.513135,0xE0,, +3.513222,0x09,, +3.513309,0x54,, +3.520922,0xA8,, +3.521009,0x01,, +3.521096,0x0C,, +3.521183,0x27,, +3.52127,0x50,, +3.521357,0x2E,, +3.521444,0xD8,, +3.521531,0x2E,, +3.521618,0xE0,, +3.521705,0x2E,, +3.521791,0xDC,, +3.521878,0x22,, +3.521965,0x60,, +3.522052,0x22,, +3.522139,0x60,, +3.522226,0x3B,, +3.522313,0x60,, +3.5224,0x22,, +3.522487,0x60,, +3.522574,0x22,, +3.522661,0x60,, +3.522748,0x3B,, +3.522835,0x60,, +3.522922,0x2E,, +3.523009,0xE0,, +3.523096,0x2E,, +3.523183,0xE0,, +3.523269,0xB5,, +3.523356,0xFD,, +3.530969,0xA8,, +3.531056,0x01,, +3.531143,0x0C,, +3.53123,0x27,, +3.531317,0xDC,, +3.531404,0x2E,, +3.531491,0xD8,, +3.531578,0x2E,, +3.531665,0xE0,, +3.531752,0x2E,, +3.531839,0xD8,, +3.531926,0x22,, +3.532012,0x60,, +3.532099,0x22,, +3.532186,0x60,, +3.532273,0x3B,, +3.53236,0x60,, +3.532447,0x22,, +3.532534,0x60,, +3.532621,0x22,, +3.532708,0x60,, +3.532795,0x3B,, +3.532882,0x60,, +3.532969,0x2E,, +3.533056,0xE0,, +3.533143,0x2E,, +3.53323,0xE0,, +3.533317,0xF9,, +3.533404,0xD2,, +3.541528,0xA8,, +3.541615,0x01,, +3.541702,0x0C,, +3.541789,0x28,, +3.541876,0x70,, +3.541963,0x2E,, +3.54205,0xD8,, +3.542137,0x2E,, +3.542224,0xE0,, +3.54231,0x2E,, +3.542397,0xD8,, +3.542484,0x22,, +3.542571,0x60,, +3.542658,0x22,, +3.542745,0x60,, +3.542832,0x3B,, +3.542919,0x60,, +3.543006,0x22,, +3.543093,0x60,, +3.54318,0x22,, +3.543267,0x60,, +3.543354,0x3B,, +3.543441,0x60,, +3.543528,0x2E,, +3.543615,0xE0,, +3.543702,0x2E,, +3.543788,0xE0,, +3.543875,0xBB,, +3.543962,0xA9,, +3.551532,0xA8,, +3.551619,0x01,, +3.551706,0x0C,, +3.551793,0x29,, +3.55188,0x08,, +3.551967,0x2E,, +3.552054,0xD8,, +3.552141,0x2E,, +3.552228,0xE0,, +3.552315,0x2E,, +3.552402,0xDC,, +3.552489,0x22,, +3.552576,0x60,, +3.552663,0x22,, +3.55275,0x60,, +3.552836,0x3B,, +3.552923,0x60,, +3.55301,0x22,, +3.553097,0x60,, +3.553184,0x22,, +3.553271,0x60,, +3.553358,0x3B,, +3.553445,0x60,, +3.553532,0x2E,, +3.553619,0xE0,, +3.553706,0x2E,, +3.553793,0xE0,, +3.55388,0x46,, +3.553967,0x5B,, +3.561535,0xA8,, +3.561622,0x01,, +3.561709,0x0C,, +3.561796,0x29,, +3.561883,0xAC,, +3.56197,0x2E,, +3.562057,0xD8,, +3.562144,0x2E,, +3.562231,0xE0,, +3.562318,0x2E,, +3.562405,0xDC,, +3.562492,0x22,, +3.562578,0x60,, +3.562665,0x22,, +3.562752,0x60,, +3.562839,0x3B,, +3.562926,0x60,, +3.563013,0x22,, +3.5631,0x60,, +3.563187,0x22,, +3.563274,0x60,, +3.563361,0x3B,, +3.563448,0x60,, +3.563535,0x2E,, +3.563622,0xE0,, +3.563709,0x2E,, +3.563796,0xE0,, +3.563883,0x31,, +3.56397,0x2B,, +3.571538,0xA8,, +3.571625,0x01,, +3.571711,0x0C,, +3.571798,0x2A,, +3.571885,0x48,, +3.571972,0x2E,, +3.572059,0xD8,, +3.572146,0x2E,, +3.572233,0xE0,, +3.57232,0x2E,, +3.572407,0xDC,, +3.572494,0x22,, +3.572581,0x60,, +3.572668,0x22,, +3.572755,0x60,, +3.572842,0x3B,, +3.572929,0x60,, +3.573016,0x22,, +3.573103,0x60,, +3.573189,0x22,, +3.573276,0x60,, +3.573363,0x3B,, +3.57345,0x60,, +3.573537,0x2E,, +3.573624,0xE0,, +3.573711,0x2E,, +3.573798,0xE0,, +3.573885,0xDE,, +3.573972,0x86,, +3.58154,0xA8,, +3.581627,0x01,, +3.581714,0x0C,, +3.581801,0x2A,, +3.581888,0xCC,, +3.581975,0x2E,, +3.582062,0xD8,, +3.582149,0x2E,, +3.582236,0xE0,, +3.582323,0x2E,, +3.58241,0xDC,, +3.582496,0x22,, +3.582583,0x60,, +3.58267,0x22,, +3.582757,0x60,, +3.582844,0x3B,, +3.582931,0x60,, +3.583018,0x22,, +3.583105,0x60,, +3.583192,0x22,, +3.583279,0x60,, +3.583366,0x3B,, +3.583453,0x60,, +3.58354,0x2E,, +3.583627,0xE0,, +3.583714,0x2E,, +3.583801,0xE0,, +3.583888,0xC9,, +3.583974,0x1A,, +3.591544,0xA8,, +3.591631,0x01,, +3.591718,0x0C,, +3.591805,0x2B,, +3.591892,0x64,, +3.591979,0x2E,, +3.592065,0xD8,, +3.592152,0x2E,, +3.592239,0xE0,, +3.592326,0x2E,, +3.592413,0xDC,, +3.5925,0x22,, +3.592587,0x60,, +3.592674,0x22,, +3.592761,0x60,, +3.592848,0x3B,, +3.592935,0x60,, +3.593022,0x22,, +3.593109,0x60,, +3.593196,0x22,, +3.593283,0x60,, +3.59337,0x3B,, +3.593456,0x60,, +3.593543,0x2E,, +3.59363,0xE0,, +3.593717,0x2E,, +3.593804,0xE0,, +3.593891,0x15,, +3.593978,0x5F,, +3.601546,0xA8,, +3.601633,0x01,, +3.60172,0x0C,, +3.601807,0x2B,, +3.601893,0xE4,, +3.60198,0x2E,, +3.602067,0xD8,, +3.602154,0x2E,, +3.602241,0xE0,, +3.602328,0x2E,, +3.602415,0xDC,, +3.602502,0x22,, +3.602589,0x60,, +3.602676,0x22,, +3.602763,0x60,, +3.60285,0x3B,, +3.602937,0x60,, +3.603024,0x22,, +3.603111,0x60,, +3.603198,0x22,, +3.603285,0x60,, +3.603371,0x3B,, +3.603458,0x60,, +3.603545,0x2E,, +3.603632,0xE0,, +3.603719,0x2E,, +3.603806,0xE0,, +3.603893,0x86,, +3.60398,0xCE,, +3.61155,0xA8,, +3.611636,0x01,, +3.611723,0x0C,, +3.61181,0x2C,, +3.611897,0x84,, +3.611984,0x2E,, +3.612071,0xD8,, +3.612158,0x2E,, +3.612245,0xE4,, +3.612332,0x2E,, +3.612419,0xDC,, +3.612506,0x22,, +3.612593,0x60,, +3.61268,0x22,, +3.612767,0x60,, +3.612854,0x3B,, +3.612941,0x60,, +3.613028,0x22,, +3.613114,0x60,, +3.613201,0x22,, +3.613288,0x60,, +3.613375,0x3B,, +3.613462,0x60,, +3.613549,0x2E,, +3.613636,0xE0,, +3.613723,0x2E,, +3.61381,0xE0,, +3.613897,0xEA,, +3.613984,0xEC,, +3.621551,0xA8,, +3.621638,0x01,, +3.621725,0x0C,, +3.621812,0x2D,, +3.621899,0x14,, +3.621986,0x2E,, +3.622073,0xD8,, +3.62216,0x2E,, +3.622247,0xE0,, +3.622333,0x2E,, +3.62242,0xDC,, +3.622507,0x22,, +3.622594,0x60,, +3.622681,0x22,, +3.622768,0x60,, +3.622855,0x3B,, +3.622942,0x60,, +3.623029,0x22,, +3.623116,0x60,, +3.623203,0x22,, +3.62329,0x60,, +3.623377,0x3B,, +3.623464,0x60,, +3.623551,0x2E,, +3.623638,0xE0,, +3.623725,0x2E,, +3.623811,0xE0,, +3.623898,0x36,, +3.623985,0x17,, +3.631554,0xA8,, +3.631641,0x01,, +3.631728,0x0C,, +3.631815,0x2D,, +3.631902,0xA0,, +3.631989,0x2E,, +3.632076,0xD8,, +3.632163,0x2E,, +3.63225,0xE0,, +3.632337,0x2E,, +3.632424,0xDC,, +3.632511,0x22,, +3.632597,0x60,, +3.632684,0x22,, +3.632771,0x60,, +3.632858,0x3B,, +3.632945,0x60,, +3.633032,0x22,, +3.633119,0x60,, +3.633206,0x22,, +3.633293,0x60,, +3.63338,0x3B,, +3.633467,0x60,, +3.633554,0x2E,, +3.633641,0xE0,, +3.633728,0x2E,, +3.633815,0xE0,, +3.633902,0x71,, +3.633989,0x11,, +3.641557,0xA8,, +3.641644,0x01,, +3.641731,0x0C,, +3.641818,0x2E,, +3.641905,0x44,, +3.641992,0x2E,, +3.642079,0xD8,, +3.642166,0x2E,, +3.642253,0xE0,, +3.64234,0x2E,, +3.642427,0xDC,, +3.642514,0x22,, +3.642601,0x60,, +3.642687,0x22,, +3.642774,0x60,, +3.642861,0x3B,, +3.642948,0x60,, +3.643035,0x22,, +3.643122,0x60,, +3.643209,0x22,, +3.643296,0x60,, +3.643383,0x3B,, +3.64347,0x60,, +3.643557,0x2E,, +3.643644,0xE0,, +3.643731,0x2E,, +3.643818,0xE0,, +3.643905,0x9E,, +3.643992,0xBC,, +3.651559,0xA8,, +3.651646,0x01,, +3.651733,0x0C,, +3.65182,0x2E,, +3.651907,0xD8,, +3.651994,0x2E,, +3.652081,0xD4,, +3.652168,0x2E,, +3.652255,0xE0,, +3.652342,0x2E,, +3.652429,0xDC,, +3.652516,0x22,, +3.652603,0x60,, +3.65269,0x22,, +3.652777,0x60,, +3.652864,0x3B,, +3.65295,0x60,, +3.653037,0x22,, +3.653124,0x60,, +3.653211,0x22,, +3.653298,0x60,, +3.653385,0x3B,, +3.653472,0x60,, +3.653559,0x2E,, +3.653646,0xE0,, +3.653733,0x2E,, +3.65382,0xE0,, +3.653907,0x09,, +3.653994,0xA0,, +3.661698,0xA8,, +3.661784,0x01,, +3.661871,0x0C,, +3.661958,0x2F,, +3.662045,0x74,, +3.662132,0x2E,, +3.662219,0xD8,, +3.662306,0x2E,, +3.662393,0xE0,, +3.66248,0x2E,, +3.662567,0xE0,, +3.662654,0x22,, +3.662741,0x60,, +3.662828,0x22,, +3.662915,0x60,, +3.663002,0x3B,, +3.663089,0x60,, +3.663176,0x22,, +3.663262,0x60,, +3.663349,0x22,, +3.663436,0x60,, +3.663523,0x3B,, +3.66361,0x60,, +3.663697,0x2E,, +3.663784,0xE0,, +3.663871,0x2E,, +3.663958,0xE0,, +3.664045,0x1F,, +3.664132,0xBE,, +3.671724,0xA8,, +3.671811,0x01,, +3.671898,0x0C,, +3.671985,0x30,, +3.672072,0x10,, +3.672159,0x2E,, +3.672246,0xD8,, +3.672332,0x2E,, +3.672419,0xE0,, +3.672506,0x2E,, +3.672593,0xDC,, +3.67268,0x22,, +3.672767,0x60,, +3.672854,0x22,, +3.672941,0x60,, +3.673028,0x3B,, +3.673115,0x60,, +3.673202,0x22,, +3.673289,0x60,, +3.673376,0x22,, +3.673463,0x60,, +3.67355,0x3B,, +3.673637,0x60,, +3.673724,0x2E,, +3.67381,0xE0,, +3.673897,0x2E,, +3.673984,0xE0,, +3.674071,0xB1,, +3.674158,0x7F,, +3.681751,0xA8,, +3.681838,0x01,, +3.681925,0x0C,, +3.682012,0x30,, +3.682099,0xC0,, +3.682186,0x2E,, +3.682273,0xD8,, +3.68236,0x2E,, +3.682447,0xE4,, +3.682534,0x2E,, +3.682621,0xDC,, +3.682708,0x22,, +3.682795,0x60,, +3.682882,0x22,, +3.682969,0x60,, +3.683055,0x3B,, +3.683142,0x60,, +3.683229,0x22,, +3.683316,0x60,, +3.683403,0x22,, +3.68349,0x60,, +3.683577,0x3B,, +3.683664,0x60,, +3.683751,0x2E,, +3.683838,0xE0,, +3.683925,0x2E,, +3.684012,0xE0,, +3.684099,0x9B,, +3.684186,0x5F,, +3.691737,0xA8,, +3.691823,0x01,, +3.69191,0x0C,, +3.691997,0x31,, +3.692084,0x68,, +3.692171,0x2E,, +3.692258,0xD8,, +3.692345,0x2E,, +3.692432,0xE0,, +3.692519,0x2E,, +3.692606,0xDC,, +3.692693,0x22,, +3.69278,0x60,, +3.692867,0x22,, +3.692954,0x60,, +3.693041,0x3B,, +3.693128,0x60,, +3.693215,0x22,, +3.693301,0x60,, +3.693388,0x22,, +3.693475,0x60,, +3.693562,0x3B,, +3.693649,0x60,, +3.693736,0x2E,, +3.693823,0xE0,, +3.69391,0x2E,, +3.693997,0xE0,, +3.694084,0x0F,, +3.694171,0x05,, +3.701778,0xA8,, +3.701865,0x01,, +3.701952,0x0C,, +3.702039,0x32,, +3.702126,0x10,, +3.702213,0x2E,, +3.702299,0xD8,, +3.702386,0x2E,, +3.702473,0xE0,, +3.70256,0x2E,, +3.702647,0xDC,, +3.702734,0x22,, +3.702821,0x60,, +3.702908,0x22,, +3.702995,0x60,, +3.703082,0x3B,, +3.703169,0x60,, +3.703256,0x22,, +3.703343,0x60,, +3.70343,0x22,, +3.703517,0x60,, +3.703604,0x3B,, +3.703691,0x60,, +3.703777,0x2E,, +3.703864,0xE0,, +3.703951,0x2E,, +3.704038,0xE0,, +3.704125,0xDF,, +3.704212,0x79,, +3.711825,0xA8,, +3.711912,0x01,, +3.711999,0x0C,, +3.712086,0x32,, +3.712173,0xA8,, +3.71226,0x2E,, +3.712347,0xD8,, +3.712434,0x2E,, +3.712521,0xE0,, +3.712608,0x2E,, +3.712695,0xDC,, +3.712782,0x22,, +3.712869,0x60,, +3.712956,0x22,, +3.713043,0x60,, +3.71313,0x3B,, +3.713216,0x60,, +3.713303,0x22,, +3.71339,0x60,, +3.713477,0x22,, +3.713564,0x60,, +3.713651,0x3B,, +3.713738,0x60,, +3.713825,0x2E,, +3.713912,0xE0,, +3.713999,0x2E,, +3.714086,0xE0,, +3.714173,0x04,, +3.71426,0x49,, +3.721977,0xA8,, +3.722064,0x01,, +3.722151,0x0C,, +3.722238,0x33,, +3.722325,0x28,, +3.722412,0x2E,, +3.722499,0xD8,, +3.722585,0x2E,, +3.722672,0xE0,, +3.722759,0x2E,, +3.722846,0xDC,, +3.722933,0x22,, +3.72302,0x60,, +3.723107,0x22,, +3.723194,0x60,, +3.723281,0x3B,, +3.723368,0x60,, +3.723455,0x22,, +3.723542,0x60,, +3.723629,0x22,, +3.723716,0x60,, +3.723803,0x3B,, +3.72389,0x60,, +3.723976,0x2E,, +3.724063,0xE0,, +3.72415,0x2E,, +3.724237,0xE0,, +3.724324,0xA0,, +3.724411,0xDB,, +3.731958,0xA8,, +3.732045,0x01,, +3.732132,0x0C,, +3.732219,0x33,, +3.732306,0xAC,, +3.732393,0x2E,, +3.73248,0xD8,, +3.732567,0x2E,, +3.732654,0xE0,, +3.732741,0x2E,, +3.732827,0xDC,, +3.732914,0x22,, +3.733001,0x60,, +3.733088,0x22,, +3.733175,0x60,, +3.733262,0x3B,, +3.733349,0x60,, +3.733436,0x22,, +3.733523,0x60,, +3.73361,0x22,, +3.733697,0x60,, +3.733784,0x3B,, +3.733871,0x60,, +3.733958,0x2E,, +3.734045,0xE0,, +3.734132,0x2E,, +3.734219,0xE0,, +3.734305,0xB7,, +3.734392,0x47,, +3.741986,0xA8,, +3.742073,0x01,, +3.74216,0x0C,, +3.742247,0x34,, +3.742334,0x28,, +3.742421,0x2E,, +3.742508,0xD8,, +3.742595,0x2E,, +3.742682,0xE0,, +3.742769,0x2E,, +3.742856,0xDC,, +3.742943,0x22,, +3.74303,0x60,, +3.743117,0x22,, +3.743204,0x60,, +3.74329,0x3B,, +3.743377,0x60,, +3.743464,0x22,, +3.743551,0x60,, +3.743638,0x22,, +3.743725,0x60,, +3.743812,0x3B,, +3.743899,0x60,, +3.743986,0x2E,, +3.744073,0xE0,, +3.74416,0x2E,, +3.744247,0xE0,, +3.744334,0x25,, +3.744421,0xD2,, +3.752016,0xA8,, +3.752103,0x01,, +3.75219,0x0C,, +3.752277,0x34,, +3.752364,0xB8,, +3.752451,0x2E,, +3.752538,0xD8,, +3.752625,0x2E,, +3.752711,0xE4,, +3.752798,0x2E,, +3.752885,0xDC,, +3.752972,0x22,, +3.753059,0x60,, +3.753146,0x22,, +3.753233,0x60,, +3.75332,0x3B,, +3.753407,0x60,, +3.753494,0x22,, +3.753581,0x60,, +3.753668,0x22,, +3.753755,0x60,, +3.753842,0x3B,, +3.753929,0x60,, +3.754016,0x2E,, +3.754103,0xE0,, +3.75419,0x2E,, +3.754276,0xE0,, +3.754363,0xCE,, +3.75445,0x2A,, +3.762063,0xA8,, +3.76215,0x01,, +3.762237,0x0C,, +3.762324,0x35,, +3.762411,0x30,, +3.762498,0x2E,, +3.762585,0xD8,, +3.762672,0x2E,, +3.762759,0xE0,, +3.762846,0x2E,, +3.762933,0xDC,, +3.76302,0x22,, +3.763107,0x60,, +3.763194,0x22,, +3.76328,0x60,, +3.763367,0x3B,, +3.763454,0x60,, +3.763541,0x22,, +3.763628,0x60,, +3.763715,0x22,, +3.763802,0x60,, +3.763889,0x3B,, +3.763976,0x60,, +3.764063,0x2E,, +3.76415,0xE0,, +3.764237,0x2E,, +3.764324,0xE0,, +3.764411,0x3A,, +3.764498,0x9C,, +3.772155,0xA8,, +3.772242,0x01,, +3.772329,0x0C,, +3.772415,0x35,, +3.772502,0xA0,, +3.772589,0x2E,, +3.772676,0xD8,, +3.772763,0x2E,, +3.77285,0xE0,, +3.772937,0x2E,, +3.773024,0xDC,, +3.773111,0x22,, +3.773198,0x60,, +3.773285,0x22,, +3.773372,0x60,, +3.773459,0x3B,, +3.773546,0x60,, +3.773633,0x22,, +3.77372,0x60,, +3.773807,0x22,, +3.773894,0x60,, +3.77398,0x3B,, +3.774067,0x60,, +3.774154,0x2E,, +3.774241,0xE0,, +3.774328,0x2E,, +3.774415,0xE0,, +3.774502,0x99,, +3.774589,0x7B,, +3.782182,0xA8,, +3.782269,0x01,, +3.782356,0x0C,, +3.782443,0x36,, +3.78253,0x1C,, +3.782617,0x2E,, +3.782704,0xD8,, +3.782791,0x2E,, +3.782877,0xE0,, +3.782964,0x2E,, +3.783051,0xDC,, +3.783138,0x22,, +3.783225,0x60,, +3.783312,0x22,, +3.783399,0x60,, +3.783486,0x3B,, +3.783573,0x60,, +3.78366,0x22,, +3.783747,0x60,, +3.783834,0x22,, +3.783921,0x60,, +3.784008,0x3B,, +3.784095,0x60,, +3.784182,0x2E,, +3.784269,0xE0,, +3.784355,0x2E,, +3.784442,0xE0,, +3.784529,0x9F,, +3.784616,0x43,, +3.792205,0xA8,, +3.792292,0x01,, +3.792379,0x0C,, +3.792466,0x36,, +3.792553,0x94,, +3.79264,0x2E,, +3.792727,0xD8,, +3.792814,0x2E,, +3.792901,0xE0,, +3.792988,0x2E,, +3.793075,0xDC,, +3.793162,0x22,, +3.793249,0x60,, +3.793336,0x22,, +3.793423,0x60,, +3.79351,0x3B,, +3.793597,0x60,, +3.793684,0x22,, +3.79377,0x60,, +3.793857,0x22,, +3.793944,0x60,, +3.794031,0x3B,, +3.794118,0x60,, +3.794205,0x2E,, +3.794292,0xE0,, +3.794379,0x2E,, +3.794466,0xE0,, +3.794553,0x14,, +3.79464,0xE9,, +3.802254,0xA8,, +3.802341,0x01,, +3.802428,0x0C,, +3.802515,0x37,, +3.802602,0x08,, +3.802689,0x2E,, +3.802776,0xD8,, +3.802862,0x2E,, +3.802949,0xE4,, +3.803036,0x2E,, +3.803123,0xDC,, +3.80321,0x22,, +3.803297,0x60,, +3.803384,0x22,, +3.803471,0x60,, +3.803558,0x3B,, +3.803645,0x60,, +3.803732,0x22,, +3.803819,0x60,, +3.803906,0x22,, +3.803993,0x60,, +3.80408,0x3B,, +3.804167,0x60,, +3.804253,0x2E,, +3.80434,0xE0,, +3.804427,0x2E,, +3.804514,0xE0,, +3.804601,0x54,, +3.804688,0x24,, +3.812302,0xA8,, +3.812389,0x01,, +3.812475,0x0C,, +3.812562,0x37,, +3.812649,0x84,, +3.812736,0x2E,, +3.812823,0xD4,, +3.81291,0x2E,, +3.812997,0xE4,, +3.813084,0x2E,, +3.813171,0xDC,, +3.813258,0x22,, +3.813345,0x60,, +3.813432,0x22,, +3.813519,0x60,, +3.813606,0x3B,, +3.813693,0x60,, +3.81378,0x22,, +3.813867,0x60,, +3.813953,0x22,, +3.81404,0x60,, +3.814127,0x3B,, +3.814214,0x60,, +3.814301,0x2E,, +3.814388,0xE0,, +3.814475,0x2E,, +3.814562,0xE0,, +3.814649,0xF3,, +3.814736,0x4E,, +3.822349,0xA8,, +3.822436,0x01,, +3.822523,0x0C,, +3.82261,0x37,, +3.822697,0xF8,, +3.822784,0x2E,, +3.822871,0xD8,, +3.822958,0x2E,, +3.823045,0xE0,, +3.823131,0x2E,, +3.823218,0xDC,, +3.823305,0x22,, +3.823392,0x60,, +3.823479,0x22,, +3.823566,0x60,, +3.823653,0x3B,, +3.82374,0x60,, +3.823827,0x22,, +3.823914,0x60,, +3.824001,0x22,, +3.824088,0x60,, +3.824175,0x3B,, +3.824262,0x60,, +3.824349,0x2E,, +3.824436,0xE0,, +3.824523,0x2E,, +3.824609,0xE0,, +3.824696,0x1E,, +3.824783,0xE8,, +3.832397,0xA8,, +3.832484,0x01,, +3.832571,0x0C,, +3.832658,0x38,, +3.832745,0x60,, +3.832832,0x2E,, +3.832919,0xD8,, +3.833006,0x2E,, +3.833093,0xE0,, +3.83318,0x2E,, +3.833267,0xDC,, +3.833354,0x22,, +3.833441,0x60,, +3.833528,0x22,, +3.833615,0x60,, +3.833701,0x3B,, +3.833788,0x60,, +3.833875,0x22,, +3.833962,0x60,, +3.834049,0x22,, +3.834136,0x60,, +3.834223,0x3B,, +3.83431,0x60,, +3.834397,0x2E,, +3.834484,0xE0,, +3.834571,0x2E,, +3.834658,0xE0,, +3.834745,0x88,, +3.834832,0x04,, +3.842453,0xA8,, +3.84254,0x01,, +3.842627,0x0C,, +3.842714,0x38,, +3.842801,0xC4,, +3.842888,0x2E,, +3.842975,0xD8,, +3.843062,0x2E,, +3.843149,0xE0,, +3.843236,0x2E,, +3.843323,0xDC,, +3.84341,0x22,, +3.843497,0x60,, +3.843584,0x22,, +3.843671,0x60,, +3.843757,0x3B,, +3.843844,0x60,, +3.843931,0x22,, +3.844018,0x60,, +3.844105,0x22,, +3.844192,0x60,, +3.844279,0x3B,, +3.844366,0x60,, +3.844453,0x2E,, +3.84454,0xE0,, +3.844627,0x2E,, +3.844714,0xE0,, +3.844801,0xFF,, +3.844888,0x74,, +3.852492,0xA8,, +3.852579,0x01,, +3.852666,0x0C,, +3.852753,0x39,, +3.85284,0x70,, +3.852927,0x2E,, +3.853013,0xD0,, +3.8531,0x2E,, +3.853187,0xE0,, +3.853274,0x2E,, +3.853361,0xDC,, +3.853448,0x22,, +3.853535,0x60,, +3.853622,0x22,, +3.853709,0x60,, +3.853796,0x3B,, +3.853883,0x60,, +3.85397,0x22,, +3.854057,0x60,, +3.854144,0x22,, +3.854231,0x60,, +3.854318,0x3B,, +3.854405,0x60,, +3.854491,0x2E,, +3.854578,0xE0,, +3.854665,0x2E,, +3.854752,0xE0,, +3.854839,0x40,, +3.854926,0x07,, +3.862539,0xA8,, +3.862626,0x01,, +3.862713,0x0C,, +3.8628,0x39,, +3.862887,0xBC,, +3.862974,0x2E,, +3.863061,0xD8,, +3.863148,0x2E,, +3.863235,0xE0,, +3.863322,0x2E,, +3.863409,0xDC,, +3.863496,0x22,, +3.863583,0x60,, +3.86367,0x22,, +3.863757,0x60,, +3.863843,0x3B,, +3.86393,0x60,, +3.864017,0x22,, +3.864104,0x60,, +3.864191,0x22,, +3.864278,0x60,, +3.864365,0x3B,, +3.864452,0x60,, +3.864539,0x2E,, +3.864626,0xE0,, +3.864713,0x2E,, +3.8648,0xE0,, +3.864887,0x41,, +3.864974,0x0E,, +3.872587,0xA8,, +3.872674,0x01,, +3.872761,0x0C,, +3.872848,0x39,, +3.872935,0xF8,, +3.873021,0x2E,, +3.873108,0xD8,, +3.873195,0x2E,, +3.873282,0xE4,, +3.873369,0x2E,, +3.873456,0xDC,, +3.873543,0x22,, +3.87363,0x60,, +3.873717,0x22,, +3.873804,0x60,, +3.873891,0x3B,, +3.873978,0x60,, +3.874065,0x22,, +3.874152,0x60,, +3.874239,0x22,, +3.874326,0x60,, +3.874413,0x3B,, +3.874499,0x60,, +3.874586,0x2E,, +3.874673,0xE0,, +3.87476,0x2E,, +3.874847,0xE0,, +3.874934,0x4C,, +3.875021,0xC4,, +3.882634,0xA8,, +3.882721,0x01,, +3.882808,0x0C,, +3.882895,0x3A,, +3.882982,0x30,, +3.883069,0x2E,, +3.883155,0xD8,, +3.883242,0x2E,, +3.883329,0xE0,, +3.883416,0x2E,, +3.883503,0xDC,, +3.88359,0x22,, +3.883677,0x60,, +3.883764,0x22,, +3.883851,0x60,, +3.883938,0x3B,, +3.884025,0x60,, +3.884112,0x22,, +3.884199,0x60,, +3.884286,0x22,, +3.884373,0x60,, +3.88446,0x3B,, +3.884547,0x60,, +3.884633,0x2E,, +3.88472,0xE0,, +3.884807,0x2E,, +3.884894,0xE0,, +3.884981,0x17,, +3.885068,0xAC,, +3.892681,0xA8,, +3.892768,0x01,, +3.892855,0x0C,, +3.892942,0x3A,, +3.893029,0x60,, +3.893116,0x2E,, +3.893203,0xD8,, +3.89329,0x2E,, +3.893377,0xE0,, +3.893464,0x2E,, +3.893551,0xDC,, +3.893638,0x22,, +3.893725,0x60,, +3.893812,0x22,, +3.893898,0x60,, +3.893985,0x3B,, +3.894072,0x60,, +3.894159,0x22,, +3.894246,0x60,, +3.894333,0x22,, +3.89442,0x60,, +3.894507,0x3B,, +3.894594,0x60,, +3.894681,0x2E,, +3.894768,0xE0,, +3.894855,0x2E,, +3.894942,0xE0,, +3.895029,0xE6,, +3.895116,0x02,, +3.902728,0xA8,, +3.902815,0x01,, +3.902902,0x0C,, +3.902989,0x3A,, +3.903076,0x80,, +3.903163,0x2E,, +3.90325,0xD8,, +3.903337,0x2E,, +3.903424,0xE0,, +3.903511,0x2E,, +3.903598,0xDC,, +3.903685,0x22,, +3.903772,0x60,, +3.903859,0x22,, +3.903946,0x60,, +3.904032,0x3B,, +3.904119,0x60,, +3.904206,0x22,, +3.904293,0x60,, +3.90438,0x22,, +3.904467,0x60,, +3.904554,0x3B,, +3.904641,0x60,, +3.904728,0x2E,, +3.904815,0xE0,, +3.904902,0x2E,, +3.904989,0xE0,, +3.905076,0xD4,, +3.905163,0xA7,, +3.912776,0xA8,, +3.912863,0x01,, +3.912949,0x0C,, +3.913036,0x3A,, +3.913123,0xA0,, +3.91321,0x2E,, +3.913297,0xD8,, +3.913384,0x2E,, +3.913471,0xE0,, +3.913558,0x2E,, +3.913645,0xDC,, +3.913732,0x22,, +3.913819,0x60,, +3.913906,0x22,, +3.913993,0x60,, +3.91408,0x3B,, +3.914167,0x60,, +3.914254,0x22,, +3.914341,0x60,, +3.914427,0x22,, +3.914514,0x60,, +3.914601,0x3B,, +3.914688,0x60,, +3.914775,0x2E,, +3.914862,0xE0,, +3.914949,0x2E,, +3.915036,0xE0,, +3.915123,0xB4,, +3.91521,0x4B,, +3.922823,0xA8,, +3.92291,0x01,, +3.922996,0x0C,, +3.923083,0x3A,, +3.92317,0xB8,, +3.923257,0x2E,, +3.923344,0xD8,, +3.923431,0x2E,, +3.923518,0xE0,, +3.923605,0x2E,, +3.923692,0xDC,, +3.923779,0x22,, +3.923866,0x60,, +3.923953,0x22,, +3.92404,0x60,, +3.924127,0x3B,, +3.924214,0x60,, +3.924301,0x22,, +3.924388,0x60,, +3.924474,0x22,, +3.924561,0x60,, +3.924648,0x3B,, +3.924735,0x60,, +3.924822,0x2E,, +3.924909,0xE0,, +3.924996,0x2E,, +3.925083,0xE0,, +3.92517,0x9C,, +3.925257,0x06,, +3.93287,0xA8,, +3.932957,0x01,, +3.933044,0x0C,, +3.933131,0x3A,, +3.933218,0xCC,, +3.933305,0x2E,, +3.933392,0xD8,, +3.933479,0x2E,, +3.933566,0xE0,, +3.933653,0x2E,, +3.93374,0xE0,, +3.933827,0x22,, +3.933914,0x60,, +3.934,0x22,, +3.934087,0x60,, +3.934174,0x3B,, +3.934261,0x60,, +3.934348,0x22,, +3.934435,0x60,, +3.934522,0x22,, +3.934609,0x60,, +3.934696,0x3B,, +3.934783,0x60,, +3.93487,0x2E,, +3.934957,0xE0,, +3.935044,0x2E,, +3.935131,0xE0,, +3.935218,0x6F,, +3.935305,0xD2,, +3.942918,0xA8,, +3.943005,0x01,, +3.943092,0x0C,, +3.943179,0x3A,, +3.943266,0xE4,, +3.943352,0x2E,, +3.943439,0xD8,, +3.943526,0x2E,, +3.943613,0xE0,, +3.9437,0x2E,, +3.943787,0xE0,, +3.943874,0x22,, +3.943961,0x60,, +3.944048,0x22,, +3.944135,0x60,, +3.944222,0x3B,, +3.944309,0x60,, +3.944396,0x22,, +3.944483,0x60,, +3.94457,0x22,, +3.944657,0x60,, +3.944744,0x3B,, +3.94483,0x60,, +3.944917,0x2E,, +3.945004,0xE0,, +3.945091,0x2E,, +3.945178,0xE0,, +3.945265,0x17,, +3.945352,0x05,, +3.952965,0xA8,, +3.953052,0x01,, +3.953139,0x0C,, +3.953226,0x3A,, +3.953313,0xF8,, +3.9534,0x2E,, +3.953487,0xD8,, +3.953574,0x2E,, +3.953661,0xE0,, +3.953748,0x2E,, +3.953835,0xE0,, +3.953922,0x22,, +3.954008,0x60,, +3.954095,0x22,, +3.954182,0x60,, +3.954269,0x3B,, +3.954356,0x60,, +3.954443,0x22,, +3.95453,0x60,, +3.954617,0x22,, +3.954704,0x60,, +3.954791,0x3B,, +3.954878,0x60,, +3.954965,0x2E,, +3.955052,0xE0,, +3.955139,0x2E,, +3.955226,0xE0,, +3.955313,0xBB,, +3.9554,0x45,, +3.963012,0xA8,, +3.963099,0x01,, +3.963186,0x0C,, +3.963273,0x3B,, +3.96336,0x10,, +3.963447,0x2E,, +3.963534,0xD8,, +3.963621,0x2E,, +3.963708,0xE0,, +3.963795,0x2E,, +3.963882,0xE0,, +3.963969,0x22,, +3.964056,0x60,, +3.964143,0x22,, +3.96423,0x60,, +3.964317,0x3B,, +3.964403,0x60,, +3.96449,0x22,, +3.964577,0x60,, +3.964664,0x22,, +3.964751,0x60,, +3.964838,0x3B,, +3.964925,0x60,, +3.965012,0x2E,, +3.965099,0xE0,, +3.965186,0x2E,, +3.965273,0xE0,, +3.96536,0xA6,, +3.965447,0xD8,, +3.97306,0xA8,, +3.973147,0x01,, +3.973234,0x0C,, +3.973321,0x3B,, +3.973408,0x24,, +3.973495,0x2E,, +3.973581,0xD8,, +3.973668,0x2E,, +3.973755,0xE0,, +3.973842,0x2E,, +3.973929,0xE0,, +3.974016,0x22,, +3.974103,0x60,, +3.97419,0x22,, +3.974277,0x60,, +3.974364,0x3B,, +3.974451,0x60,, +3.974538,0x22,, +3.974625,0x60,, +3.974712,0x22,, +3.974799,0x60,, +3.974886,0x3B,, +3.974972,0x60,, +3.975059,0x2E,, +3.975146,0xE0,, +3.975233,0x2E,, +3.97532,0xE0,, +3.975407,0x72,, +3.975494,0x4F,, +3.983107,0xA8,, +3.983194,0x01,, +3.983281,0x0C,, +3.983368,0x3B,, +3.983455,0x38,, +3.983542,0x2E,, +3.983629,0xD8,, +3.983716,0x2E,, +3.983802,0xE0,, +3.983889,0x2E,, +3.983976,0xE0,, +3.984063,0x22,, +3.98415,0x60,, +3.984237,0x22,, +3.984324,0x60,, +3.984411,0x3B,, +3.984498,0x60,, +3.984585,0x22,, +3.984672,0x60,, +3.984759,0x22,, +3.984846,0x60,, +3.984933,0x3B,, +3.98502,0x60,, +3.985107,0x2E,, +3.985194,0xE0,, +3.985281,0x2E,, +3.985367,0xE0,, +3.985454,0xDE,, +3.985541,0x0F,, +3.993155,0xA8,, +3.993242,0x01,, +3.993329,0x0C,, +3.993416,0x3B,, +3.993503,0x48,, +3.99359,0x2E,, +3.993677,0xD8,, +3.993763,0x2E,, +3.99385,0xE4,, +3.993937,0x2E,, +3.994024,0xE0,, +3.994111,0x22,, +3.994198,0x60,, +3.994285,0x22,, +3.994372,0x60,, +3.994459,0x3B,, +3.994546,0x60,, +3.994633,0x22,, +3.99472,0x60,, +3.994807,0x22,, +3.994894,0x60,, +3.994981,0x3B,, +3.995068,0x60,, +3.995154,0x2E,, +3.995241,0xE0,, +3.995328,0x2E,, +3.995415,0xE0,, +3.995502,0x07,, +3.995589,0x52,, +4.003202,0xA8,, +4.003289,0x01,, +4.003376,0x0C,, +4.003463,0x3B,, +4.00355,0x54,, +4.003637,0x2E,, +4.003724,0xD8,, +4.003811,0x2E,, +4.003898,0xE0,, +4.003985,0x2E,, +4.004071,0xE0,, +4.004158,0x22,, +4.004245,0x60,, +4.004332,0x22,, +4.004419,0x60,, +4.004506,0x3B,, +4.004593,0x60,, +4.00468,0x22,, +4.004767,0x60,, +4.004854,0x22,, +4.004941,0x60,, +4.005028,0x3B,, +4.005115,0x60,, +4.005202,0x2E,, +4.005289,0xE0,, +4.005376,0x2E,, +4.005463,0xE0,, +4.005549,0xE3,, +4.005636,0x0D,, +4.01325,0xA8,, +4.013337,0x01,, +4.013424,0x0C,, +4.013511,0x3B,, +4.013597,0x60,, +4.013684,0x2E,, +4.013771,0xD8,, +4.013858,0x2E,, +4.013945,0xE0,, +4.014032,0x2E,, +4.014119,0xE0,, +4.014206,0x22,, +4.014293,0x60,, +4.01438,0x22,, +4.014467,0x60,, +4.014554,0x3B,, +4.014641,0x60,, +4.014728,0x22,, +4.014815,0x60,, +4.014902,0x22,, +4.014989,0x60,, +4.015075,0x3B,, +4.015162,0x60,, +4.015249,0x2E,, +4.015336,0xE0,, +4.015423,0x2E,, +4.01551,0xE0,, +4.015597,0x37,, +4.015684,0x9A,, +4.023297,0xA8,, +4.023384,0x01,, +4.023471,0x0C,, +4.023558,0x3B,, +4.023645,0x60,, +4.023732,0x2E,, +4.023819,0xD8,, +4.023906,0x2E,, +4.023993,0xEC,, +4.02408,0x2E,, +4.024167,0xE0,, +4.024253,0x22,, +4.02434,0x60,, +4.024427,0x22,, +4.024514,0x60,, +4.024601,0x3B,, +4.024688,0x60,, +4.024775,0x22,, +4.024862,0x60,, +4.024949,0x22,, +4.025036,0x60,, +4.025123,0x3B,, +4.02521,0x60,, +4.025297,0x2E,, +4.025384,0xE0,, +4.025471,0x2E,, +4.025558,0xE0,, +4.025645,0xEF,, +4.025731,0xBB,, +4.033345,0xA8,, +4.033432,0x01,, +4.033519,0x0C,, +4.033606,0x3B,, +4.033693,0x60,, +4.033779,0x2E,, +4.033866,0xD8,, +4.033953,0x2E,, +4.03404,0xE4,, +4.034127,0x2E,, +4.034214,0xE0,, +4.034301,0x22,, +4.034388,0x60,, +4.034475,0x22,, +4.034562,0x60,, +4.034649,0x3B,, +4.034736,0x60,, +4.034823,0x22,, +4.03491,0x60,, +4.034997,0x22,, +4.035084,0x60,, +4.035171,0x3B,, +4.035257,0x60,, +4.035344,0x2E,, +4.035431,0xE0,, +4.035518,0x2E,, +4.035605,0xE0,, +4.035692,0x7F,, +4.035779,0x85,, +4.043399,0xA8,, +4.043486,0x01,, +4.043573,0x0C,, +4.04366,0x3B,, +4.043747,0x60,, +4.043834,0x2E,, +4.043921,0xD8,, +4.044008,0x2E,, +4.044095,0xE4,, +4.044182,0x2E,, +4.044269,0xE0,, +4.044356,0x22,, +4.044443,0x60,, +4.04453,0x22,, +4.044617,0x60,, +4.044704,0x3B,, +4.044791,0x60,, +4.044877,0x22,, +4.044964,0x60,, +4.045051,0x22,, +4.045138,0x60,, +4.045225,0x3B,, +4.045312,0x60,, +4.045399,0x2E,, +4.045486,0xE0,, +4.045573,0x2E,, +4.04566,0xE0,, +4.045747,0x7F,, +4.045834,0x85,, +4.053447,0xA8,, +4.053534,0x01,, +4.053621,0x0C,, +4.053708,0x3B,, +4.053795,0x60,, +4.053882,0x2E,, +4.053969,0xD8,, +4.054056,0x2E,, +4.054142,0xE4,, +4.054229,0x2E,, +4.054316,0xE0,, +4.054403,0x22,, +4.05449,0x60,, +4.054577,0x22,, +4.054664,0x60,, +4.054751,0x3B,, +4.054838,0x60,, +4.054925,0x22,, +4.055012,0x60,, +4.055099,0x22,, +4.055186,0x60,, +4.055273,0x3B,, +4.05536,0x60,, +4.055447,0x2E,, +4.055534,0xE0,, +4.05562,0x2E,, +4.055707,0xE0,, +4.055794,0x7F,, +4.055881,0x85,, +4.063494,0xA8,, +4.063581,0x01,, +4.063668,0x0C,, +4.063755,0x3B,, +4.063842,0x60,, +4.063929,0x2E,, +4.064016,0xD8,, +4.064103,0x2E,, +4.06419,0xE4,, +4.064277,0x2E,, +4.064364,0xE0,, +4.064451,0x22,, +4.064538,0x60,, +4.064625,0x22,, +4.064712,0x60,, +4.064798,0x3B,, +4.064885,0x60,, +4.064972,0x22,, +4.065059,0x60,, +4.065146,0x22,, +4.065233,0x60,, +4.06532,0x3B,, +4.065407,0x60,, +4.065494,0x2E,, +4.065581,0xE0,, +4.065668,0x2E,, +4.065755,0xE0,, +4.065842,0x7F,, +4.065929,0x85,, +4.073542,0xA8,, +4.073629,0x01,, +4.073716,0x0C,, +4.073803,0x3B,, +4.07389,0x60,, +4.073977,0x2E,, +4.074064,0xD8,, +4.074151,0x2E,, +4.074238,0xE0,, +4.074325,0x2E,, +4.074412,0xE0,, +4.074499,0x22,, +4.074586,0x60,, +4.074673,0x22,, +4.074759,0x60,, +4.074846,0x3B,, +4.074933,0x60,, +4.07502,0x22,, +4.075107,0x60,, +4.075194,0x22,, +4.075281,0x60,, +4.075368,0x3B,, +4.075455,0x60,, +4.075542,0x2E,, +4.075629,0xE0,, +4.075716,0x2E,, +4.075803,0xE0,, +4.07589,0x37,, +4.075977,0x9A,, +4.083589,0xA8,, +4.083676,0x01,, +4.083763,0x0C,, +4.08385,0x3B,, +4.083937,0x60,, +4.084024,0x2E,, +4.084111,0xD8,, +4.084198,0x2E,, +4.084285,0xE4,, +4.084372,0x2E,, +4.084459,0xE0,, +4.084545,0x22,, +4.084632,0x60,, +4.084719,0x22,, +4.084806,0x60,, +4.084893,0x3B,, +4.08498,0x60,, +4.085067,0x22,, +4.085154,0x60,, +4.085241,0x22,, +4.085328,0x60,, +4.085415,0x3B,, +4.085502,0x60,, +4.085589,0x2E,, +4.085676,0xE0,, +4.085763,0x2E,, +4.08585,0xE0,, +4.085937,0x7F,, +4.086023,0x85,, +4.093656,0xA8,, +4.093743,0x01,, +4.09383,0x0C,, +4.093917,0x3B,, +4.094004,0x60,, +4.094091,0x2E,, +4.094178,0xD8,, +4.094265,0x2E,, +4.094352,0xE0,, +4.094439,0x2E,, +4.094526,0xE0,, +4.094612,0x22,, +4.094699,0x60,, +4.094786,0x22,, +4.094873,0x60,, +4.09496,0x3B,, +4.095047,0x60,, +4.095134,0x22,, +4.095221,0x60,, +4.095308,0x22,, +4.095395,0x60,, +4.095482,0x3B,, +4.095569,0x60,, +4.095656,0x2E,, +4.095743,0xE0,, +4.09583,0x2E,, +4.095917,0xE0,, +4.096004,0x37,, +4.09609,0x9A,, +4.103704,0xA8,, +4.103791,0x01,, +4.103878,0x0C,, +4.103965,0x3B,, +4.104052,0x60,, +4.104139,0x2E,, +4.104226,0xD8,, +4.104313,0x2E,, +4.1044,0xE4,, +4.104487,0x2E,, +4.104574,0xE0,, +4.104661,0x22,, +4.104748,0x60,, +4.104835,0x22,, +4.104922,0x60,, +4.105008,0x3B,, +4.105095,0x60,, +4.105182,0x22,, +4.105269,0x60,, +4.105356,0x22,, +4.105443,0x60,, +4.10553,0x3B,, +4.105617,0x60,, +4.105704,0x2E,, +4.105791,0xE0,, +4.105878,0x2E,, +4.105965,0xE0,, +4.106052,0x7F,, +4.106139,0x85,, +4.113752,0xA8,, +4.113839,0x01,, +4.113926,0x0C,, +4.114012,0x3B,, +4.114099,0x60,, +4.114186,0x2E,, +4.114273,0xD8,, +4.11436,0x2E,, +4.114447,0xE4,, +4.114534,0x2E,, +4.114621,0xE0,, +4.114708,0x22,, +4.114795,0x60,, +4.114882,0x22,, +4.114969,0x60,, +4.115056,0x3B,, +4.115143,0x60,, +4.11523,0x22,, +4.115317,0x60,, +4.115404,0x22,, +4.115491,0x60,, +4.115577,0x3B,, +4.115664,0x60,, +4.115751,0x2E,, +4.115838,0xE0,, +4.115925,0x2E,, +4.116012,0xE0,, +4.116099,0x7F,, +4.116186,0x85,, +4.1238,0xA8,, +4.123887,0x01,, +4.123974,0x0C,, +4.12406,0x3B,, +4.124147,0x60,, +4.124234,0x2E,, +4.124321,0xD8,, +4.124408,0x2E,, +4.124495,0xE0,, +4.124582,0x2E,, +4.124669,0xE0,, +4.124756,0x22,, +4.124843,0x60,, +4.12493,0x22,, +4.125017,0x60,, +4.125104,0x3B,, +4.125191,0x60,, +4.125278,0x22,, +4.125365,0x60,, +4.125452,0x22,, +4.125538,0x60,, +4.125625,0x3B,, +4.125712,0x60,, +4.125799,0x2E,, +4.125886,0xE0,, +4.125973,0x2E,, +4.12606,0xE0,, +4.126147,0x37,, +4.126234,0x9A,, +4.133848,0xA8,, +4.133935,0x01,, +4.134022,0x0C,, +4.134109,0x3B,, +4.134196,0x60,, +4.134283,0x2E,, +4.134369,0xD8,, +4.134456,0x2E,, +4.134543,0xE4,, +4.13463,0x2E,, +4.134717,0xE0,, +4.134804,0x22,, +4.134891,0x60,, +4.134978,0x22,, +4.135065,0x60,, +4.135152,0x3B,, +4.135239,0x60,, +4.135326,0x22,, +4.135413,0x60,, +4.1355,0x22,, +4.135587,0x60,, +4.135674,0x3B,, +4.135761,0x60,, +4.135847,0x2E,, +4.135934,0xE0,, +4.136021,0x2E,, +4.136108,0xE0,, +4.136195,0x7F,, +4.136282,0x85,, +4.143896,0xA8,, +4.143983,0x01,, +4.14407,0x0C,, +4.144157,0x3B,, +4.144244,0x60,, +4.144331,0x2E,, +4.144418,0xD8,, +4.144504,0x2E,, +4.144591,0xE0,, +4.144678,0x2E,, +4.144765,0xE0,, +4.144852,0x22,, +4.144939,0x60,, +4.145026,0x22,, +4.145113,0x60,, +4.1452,0x3B,, +4.145287,0x60,, +4.145374,0x22,, +4.145461,0x60,, +4.145548,0x22,, +4.145635,0x60,, +4.145722,0x3B,, +4.145809,0x60,, +4.145896,0x2E,, +4.145982,0xE0,, +4.146069,0x2E,, +4.146156,0xE0,, +4.146243,0x37,, +4.14633,0x9A,, +4.153944,0xA8,, +4.154031,0x01,, +4.154118,0x0C,, +4.154205,0x3B,, +4.154291,0x60,, +4.154378,0x2E,, +4.154465,0xD8,, +4.154552,0x2E,, +4.154639,0xE0,, +4.154726,0x2E,, +4.154813,0xE0,, +4.1549,0x22,, +4.154987,0x60,, +4.155074,0x22,, +4.155161,0x60,, +4.155248,0x3B,, +4.155335,0x60,, +4.155422,0x22,, +4.155509,0x60,, +4.155596,0x22,, +4.155683,0x60,, +4.155769,0x3B,, +4.155856,0x60,, +4.155943,0x2E,, +4.15603,0xE0,, +4.156117,0x2E,, +4.156204,0xE0,, +4.156291,0x37,, +4.156378,0x9A,, +4.163992,0xA8,, +4.164079,0x01,, +4.164166,0x0C,, +4.164253,0x3B,, +4.16434,0x60,, +4.164427,0x2E,, +4.164514,0xD8,, +4.164601,0x2E,, +4.164687,0xE4,, +4.164774,0x2E,, +4.164861,0xE0,, +4.164948,0x22,, +4.165035,0x60,, +4.165122,0x22,, +4.165209,0x60,, +4.165296,0x3B,, +4.165383,0x60,, +4.16547,0x22,, +4.165557,0x60,, +4.165644,0x22,, +4.165731,0x60,, +4.165818,0x3B,, +4.165905,0x60,, +4.165992,0x2E,, +4.166079,0xE0,, +4.166165,0x2E,, +4.166252,0xE0,, +4.166339,0x7F,, +4.166426,0x85,, +4.174039,0xA8,, +4.174126,0x01,, +4.174213,0x0C,, +4.1743,0x3B,, +4.174387,0x60,, +4.174474,0x2E,, +4.174561,0xD8,, +4.174648,0x2E,, +4.174735,0xE0,, +4.174822,0x2E,, +4.174909,0xE0,, +4.174996,0x22,, +4.175083,0x60,, +4.17517,0x22,, +4.175257,0x60,, +4.175344,0x3B,, +4.175431,0x60,, +4.175517,0x22,, +4.175604,0x60,, +4.175691,0x22,, +4.175778,0x60,, +4.175865,0x3B,, +4.175952,0x60,, +4.176039,0x2E,, +4.176126,0xE0,, +4.176213,0x2E,, +4.1763,0xE0,, +4.176387,0x37,, +4.176474,0x9A,, +4.184089,0xA8,, +4.184176,0x01,, +4.184263,0x0C,, +4.18435,0x3B,, +4.184437,0x60,, +4.184524,0x2E,, +4.184611,0xD8,, +4.184698,0x2E,, +4.184784,0xE0,, +4.184871,0x2E,, +4.184958,0xE0,, +4.185045,0x22,, +4.185132,0x60,, +4.185219,0x22,, +4.185306,0x60,, +4.185393,0x3B,, +4.18548,0x60,, +4.185567,0x22,, +4.185654,0x60,, +4.185741,0x22,, +4.185828,0x60,, +4.185915,0x3B,, +4.186002,0x60,, +4.186089,0x2E,, +4.186176,0xE0,, +4.186262,0x2E,, +4.186349,0xE0,, +4.186436,0x37,, +4.186523,0x9A,, +4.194135,0xA8,, +4.194222,0x01,, +4.194309,0x0C,, +4.194396,0x3B,, +4.194483,0x60,, +4.19457,0x2E,, +4.194657,0xD8,, +4.194744,0x2E,, +4.194831,0xE0,, +4.194918,0x2E,, +4.195005,0xE0,, +4.195092,0x22,, +4.195179,0x60,, +4.195266,0x22,, +4.195353,0x60,, +4.195439,0x3B,, +4.195526,0x60,, +4.195613,0x22,, +4.1957,0x60,, +4.195787,0x22,, +4.195874,0x60,, +4.195961,0x3B,, +4.196048,0x60,, +4.196135,0x2E,, +4.196222,0xE0,, +4.196309,0x2E,, +4.196396,0xE0,, +4.196483,0x37,, +4.19657,0x9A,, +4.204183,0xA8,, +4.20427,0x01,, +4.204357,0x0C,, +4.204444,0x3B,, +4.204531,0x60,, +4.204618,0x2E,, +4.204705,0xD8,, +4.204792,0x2E,, +4.204878,0xE0,, +4.204965,0x2E,, +4.205052,0xE0,, +4.205139,0x22,, +4.205226,0x60,, +4.205313,0x22,, +4.2054,0x60,, +4.205487,0x3B,, +4.205574,0x60,, +4.205661,0x22,, +4.205748,0x60,, +4.205835,0x22,, +4.205922,0x60,, +4.206009,0x3B,, +4.206096,0x60,, +4.206183,0x2E,, +4.20627,0xE0,, +4.206356,0x2E,, +4.206443,0xE0,, +4.20653,0x37,, +4.206617,0x9A,, +4.214734,0xA8,, +4.214821,0x01,, +4.214908,0x0C,, +4.214995,0x3B,, +4.215082,0x60,, +4.215169,0x2E,, +4.215256,0xD8,, +4.215343,0x2E,, +4.21543,0xE0,, +4.215517,0x2E,, +4.215604,0xE0,, +4.215691,0x22,, +4.215777,0x60,, +4.215864,0x22,, +4.215951,0x60,, +4.216038,0x3B,, +4.216125,0x60,, +4.216212,0x22,, +4.216299,0x60,, +4.216386,0x22,, +4.216473,0x60,, +4.21656,0x3B,, +4.216647,0x60,, +4.216734,0x2E,, +4.216821,0xE0,, +4.216908,0x2E,, +4.216995,0xE0,, +4.217082,0x37,, +4.217169,0x9A,, +4.224783,0xA8,, +4.224869,0x01,, +4.224956,0x0C,, +4.225043,0x3B,, +4.22513,0x60,, +4.225217,0x2E,, +4.225304,0xD8,, +4.225391,0x2E,, +4.225478,0xE0,, +4.225565,0x2E,, +4.225652,0xDC,, +4.225739,0x22,, +4.225826,0x60,, +4.225913,0x22,, +4.226,0x60,, +4.226087,0x3B,, +4.226174,0x60,, +4.226261,0x22,, +4.226347,0x60,, +4.226434,0x22,, +4.226521,0x60,, +4.226608,0x3B,, +4.226695,0x60,, +4.226782,0x2E,, +4.226869,0xE0,, +4.226956,0x2E,, +4.227043,0xE0,, +4.22713,0xD1,, +4.227217,0x01,, +4.234905,0xA8,, +4.234992,0x01,, +4.235079,0x0C,, +4.235166,0x3B,, +4.235253,0x60,, +4.23534,0x2E,, +4.235427,0xD8,, +4.235514,0x2E,, +4.235601,0xE4,, +4.235688,0x2E,, +4.235775,0xDC,, +4.235862,0x22,, +4.235949,0x60,, +4.236036,0x22,, +4.236123,0x60,, +4.236209,0x3B,, +4.236296,0x60,, +4.236383,0x22,, +4.23647,0x60,, +4.236557,0x22,, +4.236644,0x60,, +4.236731,0x3B,, +4.236818,0x60,, +4.236905,0x2E,, +4.236992,0xE0,, +4.237079,0x2E,, +4.237166,0xE0,, +4.237253,0x99,, +4.23734,0x1E,, +4.244953,0xA8,, +4.24504,0x01,, +4.245127,0x0C,, +4.245214,0x3B,, +4.2453,0x60,, +4.245387,0x2E,, +4.245474,0xD8,, +4.245561,0x2E,, +4.245648,0xE0,, +4.245735,0x2E,, +4.245822,0xDC,, +4.245909,0x22,, +4.245996,0x60,, +4.246083,0x22,, +4.24617,0x60,, +4.246257,0x3B,, +4.246344,0x60,, +4.246431,0x22,, +4.246518,0x60,, +4.246605,0x22,, +4.246692,0x60,, +4.246779,0x3B,, +4.246865,0x60,, +4.246952,0x2E,, +4.247039,0xE0,, +4.247126,0x2E,, +4.247213,0xE0,, +4.2473,0xD1,, +4.247387,0x01,, +4.255,0xA8,, +4.255087,0x01,, +4.255174,0x0C,, +4.255261,0x3B,, +4.255348,0x60,, +4.255435,0x2E,, +4.255522,0xD8,, +4.255609,0x2E,, +4.255696,0xE0,, +4.255783,0x2E,, +4.25587,0xDC,, +4.255957,0x22,, +4.256044,0x60,, +4.256131,0x22,, +4.256218,0x60,, +4.256304,0x3B,, +4.256391,0x60,, +4.256478,0x22,, +4.256565,0x60,, +4.256652,0x22,, +4.256739,0x60,, +4.256826,0x3B,, +4.256913,0x60,, +4.257,0x2E,, +4.257087,0xE0,, +4.257174,0x2E,, +4.257261,0xE0,, +4.257348,0xD1,, +4.257435,0x01,, +4.265102,0xA8,, +4.265189,0x01,, +4.265276,0x0C,, +4.265363,0x3B,, +4.26545,0x60,, +4.265537,0x2E,, +4.265624,0xD8,, +4.265711,0x2E,, +4.265798,0xE0,, +4.265885,0x2E,, +4.265972,0xDC,, +4.266059,0x22,, +4.266146,0x60,, +4.266233,0x22,, +4.26632,0x60,, +4.266407,0x3B,, +4.266493,0x60,, +4.26658,0x22,, +4.266667,0x60,, +4.266754,0x22,, +4.266841,0x60,, +4.266928,0x3B,, +4.267015,0x60,, +4.267102,0x2E,, +4.267189,0xE0,, +4.267276,0x2E,, +4.267363,0xE0,, +4.26745,0xD1,, +4.267537,0x01,, +4.27515,0xA8,, +4.275237,0x01,, +4.275324,0x0C,, +4.27541,0x3B,, +4.275497,0x58,, +4.275584,0x2E,, +4.275671,0xD8,, +4.275758,0x2E,, +4.275845,0xE0,, +4.275932,0x2E,, +4.276019,0xD8,, +4.276106,0x22,, +4.276193,0x60,, +4.27628,0x22,, +4.276367,0x60,, +4.276454,0x3B,, +4.276541,0x60,, +4.276628,0x22,, +4.276715,0x60,, +4.276802,0x22,, +4.276888,0x60,, +4.276975,0x3B,, +4.277062,0x60,, +4.277149,0x2E,, +4.277236,0xE0,, +4.277323,0x2E,, +4.27741,0xE0,, +4.277497,0xDA,, +4.277584,0x28,, +4.285197,0xA8,, +4.285284,0x01,, +4.285371,0x0C,, +4.285458,0x3B,, +4.285545,0x4C,, +4.285632,0x2E,, +4.285719,0xD8,, +4.285806,0x2E,, +4.285893,0xE4,, +4.28598,0x2E,, +4.286067,0xDC,, +4.286154,0x22,, +4.286241,0x60,, +4.286328,0x22,, +4.286415,0x60,, +4.286501,0x3B,, +4.286588,0x60,, +4.286675,0x22,, +4.286762,0x60,, +4.286849,0x22,, +4.286936,0x60,, +4.287023,0x3B,, +4.28711,0x60,, +4.287197,0x2E,, +4.287284,0xE0,, +4.287371,0x2E,, +4.287458,0xE0,, +4.287545,0x65,, +4.287632,0xC4,, +4.295244,0xA8,, +4.295331,0x01,, +4.295418,0x0C,, +4.295505,0x3B,, +4.295592,0x2C,, +4.295679,0x2E,, +4.295766,0xD8,, +4.295853,0x2E,, +4.29594,0xE0,, +4.296027,0x2E,, +4.296114,0xD8,, +4.296201,0x22,, +4.296288,0x60,, +4.296375,0x22,, +4.296462,0x60,, +4.296548,0x3B,, +4.296635,0x60,, +4.296722,0x22,, +4.296809,0x60,, +4.296896,0x22,, +4.296983,0x60,, +4.29707,0x3B,, +4.297157,0x60,, +4.297244,0x2E,, +4.297331,0xE0,, +4.297418,0x2E,, +4.297505,0xE0,, +4.297592,0xCF,, +4.297679,0x67,, +4.305351,0xA8,, +4.305438,0x01,, +4.305525,0x0C,, +4.305612,0x3B,, +4.305699,0x08,, +4.305786,0x2E,, +4.305873,0xD4,, +4.30596,0x2E,, +4.306047,0xE0,, +4.306134,0x2E,, +4.306221,0xD8,, +4.306308,0x22,, +4.306395,0x60,, +4.306482,0x22,, +4.306569,0x60,, +4.306656,0x3B,, +4.306742,0x60,, +4.306829,0x22,, +4.306916,0x60,, +4.307003,0x22,, +4.30709,0x60,, +4.307177,0x3B,, +4.307264,0x60,, +4.307351,0x2E,, +4.307438,0xE0,, +4.307525,0x2E,, +4.307612,0xE0,, +4.307699,0x83,, +4.307786,0x4B,, +4.315399,0xA8,, +4.315486,0x01,, +4.315573,0x0C,, +4.31566,0x3A,, +4.315747,0xD4,, +4.315833,0x2E,, +4.31592,0xD8,, +4.316007,0x2E,, +4.316094,0xE4,, +4.316181,0x2E,, +4.316268,0xD8,, +4.316355,0x22,, +4.316442,0x60,, +4.316529,0x22,, +4.316616,0x60,, +4.316703,0x3B,, +4.31679,0x60,, +4.316877,0x22,, +4.316964,0x60,, +4.317051,0x22,, +4.317138,0x60,, +4.317225,0x3B,, +4.317311,0x60,, +4.317398,0x2E,, +4.317485,0xE0,, +4.317572,0x2E,, +4.317659,0xE0,, +4.317746,0xAA,, +4.317833,0x93,, +4.325447,0xA8,, +4.325534,0x01,, +4.325621,0x0C,, +4.325708,0x3A,, +4.325795,0x98,, +4.325882,0x2E,, +4.325968,0xD8,, +4.326055,0x2E,, +4.326142,0xE0,, +4.326229,0x2E,, +4.326316,0xDC,, +4.326403,0x22,, +4.32649,0x60,, +4.326577,0x22,, +4.326664,0x60,, +4.326751,0x3B,, +4.326838,0x60,, +4.326925,0x22,, +4.327012,0x60,, +4.327099,0x22,, +4.327186,0x60,, +4.327273,0x3B,, +4.32736,0x60,, +4.327446,0x2E,, +4.327533,0xE0,, +4.32762,0x2E,, +4.327707,0xE0,, +4.327794,0xFC,, +4.327881,0xEA,, +4.335494,0xA8,, +4.335581,0x01,, +4.335668,0x0C,, +4.335755,0x3A,, +4.335842,0x60,, +4.335929,0x2E,, +4.336016,0xD8,, +4.336103,0x2E,, +4.33619,0xE0,, +4.336277,0x2E,, +4.336364,0xD8,, +4.336451,0x22,, +4.336538,0x60,, +4.336624,0x22,, +4.336711,0x60,, +4.336798,0x3B,, +4.336885,0x60,, +4.336972,0x22,, +4.337059,0x60,, +4.337146,0x22,, +4.337233,0x60,, +4.33732,0x3B,, +4.337407,0x60,, +4.337494,0x2E,, +4.337581,0xE0,, +4.337668,0x2E,, +4.337755,0xE0,, +4.337842,0xA5,, +4.337929,0x8A,, +4.345542,0xA8,, +4.345628,0x01,, +4.345715,0x0C,, +4.345802,0x3A,, +4.345889,0x14,, +4.345976,0x2E,, +4.346063,0xD8,, +4.34615,0x2E,, +4.346237,0xE4,, +4.346324,0x2E,, +4.346411,0xD8,, +4.346498,0x22,, +4.346585,0x60,, +4.346672,0x22,, +4.346759,0x60,, +4.346846,0x3B,, +4.346933,0x60,, +4.34702,0x22,, +4.347106,0x60,, +4.347193,0x22,, +4.34728,0x60,, +4.347367,0x3B,, +4.347454,0x60,, +4.347541,0x2E,, +4.347628,0xE0,, +4.347715,0x2E,, +4.347802,0xE0,, +4.347889,0xF8,, +4.347976,0xDA,, +4.355589,0xA8,, +4.355676,0x01,, +4.355763,0x0C,, +4.35585,0x39,, +4.355937,0xC4,, +4.356024,0x2E,, +4.356111,0xD8,, +4.356198,0x2E,, +4.356285,0xE0,, +4.356372,0x2E,, +4.356459,0xDC,, +4.356546,0x22,, +4.356633,0x60,, +4.35672,0x22,, +4.356806,0x60,, +4.356893,0x3B,, +4.35698,0x60,, +4.357067,0x22,, +4.357154,0x60,, +4.357241,0x22,, +4.357328,0x60,, +4.357415,0x3B,, +4.357502,0x60,, +4.357589,0x2E,, +4.357676,0xE0,, +4.357763,0x2E,, +4.35785,0xE0,, +4.357937,0xC8,, +4.358024,0x77,, +4.365637,0xA8,, +4.365723,0x01,, +4.36581,0x0C,, +4.365897,0x39,, +4.365984,0x70,, +4.366071,0x2E,, +4.366158,0xD4,, +4.366245,0x2E,, +4.366332,0xE0,, +4.366419,0x2E,, +4.366506,0xD8,, +4.366593,0x22,, +4.36668,0x60,, +4.366767,0x22,, +4.366854,0x60,, +4.366941,0x3B,, +4.367028,0x60,, +4.367115,0x22,, +4.367201,0x60,, +4.367288,0x22,, +4.367375,0x60,, +4.367462,0x3B,, +4.367549,0x60,, +4.367636,0x2E,, +4.367723,0xE0,, +4.36781,0x2E,, +4.367897,0xE0,, +4.367984,0x64,, +4.368071,0x34,, +4.375684,0xA8,, +4.375771,0x01,, +4.375858,0x0C,, +4.375945,0x39,, +4.376032,0x0C,, +4.376119,0x2E,, +4.376206,0xD8,, +4.376293,0x2E,, +4.37638,0xE0,, +4.376467,0x2E,, +4.376554,0xD8,, +4.376641,0x22,, +4.376728,0x60,, +4.376814,0x22,, +4.376901,0x60,, +4.376988,0x3B,, +4.377075,0x60,, +4.377162,0x22,, +4.377249,0x60,, +4.377336,0x22,, +4.377423,0x60,, +4.37751,0x3B,, +4.377597,0x60,, +4.377684,0x2E,, +4.377771,0xE0,, +4.377858,0x2E,, +4.377945,0xE0,, +4.378032,0xC1,, +4.378119,0x8D,, +4.385732,0xA8,, +4.385819,0x01,, +4.385906,0x0C,, +4.385993,0x38,, +4.38608,0xA0,, +4.386167,0x2E,, +4.386254,0xD8,, +4.386341,0x2E,, +4.386427,0xE0,, +4.386514,0x2E,, +4.386601,0xD8,, +4.386688,0x22,, +4.386775,0x60,, +4.386862,0x22,, +4.386949,0x60,, +4.387036,0x3B,, +4.387123,0x60,, +4.38721,0x22,, +4.387297,0x60,, +4.387384,0x22,, +4.387471,0x60,, +4.387558,0x3B,, +4.387645,0x60,, +4.387732,0x2E,, +4.387818,0xE0,, +4.387905,0x2E,, +4.387992,0xE0,, +4.388079,0x99,, +4.388166,0xC5,, +4.395779,0xA8,, +4.395866,0x01,, +4.395953,0x0C,, +4.39604,0x38,, +4.396127,0x34,, +4.396214,0x2E,, +4.396301,0xD8,, +4.396388,0x2E,, +4.396475,0xE4,, +4.396562,0x2E,, +4.396649,0xD8,, +4.396736,0x22,, +4.396823,0x60,, +4.39691,0x22,, +4.396997,0x60,, +4.397084,0x3B,, +4.39717,0x60,, +4.397257,0x22,, +4.397344,0x60,, +4.397431,0x22,, +4.397518,0x60,, +4.397605,0x3B,, +4.397692,0x60,, +4.397779,0x2E,, +4.397866,0xE0,, +4.397953,0x2E,, +4.39804,0xE0,, +4.398127,0xF6,, +4.398214,0x30,, +4.405827,0xA8,, +4.405914,0x01,, +4.406001,0x0C,, +4.406088,0x37,, +4.406175,0xB8,, +4.406262,0x2E,, +4.406348,0xD8,, +4.406435,0x2E,, +4.406522,0xE4,, +4.406609,0x2E,, +4.406696,0xD8,, +4.406783,0x22,, +4.40687,0x60,, +4.406957,0x22,, +4.407044,0x60,, +4.407131,0x3B,, +4.407218,0x60,, +4.407305,0x22,, +4.407392,0x60,, +4.407479,0x22,, +4.407566,0x60,, +4.407653,0x3B,, +4.407739,0x60,, +4.407826,0x2E,, +4.407913,0xE0,, +4.408,0x2E,, +4.408087,0xE0,, +4.408174,0xD4,, +4.408261,0xA7,, +4.415874,0xA8,, +4.415961,0x01,, +4.416048,0x0C,, +4.416135,0x37,, +4.416222,0x34,, +4.416309,0x2E,, +4.416396,0xD8,, +4.416483,0x2E,, +4.41657,0xE4,, +4.416657,0x2E,, +4.416744,0xDC,, +4.41683,0x22,, +4.416917,0x60,, +4.417004,0x22,, +4.417091,0x60,, +4.417178,0x3B,, +4.417265,0x60,, +4.417352,0x22,, +4.417439,0x60,, +4.417526,0x22,, +4.417613,0x60,, +4.4177,0x3B,, +4.417787,0x60,, +4.417874,0x2E,, +4.417961,0xE0,, +4.418048,0x2E,, +4.418135,0xE0,, +4.418222,0x98,, +4.418308,0x88,, +4.425928,0xA8,, +4.426014,0x01,, +4.426101,0x0C,, +4.426188,0x36,, +4.426275,0xC0,, +4.426362,0x2E,, +4.426449,0xD8,, +4.426536,0x2E,, +4.426623,0xE0,, +4.42671,0x2E,, +4.426797,0xD8,, +4.426884,0x22,, +4.426971,0x60,, +4.427058,0x22,, +4.427145,0x60,, +4.427232,0x3B,, +4.427319,0x60,, +4.427406,0x22,, +4.427492,0x60,, +4.427579,0x22,, +4.427666,0x60,, +4.427753,0x3B,, +4.42784,0x60,, +4.427927,0x2E,, +4.428014,0xE0,, +4.428101,0x2E,, +4.428188,0xE0,, +4.428275,0x22,, +4.428362,0xC2,, +4.435975,0xA8,, +4.436062,0x01,, +4.436149,0x0C,, +4.436236,0x36,, +4.436323,0x50,, +4.43641,0x2E,, +4.436497,0xD8,, +4.436584,0x2E,, +4.43667,0xE0,, +4.436757,0x2E,, +4.436844,0xD8,, +4.436931,0x22,, +4.437018,0x60,, +4.437105,0x22,, +4.437192,0x60,, +4.437279,0x3B,, +4.437366,0x60,, +4.437453,0x22,, +4.43754,0x60,, +4.437627,0x22,, +4.437714,0x60,, +4.437801,0x3B,, +4.437888,0x60,, +4.437975,0x2E,, +4.438061,0xE0,, +4.438148,0x2E,, +4.438235,0xE0,, +4.438322,0x81,, +4.438409,0x25,, +4.446022,0xA8,, +4.446109,0x01,, +4.446196,0x0C,, +4.446283,0x35,, +4.44637,0xCC,, +4.446457,0x2E,, +4.446544,0xD8,, +4.446631,0x2E,, +4.446718,0xE4,, +4.446805,0x2E,, +4.446892,0xD8,, +4.446979,0x22,, +4.447065,0x60,, +4.447152,0x22,, +4.447239,0x60,, +4.447326,0x3B,, +4.447413,0x60,, +4.4475,0x22,, +4.447587,0x60,, +4.447674,0x22,, +4.447761,0x60,, +4.447848,0x3B,, +4.447935,0x60,, +4.448022,0x2E,, +4.448109,0xE0,, +4.448196,0x2E,, +4.448283,0xE0,, +4.44837,0xAF,, +4.448457,0xEE,, +4.45607,0xA8,, +4.456156,0x01,, +4.456243,0x0C,, +4.45633,0x35,, +4.456417,0x4C,, +4.456504,0x2E,, +4.456591,0xD8,, +4.456678,0x2E,, +4.456765,0xE4,, +4.456852,0x2E,, +4.456939,0xDC,, +4.457026,0x22,, +4.457113,0x60,, +4.4572,0x22,, +4.457287,0x60,, +4.457374,0x3B,, +4.457461,0x60,, +4.457548,0x22,, +4.457634,0x60,, +4.457721,0x22,, +4.457808,0x60,, +4.457895,0x3B,, +4.457982,0x60,, +4.458069,0x2E,, +4.458156,0xE0,, +4.458243,0x2E,, +4.45833,0xE0,, +4.458417,0x7F,, +4.458504,0xF7,, +4.466117,0xA8,, +4.466204,0x01,, +4.466291,0x0C,, +4.466378,0x34,, +4.466465,0xE0,, +4.466552,0x2E,, +4.466639,0xD8,, +4.466726,0x2E,, +4.466813,0xE0,, +4.4669,0x2E,, +4.466987,0xDC,, +4.467074,0x22,, +4.46716,0x60,, +4.467247,0x22,, +4.467334,0x60,, +4.467421,0x3B,, +4.467508,0x60,, +4.467595,0x22,, +4.467682,0x60,, +4.467769,0x22,, +4.467856,0x60,, +4.467943,0x3B,, +4.46803,0x60,, +4.468117,0x2E,, +4.468204,0xE0,, +4.468291,0x2E,, +4.468378,0xE0,, +4.468465,0x6F,, +4.468552,0xA0,, +4.476165,0xA8,, +4.476252,0x01,, +4.476339,0x0C,, +4.476425,0x34,, +4.476512,0x54,, +4.476599,0x2E,, +4.476686,0xD8,, +4.476773,0x2E,, +4.47686,0xE0,, +4.476947,0x2E,, +4.477034,0xDC,, +4.477121,0x22,, +4.477208,0x60,, +4.477295,0x22,, +4.477382,0x60,, +4.477469,0x3B,, +4.477556,0x60,, +4.477643,0x22,, +4.47773,0x60,, +4.477817,0x22,, +4.477903,0x60,, +4.47799,0x3B,, +4.478077,0x60,, +4.478164,0x2E,, +4.478251,0xE0,, +4.478338,0x2E,, +4.478425,0xE0,, +4.478512,0x28,, +4.478599,0xA6,, +4.486212,0xA8,, +4.486299,0x01,, +4.486386,0x0C,, +4.486473,0x33,, +4.48656,0xE0,, +4.486647,0x2E,, +4.486734,0xD8,, +4.486821,0x2E,, +4.486908,0xE0,, +4.486995,0x2E,, +4.487082,0xD8,, +4.487169,0x22,, +4.487255,0x60,, +4.487342,0x22,, +4.487429,0x60,, +4.487516,0x3B,, +4.487603,0x60,, +4.48769,0x22,, +4.487777,0x60,, +4.487864,0x22,, +4.487951,0x60,, +4.488038,0x3B,, +4.488125,0x60,, +4.488212,0x2E,, +4.488299,0xE0,, +4.488386,0x2E,, +4.488473,0xE0,, +4.48856,0xA9,, +4.488647,0x21,, +4.496259,0xA8,, +4.496346,0x01,, +4.496433,0x0C,, +4.49652,0x33,, +4.496607,0x68,, +4.496694,0x2E,, +4.496781,0xD8,, +4.496868,0x2E,, +4.496955,0xE0,, +4.497042,0x2E,, +4.497129,0xD8,, +4.497216,0x22,, +4.497303,0x60,, +4.49739,0x22,, +4.497476,0x60,, +4.497563,0x3B,, +4.49765,0x60,, +4.497737,0x22,, +4.497824,0x60,, +4.497911,0x22,, +4.497998,0x60,, +4.498085,0x3B,, +4.498172,0x60,, +4.498259,0x2E,, +4.498346,0xE0,, +4.498433,0x2E,, +4.49852,0xE0,, +4.498607,0x22,, +4.498694,0x8B,, +4.506307,0xA8,, +4.506394,0x01,, +4.506481,0x0C,, +4.506568,0x33,, +4.506655,0x00,, +4.506742,0x2E,, +4.506829,0xD8,, +4.506916,0x2E,, +4.507003,0xE0,, +4.50709,0x2E,, +4.507177,0xD8,, +4.507264,0x22,, +4.507351,0x60,, +4.507437,0x22,, +4.507524,0x60,, +4.507611,0x3B,, +4.507698,0x60,, +4.507785,0x22,, +4.507872,0x60,, +4.507959,0x22,, +4.508046,0x60,, +4.508133,0x3B,, +4.50822,0x60,, +4.508307,0x2E,, +4.508394,0xE0,, +4.508481,0x2E,, +4.508568,0xE0,, +4.508655,0x9B,, +4.508742,0x84,, +4.516354,0xA8,, +4.516441,0x01,, +4.516528,0x0C,, +4.516615,0x32,, +4.516702,0x80,, +4.516789,0x2E,, +4.516876,0xD8,, +4.516963,0x2E,, +4.517049,0xE4,, +4.517136,0x2E,, +4.517223,0xD8,, +4.51731,0x22,, +4.517397,0x60,, +4.517484,0x22,, +4.517571,0x60,, +4.517658,0x3B,, +4.517745,0x60,, +4.517832,0x22,, +4.517919,0x60,, +4.518006,0x22,, +4.518093,0x60,, +4.51818,0x3B,, +4.518267,0x60,, +4.518354,0x2E,, +4.518441,0xE0,, +4.518527,0x2E,, +4.518614,0xE0,, +4.518701,0x77,, +4.518788,0x09,, +4.526402,0xA8,, +4.526489,0x01,, +4.526576,0x0C,, +4.526663,0x31,, +4.52675,0xDC,, +4.526837,0x2E,, +4.526924,0xD8,, +4.527011,0x2E,, +4.527098,0xE0,, +4.527185,0x2E,, +4.527272,0xD8,, +4.527359,0x22,, +4.527446,0x60,, +4.527533,0x22,, +4.52762,0x60,, +4.527706,0x3B,, +4.527793,0x60,, +4.52788,0x22,, +4.527967,0x60,, +4.528054,0x22,, +4.528141,0x60,, +4.528228,0x3B,, +4.528315,0x60,, +4.528402,0x2E,, +4.528489,0xE0,, +4.528576,0x2E,, +4.528663,0xE0,, +4.52875,0x0B,, +4.528837,0x8B,, +4.536449,0xA8,, +4.536536,0x01,, +4.536623,0x0C,, +4.53671,0x31,, +4.536797,0x38,, +4.536884,0x2E,, +4.536971,0xD8,, +4.537058,0x2E,, +4.537145,0xE4,, +4.537232,0x2E,, +4.537319,0xD8,, +4.537406,0x22,, +4.537493,0x60,, +4.53758,0x22,, +4.537667,0x60,, +4.537753,0x3B,, +4.53784,0x60,, +4.537927,0x22,, +4.538014,0x60,, +4.538101,0x22,, +4.538188,0x60,, +4.538275,0x3B,, +4.538362,0x60,, +4.538449,0x2E,, +4.538536,0xE0,, +4.538623,0x2E,, +4.53871,0xE0,, +4.538797,0xF5,, +4.538884,0x3C,, +4.546497,0xA8,, +4.546584,0x01,, +4.546671,0x0C,, +4.546758,0x30,, +4.546845,0x8C,, +4.546931,0x2E,, +4.547018,0xD8,, +4.547105,0x2E,, +4.547192,0xE4,, +4.547279,0x2E,, +4.547366,0xD8,, +4.547453,0x22,, +4.54754,0x60,, +4.547627,0x22,, +4.547714,0x60,, +4.547801,0x3B,, +4.547888,0x60,, +4.547975,0x22,, +4.548062,0x60,, +4.548149,0x22,, +4.548236,0x60,, +4.548323,0x3B,, +4.548409,0x60,, +4.548496,0x2E,, +4.548583,0xE0,, +4.54867,0x2E,, +4.548757,0xE0,, +4.548844,0x85,, +4.548931,0x39,, +4.556546,0xA8,, +4.556633,0x01,, +4.55672,0x0C,, +4.556806,0x2F,, +4.556893,0xF0,, +4.55698,0x2E,, +4.557067,0xD8,, +4.557154,0x2E,, +4.557241,0xE0,, +4.557328,0x2E,, +4.557415,0xD8,, +4.557502,0x22,, +4.557589,0x60,, +4.557676,0x22,, +4.557763,0x60,, +4.55785,0x3B,, +4.557937,0x60,, +4.558024,0x22,, +4.558111,0x60,, +4.558198,0x22,, +4.558284,0x60,, +4.558371,0x3B,, +4.558458,0x60,, +4.558545,0x2E,, +4.558632,0xE0,, +4.558719,0x2E,, +4.558806,0xE0,, +4.558893,0xAD,, +4.55898,0x31,, +4.566593,0xA8,, +4.56668,0x01,, +4.566767,0x0C,, +4.566854,0x2F,, +4.566941,0x54,, +4.567028,0x2E,, +4.567114,0xD8,, +4.567201,0x2E,, +4.567288,0xE0,, +4.567375,0x2E,, +4.567462,0xD8,, +4.567549,0x22,, +4.567636,0x60,, +4.567723,0x22,, +4.56781,0x60,, +4.567897,0x3B,, +4.567984,0x60,, +4.568071,0x22,, +4.568158,0x60,, +4.568245,0x22,, +4.568332,0x60,, +4.568419,0x3B,, +4.568506,0x60,, +4.568592,0x2E,, +4.568679,0xE0,, +4.568766,0x2E,, +4.568853,0xE0,, +4.56894,0xDA,, +4.569027,0x41,, +4.576652,0xA8,, +4.576739,0x01,, +4.576826,0x0C,, +4.576913,0x2E,, +4.577,0xC4,, +4.577087,0x2E,, +4.577174,0xD8,, +4.577261,0x2E,, +4.577348,0xE4,, +4.577435,0x2E,, +4.577522,0xD8,, +4.577609,0x22,, +4.577696,0x60,, +4.577782,0x22,, +4.577869,0x60,, +4.577956,0x3B,, +4.578043,0x60,, +4.57813,0x22,, +4.578217,0x60,, +4.578304,0x22,, +4.578391,0x60,, +4.578478,0x3B,, +4.578565,0x60,, +4.578652,0x2E,, +4.578739,0xE0,, +4.578826,0x2E,, +4.578913,0xE0,, +4.579,0x06,, +4.579087,0xBA,, +4.5867,0xA8,, +4.586787,0x01,, +4.586873,0x0C,, +4.58696,0x2E,, +4.587047,0x2C,, +4.587134,0x2E,, +4.587221,0xD8,, +4.587308,0x2E,, +4.587395,0xE4,, +4.587482,0x2E,, +4.587569,0xD8,, +4.587656,0x22,, +4.587743,0x60,, +4.58783,0x22,, +4.587917,0x60,, +4.588004,0x3B,, +4.588091,0x60,, +4.588178,0x22,, +4.588265,0x60,, +4.588351,0x22,, +4.588438,0x60,, +4.588525,0x3B,, +4.588612,0x60,, +4.588699,0x2E,, +4.588786,0xE0,, +4.588873,0x2E,, +4.58896,0xE0,, +4.589047,0x2C,, +4.589134,0x24,, +4.596747,0xA8,, +4.596834,0x01,, +4.596921,0x0C,, +4.597008,0x2D,, +4.597095,0x88,, +4.597182,0x2E,, +4.597269,0xD8,, +4.597356,0x2E,, +4.597443,0xE4,, +4.59753,0x2E,, +4.597617,0xD8,, +4.597704,0x22,, +4.597791,0x60,, +4.597878,0x22,, +4.597965,0x60,, +4.598052,0x3B,, +4.598138,0x60,, +4.598225,0x22,, +4.598312,0x60,, +4.598399,0x22,, +4.598486,0x60,, +4.598573,0x3B,, +4.59866,0x60,, +4.598747,0x2E,, +4.598834,0xE0,, +4.598921,0x2E,, +4.599008,0xE0,, +4.599095,0x02,, +4.599182,0x51,, +4.606814,0xA8,, +4.606901,0x01,, +4.606988,0x0C,, +4.607075,0x2D,, +4.607162,0x04,, +4.607249,0x2E,, +4.607336,0xD8,, +4.607423,0x2E,, +4.60751,0xE4,, +4.607597,0x2E,, +4.607684,0xD8,, +4.607771,0x22,, +4.607858,0x60,, +4.607945,0x22,, +4.608032,0x60,, +4.608119,0x3B,, +4.608205,0x60,, +4.608292,0x22,, +4.608379,0x60,, +4.608466,0x22,, +4.608553,0x60,, +4.60864,0x3B,, +4.608727,0x60,, +4.608814,0x2E,, +4.608901,0xE0,, +4.608988,0x2E,, +4.609075,0xE0,, +4.609162,0x0D,, +4.609249,0xF6,, +4.616862,0xA8,, +4.616949,0x01,, +4.617036,0x0C,, +4.617123,0x2C,, +4.61721,0x6C,, +4.617297,0x2E,, +4.617384,0xD8,, +4.617471,0x2E,, +4.617558,0xE4,, +4.617645,0x2E,, +4.617732,0xD8,, +4.617818,0x22,, +4.617905,0x60,, +4.617992,0x22,, +4.618079,0x60,, +4.618166,0x3B,, +4.618253,0x60,, +4.61834,0x22,, +4.618427,0x60,, +4.618514,0x22,, +4.618601,0x60,, +4.618688,0x3B,, +4.618775,0x60,, +4.618862,0x2E,, +4.618949,0xE0,, +4.619036,0x2E,, +4.619123,0xE0,, +4.61921,0x83,, +4.619296,0xFA,, +4.62691,0xA8,, +4.626996,0x01,, +4.627083,0x0C,, +4.62717,0x2B,, +4.627257,0xE4,, +4.627344,0x2E,, +4.627431,0xD8,, +4.627518,0x2E,, +4.627605,0xE4,, +4.627692,0x2E,, +4.627779,0xD8,, +4.627866,0x22,, +4.627953,0x60,, +4.62804,0x22,, +4.628127,0x60,, +4.628214,0x3B,, +4.628301,0x60,, +4.628388,0x22,, +4.628474,0x60,, +4.628561,0x22,, +4.628648,0x60,, +4.628735,0x3B,, +4.628822,0x60,, +4.628909,0x2E,, +4.628996,0xE0,, +4.629083,0x2E,, +4.62917,0xE0,, +4.629257,0x8D,, +4.629344,0x59,, +4.636957,0xA8,, +4.637044,0x01,, +4.637131,0x0C,, +4.637217,0x2B,, +4.637304,0x70,, +4.637391,0x2E,, +4.637478,0xD8,, +4.637565,0x2E,, +4.637652,0xE8,, +4.637739,0x2E,, +4.637826,0xD8,, +4.637913,0x22,, +4.638,0x60,, +4.638087,0x22,, +4.638174,0x60,, +4.638261,0x3B,, +4.638348,0x60,, +4.638435,0x22,, +4.638522,0x60,, +4.638609,0x22,, +4.638695,0x60,, +4.638782,0x3B,, +4.638869,0x60,, +4.638956,0x2E,, +4.639043,0xE0,, +4.63913,0x2E,, +4.639217,0xE0,, +4.639304,0x72,, +4.639391,0x92,, +4.647005,0xA8,, +4.647092,0x01,, +4.647179,0x0C,, +4.647266,0x2A,, +4.647353,0xDC,, +4.64744,0x2E,, +4.647527,0xD8,, +4.647614,0x2E,, +4.647701,0xE0,, +4.647788,0x2E,, +4.647875,0xD8,, +4.647961,0x22,, +4.648048,0x60,, +4.648135,0x22,, +4.648222,0x60,, +4.648309,0x3B,, +4.648396,0x60,, +4.648483,0x22,, +4.64857,0x60,, +4.648657,0x22,, +4.648744,0x60,, +4.648831,0x3B,, +4.648918,0x60,, +4.649005,0x2E,, +4.649092,0xE0,, +4.649179,0x2E,, +4.649266,0xE0,, +4.649353,0xBA,, +4.649439,0xE4,, +4.657052,0xA8,, +4.657139,0x01,, +4.657226,0x0C,, +4.657313,0x2A,, +4.6574,0x58,, +4.657487,0x2E,, +4.657574,0xD8,, +4.657661,0x2E,, +4.657748,0xE4,, +4.657834,0x2E,, +4.657921,0xD8,, +4.658008,0x22,, +4.658095,0x60,, +4.658182,0x22,, +4.658269,0x60,, +4.658356,0x3B,, +4.658443,0x60,, +4.65853,0x22,, +4.658617,0x60,, +4.658704,0x22,, +4.658791,0x60,, +4.658878,0x3B,, +4.658965,0x60,, +4.659052,0x2E,, +4.659139,0xE0,, +4.659226,0x2E,, +4.659312,0xE0,, +4.659399,0xE5,, +4.659486,0x67,, +4.6671,0xA8,, +4.667187,0x01,, +4.667274,0x0C,, +4.667361,0x29,, +4.667447,0xCC,, +4.667534,0x2E,, +4.667621,0xD8,, +4.667708,0x2E,, +4.667795,0xE0,, +4.667882,0x2E,, +4.667969,0xD8,, +4.668056,0x22,, +4.668143,0x60,, +4.66823,0x22,, +4.668317,0x60,, +4.668404,0x3B,, +4.668491,0x60,, +4.668578,0x22,, +4.668665,0x60,, +4.668752,0x22,, +4.668839,0x60,, +4.668926,0x3B,, +4.669012,0x60,, +4.669099,0x2E,, +4.669186,0xE0,, +4.669273,0x2E,, +4.66936,0xE0,, +4.669447,0xD3,, +4.669534,0x97,, +4.677147,0xA8,, +4.677234,0x01,, +4.677321,0x0C,, +4.677408,0x29,, +4.677495,0x40,, +4.677582,0x2E,, +4.677669,0xD8,, +4.677756,0x2E,, +4.677843,0xE0,, +4.67793,0x2E,, +4.678017,0xD8,, +4.678104,0x22,, +4.67819,0x60,, +4.678277,0x22,, +4.678364,0x60,, +4.678451,0x3B,, +4.678538,0x60,, +4.678625,0x22,, +4.678712,0x60,, +4.678799,0x22,, +4.678886,0x60,, +4.678973,0x3B,, +4.67906,0x60,, +4.679147,0x2E,, +4.679234,0xE0,, +4.679321,0x2E,, +4.679408,0xE0,, +4.679495,0xDC,, +4.679582,0x30,, +4.687194,0xA8,, +4.687281,0x01,, +4.687368,0x0C,, +4.687455,0x28,, +4.687542,0xB0,, +4.687629,0x2E,, +4.687716,0xD8,, +4.687803,0x2E,, +4.68789,0xE0,, +4.687977,0x2E,, +4.688064,0xD8,, +4.688151,0x22,, +4.688238,0x60,, +4.688325,0x22,, +4.688412,0x60,, +4.688499,0x3B,, +4.688586,0x60,, +4.688673,0x22,, +4.688759,0x60,, +4.688846,0x22,, +4.688933,0x60,, +4.68902,0x3B,, +4.689107,0x60,, +4.689194,0x2E,, +4.689281,0xE0,, +4.689368,0x2E,, +4.689455,0xE0,, +4.689542,0xE9,, +4.689629,0xE0,, +4.697241,0xA8,, +4.697328,0x01,, +4.697415,0x0C,, +4.697502,0x28,, +4.697589,0x34,, +4.697676,0x2E,, +4.697763,0xD8,, +4.69785,0x2E,, +4.697937,0xE4,, +4.698024,0x2E,, +4.698111,0xD8,, +4.698198,0x22,, +4.698284,0x60,, +4.698371,0x22,, +4.698458,0x60,, +4.698545,0x3B,, +4.698632,0x60,, +4.698719,0x22,, +4.698806,0x60,, +4.698893,0x22,, +4.69898,0x60,, +4.699067,0x3B,, +4.699154,0x60,, +4.699241,0x2E,, +4.699328,0xE0,, +4.699415,0x2E,, +4.699502,0xE0,, +4.699589,0xB6,, +4.699676,0x63,, +4.707289,0xA8,, +4.707376,0x01,, +4.707463,0x0C,, +4.70755,0x27,, +4.707637,0xB0,, +4.707724,0x2E,, +4.707811,0xD8,, +4.707898,0x2E,, +4.707985,0xE0,, +4.708072,0x2E,, +4.708159,0xD8,, +4.708246,0x22,, +4.708332,0x60,, +4.708419,0x22,, +4.708506,0x60,, +4.708593,0x3B,, +4.70868,0x60,, +4.708767,0x22,, +4.708854,0x60,, +4.708941,0x22,, +4.709028,0x60,, +4.709115,0x3B,, +4.709202,0x60,, +4.709289,0x2E,, +4.709376,0xE0,, +4.709463,0x2E,, +4.70955,0xE0,, +4.709637,0xC4,, +4.709724,0xD0,, +4.717343,0xA8,, +4.71743,0x01,, +4.717517,0x0C,, +4.717604,0x27,, +4.717691,0x34,, +4.717778,0x2E,, +4.717865,0xD8,, +4.717952,0x2E,, +4.718039,0xE4,, +4.718126,0x2E,, +4.718213,0xD8,, +4.7183,0x22,, +4.718387,0x60,, +4.718474,0x22,, +4.718561,0x60,, +4.718648,0x3B,, +4.718734,0x60,, +4.718821,0x22,, +4.718908,0x60,, +4.718995,0x22,, +4.719082,0x60,, +4.719169,0x3B,, +4.719256,0x60,, +4.719343,0x2E,, +4.71943,0xE0,, +4.719517,0x2E,, +4.719604,0xE0,, +4.719691,0x9B,, +4.719778,0x53,, +4.727893,0xA8,, +4.72798,0x01,, +4.728067,0x0C,, +4.728154,0x26,, +4.728241,0xC8,, +4.728328,0x2E,, +4.728415,0xD8,, +4.728502,0x2E,, +4.728588,0xE4,, +4.728675,0x2E,, +4.728762,0xD8,, +4.728849,0x22,, +4.728936,0x60,, +4.729023,0x22,, +4.72911,0x60,, +4.729197,0x3B,, +4.729284,0x60,, +4.729371,0x22,, +4.729458,0x60,, +4.729545,0x22,, +4.729632,0x60,, +4.729719,0x3B,, +4.729806,0x60,, +4.729893,0x2E,, +4.729979,0xE0,, +4.730066,0x2E,, +4.730153,0xE0,, +4.73024,0x32,, +4.730327,0xB5,, +4.738034,0xA8,, +4.738121,0x01,, +4.738208,0x0C,, +4.738295,0x26,, +4.738382,0x5C,, +4.738469,0x2E,, +4.738556,0xD8,, +4.738643,0x2E,, +4.73873,0xE4,, +4.738817,0x2E,, +4.738904,0xD8,, +4.738991,0x22,, +4.739078,0x60,, +4.739165,0x22,, +4.739252,0x60,, +4.739338,0x3B,, +4.739425,0x60,, +4.739512,0x22,, +4.739599,0x60,, +4.739686,0x22,, +4.739773,0x60,, +4.73986,0x3B,, +4.739947,0x60,, +4.740034,0x2E,, +4.740121,0xE0,, +4.740208,0x2E,, +4.740295,0xE0,, +4.740382,0x15,, +4.740469,0x5F,, +4.748081,0xA8,, +4.748168,0x01,, +4.748255,0x0C,, +4.748342,0x26,, +4.748429,0x0C,, +4.748516,0x2E,, +4.748603,0xD8,, +4.74869,0x2E,, +4.748777,0xE4,, +4.748863,0x2E,, +4.74895,0xD8,, +4.749037,0x22,, +4.749124,0x60,, +4.749211,0x22,, +4.749298,0x60,, +4.749385,0x3B,, +4.749472,0x60,, +4.749559,0x22,, +4.749646,0x60,, +4.749733,0x22,, +4.74982,0x60,, +4.749907,0x3B,, +4.749994,0x60,, +4.750081,0x2E,, +4.750168,0xE0,, +4.750255,0x2E,, +4.750341,0xE0,, +4.750428,0xE4,, +4.750515,0xF1,, +4.758128,0xA8,, +4.758215,0x01,, +4.758302,0x0C,, +4.758389,0x25,, +4.758476,0xA4,, +4.758563,0x2E,, +4.75865,0xD8,, +4.758737,0x2E,, +4.758824,0xE0,, +4.758911,0x2E,, +4.758998,0xD8,, +4.759085,0x22,, +4.759171,0x60,, +4.759258,0x22,, +4.759345,0x60,, +4.759432,0x3B,, +4.759519,0x60,, +4.759606,0x22,, +4.759693,0x60,, +4.75978,0x22,, +4.759867,0x60,, +4.759954,0x3B,, +4.760041,0x60,, +4.760128,0x2E,, +4.760215,0xE0,, +4.760302,0x2E,, +4.760389,0xE0,, +4.760476,0x1E,, +4.760563,0xAD,, +4.768279,0xA8,, +4.768366,0x01,, +4.768453,0x0C,, +4.76854,0x25,, +4.768627,0x40,, +4.768714,0x2E,, +4.768801,0xD8,, +4.768888,0x2E,, +4.768975,0xE0,, +4.769062,0x2E,, +4.769149,0xD8,, +4.769236,0x22,, +4.769323,0x60,, +4.76941,0x22,, +4.769496,0x60,, +4.769583,0x3B,, +4.76967,0x60,, +4.769757,0x22,, +4.769844,0x60,, +4.769931,0x22,, +4.770018,0x60,, +4.770105,0x3B,, +4.770192,0x60,, +4.770279,0x2E,, +4.770366,0xE0,, +4.770453,0x2E,, +4.77054,0xE0,, +4.770627,0xA8,, +4.770714,0x05,, +4.778326,0xA8,, +4.778413,0x01,, +4.7785,0x0C,, +4.778587,0x24,, +4.778674,0xD0,, +4.778761,0x2E,, +4.778848,0xD8,, +4.778935,0x2E,, +4.779022,0xE0,, +4.779109,0x2E,, +4.779196,0xCC,, +4.779283,0x22,, +4.77937,0x60,, +4.779457,0x22,, +4.779544,0x60,, +4.77963,0x3B,, +4.779717,0x60,, +4.779804,0x22,, +4.779891,0x60,, +4.779978,0x22,, +4.780065,0x60,, +4.780152,0x3B,, +4.780239,0x60,, +4.780326,0x2E,, +4.780413,0xE0,, +4.7805,0x2E,, +4.780587,0xE0,, +4.780674,0x61,, +4.780761,0x68,, +4.788373,0xA8,, +4.78846,0x01,, +4.788547,0x0C,, +4.788634,0x24,, +4.788721,0x60,, +4.788808,0x2E,, +4.788895,0xD8,, +4.788982,0x2E,, +4.789069,0xE0,, +4.789156,0x2E,, +4.789243,0xD8,, +4.78933,0x22,, +4.789417,0x60,, +4.789504,0x22,, +4.789591,0x60,, +4.789678,0x3B,, +4.789765,0x60,, +4.789851,0x22,, +4.789938,0x60,, +4.790025,0x22,, +4.790112,0x60,, +4.790199,0x3B,, +4.790286,0x60,, +4.790373,0x2E,, +4.79046,0xE0,, +4.790547,0x2E,, +4.790634,0xE0,, +4.790721,0xFF,, +4.790808,0xEA,, +4.79842,0xA8,, +4.798507,0x01,, +4.798594,0x0C,, +4.798681,0x24,, +4.798768,0x00,, +4.798855,0x2E,, +4.798942,0xD8,, +4.799029,0x2E,, +4.799116,0xEC,, +4.799202,0x2E,, +4.799289,0xD8,, +4.799376,0x22,, +4.799463,0x60,, +4.79955,0x22,, +4.799637,0x60,, +4.799724,0x3B,, +4.799811,0x60,, +4.799898,0x22,, +4.799985,0x60,, +4.800072,0x22,, +4.800159,0x60,, +4.800246,0x3B,, +4.800333,0x60,, +4.80042,0x2E,, +4.800507,0xE0,, +4.800594,0x2E,, +4.80068,0xE0,, +4.800767,0x86,, +4.800854,0xFF,, +4.808478,0xA8,, +4.808565,0x01,, +4.808652,0x0C,, +4.808739,0x23,, +4.808826,0xA4,, +4.808913,0x2E,, +4.809,0xD0,, +4.809087,0x2E,, +4.809174,0xE0,, +4.80926,0x2E,, +4.809347,0xD8,, +4.809434,0x22,, +4.809521,0x60,, +4.809608,0x22,, +4.809695,0x60,, +4.809782,0x3B,, +4.809869,0x60,, +4.809956,0x22,, +4.810043,0x60,, +4.81013,0x22,, +4.810217,0x60,, +4.810304,0x3B,, +4.810391,0x60,, +4.810478,0x2E,, +4.810565,0xE0,, +4.810652,0x2E,, +4.810738,0xE0,, +4.810825,0x63,, +4.810912,0xD1,, +4.818525,0xA8,, +4.818612,0x01,, +4.818699,0x0C,, +4.818786,0x23,, +4.818873,0x54,, +4.81896,0x2E,, +4.819047,0xD8,, +4.819134,0x2E,, +4.81922,0xE0,, +4.819307,0x2E,, +4.819394,0xD8,, +4.819481,0x22,, +4.819568,0x60,, +4.819655,0x22,, +4.819742,0x60,, +4.819829,0x3B,, +4.819916,0x60,, +4.820003,0x22,, +4.82009,0x60,, +4.820177,0x22,, +4.820264,0x60,, +4.820351,0x3B,, +4.820438,0x60,, +4.820525,0x2E,, +4.820612,0xE0,, +4.820698,0x2E,, +4.820785,0xE0,, +4.820872,0xAE,, +4.820959,0x74,, +4.828572,0xA8,, +4.828659,0x01,, +4.828746,0x0C,, +4.828833,0x23,, +4.82892,0x08,, +4.829007,0x2E,, +4.829094,0xD8,, +4.829181,0x2E,, +4.829268,0xE4,, +4.829355,0x2E,, +4.829442,0xD8,, +4.829528,0x22,, +4.829615,0x60,, +4.829702,0x22,, +4.829789,0x60,, +4.829876,0x3B,, +4.829963,0x60,, +4.83005,0x22,, +4.830137,0x60,, +4.830224,0x22,, +4.830311,0x60,, +4.830398,0x3B,, +4.830485,0x60,, +4.830572,0x2E,, +4.830659,0xE0,, +4.830746,0x2E,, +4.830833,0xE0,, +4.83092,0x8B,, +4.831006,0xF3,, +4.838619,0xA8,, +4.838706,0x01,, +4.838793,0x0C,, +4.838879,0x22,, +4.838966,0xC4,, +4.839053,0x2E,, +4.83914,0xD8,, +4.839227,0x2E,, +4.839314,0xE4,, +4.839401,0x2E,, +4.839488,0xD8,, +4.839575,0x22,, +4.839662,0x60,, +4.839749,0x22,, +4.839836,0x60,, +4.839923,0x3B,, +4.84001,0x60,, +4.840097,0x22,, +4.840184,0x60,, +4.840271,0x22,, +4.840357,0x60,, +4.840444,0x3B,, +4.840531,0x60,, +4.840618,0x2E,, +4.840705,0xE0,, +4.840792,0x2E,, +4.840879,0xE0,, +4.840966,0x72,, +4.841053,0x8F,, +4.848666,0xA8,, +4.848753,0x01,, +4.84884,0x0C,, +4.848927,0x22,, +4.849014,0x84,, +4.849101,0x2E,, +4.849187,0xD8,, +4.849274,0x2E,, +4.849361,0xE0,, +4.849448,0x2E,, +4.849535,0xD8,, +4.849622,0x22,, +4.849709,0x60,, +4.849796,0x22,, +4.849883,0x60,, +4.84997,0x3B,, +4.850057,0x60,, +4.850144,0x22,, +4.850231,0x60,, +4.850318,0x22,, +4.850405,0x60,, +4.850492,0x3B,, +4.850579,0x60,, +4.850665,0x2E,, +4.850752,0xE0,, +4.850839,0x2E,, +4.850926,0xE0,, +4.851013,0xFB,, +4.8511,0x48,, +4.858713,0xA8,, +4.8588,0x01,, +4.858887,0x0C,, +4.858974,0x22,, +4.859061,0x78,, +4.859147,0x2E,, +4.859234,0xD8,, +4.859321,0x2E,, +4.859408,0xE0,, +4.859495,0x2E,, +4.859582,0xD8,, +4.859669,0x22,, +4.859756,0x60,, +4.859843,0x22,, +4.85993,0x60,, +4.860017,0x3B,, +4.860104,0x60,, +4.860191,0x22,, +4.860278,0x60,, +4.860365,0x22,, +4.860452,0x60,, +4.860539,0x3B,, +4.860625,0x60,, +4.860712,0x2E,, +4.860799,0xE0,, +4.860886,0x2E,, +4.860973,0xE0,, +4.86106,0x65,, +4.861147,0xAD,, +4.86876,0xA8,, +4.868847,0x01,, +4.868934,0x0C,, +4.869021,0x22,, +4.869108,0x78,, +4.869195,0x2E,, +4.869282,0xD8,, +4.869369,0x2E,, +4.869456,0xE4,, +4.869543,0x2E,, +4.86963,0xD8,, +4.869716,0x22,, +4.869803,0x60,, +4.86989,0x22,, +4.869977,0x60,, +4.870064,0x3B,, +4.870151,0x60,, +4.870238,0x22,, +4.870325,0x60,, +4.870412,0x22,, +4.870499,0x60,, +4.870586,0x3B,, +4.870673,0x60,, +4.87076,0x2E,, +4.870847,0xE0,, +4.870934,0x2E,, +4.871021,0xE0,, +4.871108,0x2D,, +4.871194,0xB2,, +4.878807,0xA8,, +4.878894,0x01,, +4.878981,0x0C,, +4.879068,0x22,, +4.879155,0x74,, +4.879242,0x2E,, +4.879329,0xD8,, +4.879416,0x2E,, +4.879503,0xE4,, +4.87959,0x2E,, +4.879677,0xD8,, +4.879764,0x22,, +4.879851,0x60,, +4.879937,0x22,, +4.880024,0x60,, +4.880111,0x3B,, +4.880198,0x60,, +4.880285,0x22,, +4.880372,0x60,, +4.880459,0x22,, +4.880546,0x60,, +4.880633,0x3B,, +4.88072,0x60,, +4.880807,0x2E,, +4.880894,0xE0,, +4.880981,0x2E,, +4.881068,0xE0,, +4.881155,0xB1,, +4.881242,0x84,, +4.888855,0xA8,, +4.888941,0x01,, +4.889028,0x0C,, +4.889115,0x22,, +4.889202,0x78,, +4.889289,0x2E,, +4.889376,0xD8,, +4.889463,0x2E,, +4.88955,0xE0,, +4.889637,0x2E,, +4.889724,0xD8,, +4.889811,0x22,, +4.889898,0x60,, +4.889985,0x22,, +4.890072,0x60,, +4.890159,0x3B,, +4.890246,0x60,, +4.890333,0x22,, +4.890419,0x60,, +4.890506,0x22,, +4.890593,0x60,, +4.89068,0x3B,, +4.890767,0x60,, +4.890854,0x2E,, +4.890941,0xE0,, +4.891028,0x2E,, +4.891115,0xE0,, +4.891202,0x65,, +4.891289,0xAD,, +4.898902,0xA8,, +4.898989,0x01,, +4.899076,0x0C,, +4.899163,0x22,, +4.89925,0x78,, +4.899337,0x2E,, +4.899424,0xD8,, +4.899511,0x2E,, +4.899598,0xE4,, +4.899685,0x2E,, +4.899772,0xD8,, +4.899859,0x22,, +4.899946,0x60,, +4.900033,0x22,, +4.90012,0x60,, +4.900206,0x3B,, +4.900293,0x60,, +4.90038,0x22,, +4.900467,0x60,, +4.900554,0x22,, +4.900641,0x60,, +4.900728,0x3B,, +4.900815,0x60,, +4.900902,0x2E,, +4.900989,0xE0,, +4.901076,0x2E,, +4.901163,0xE0,, +4.90125,0x2D,, +4.901337,0xB2,, +4.908949,0xA8,, +4.909036,0x01,, +4.909123,0x0C,, +4.90921,0x22,, +4.909297,0x78,, +4.909384,0x2E,, +4.909471,0xDC,, +4.909558,0x2E,, +4.909645,0xE0,, +4.909732,0x2E,, +4.909819,0xD8,, +4.909906,0x22,, +4.909993,0x60,, +4.910079,0x22,, +4.910166,0x60,, +4.910253,0x3B,, +4.91034,0x60,, +4.910427,0x22,, +4.910514,0x60,, +4.910601,0x22,, +4.910688,0x60,, +4.910775,0x3B,, +4.910862,0x60,, +4.910949,0x2E,, +4.911036,0xE0,, +4.911123,0x2E,, +4.91121,0xE0,, +4.911297,0x02,, +4.911384,0x16,, +4.918996,0xA8,, +4.919083,0x01,, +4.91917,0x0C,, +4.919257,0x22,, +4.919344,0x78,, +4.919431,0x2E,, +4.919518,0xD8,, +4.919605,0x2E,, +4.919692,0xE4,, +4.919779,0x2E,, +4.919865,0xD8,, +4.919952,0x22,, +4.920039,0x60,, +4.920126,0x22,, +4.920213,0x60,, +4.9203,0x3B,, +4.920387,0x60,, +4.920474,0x22,, +4.920561,0x60,, +4.920648,0x22,, +4.920735,0x60,, +4.920822,0x3B,, +4.920909,0x60,, +4.920996,0x2E,, +4.921083,0xE0,, +4.92117,0x2E,, +4.921257,0xE0,, +4.921344,0x2D,, +4.92143,0xB2,, +4.929043,0xA8,, +4.92913,0x01,, +4.929217,0x0C,, +4.929304,0x22,, +4.929391,0x78,, +4.929478,0x2E,, +4.929565,0xD8,, +4.929652,0x2E,, +4.929739,0xE0,, +4.929826,0x2E,, +4.929913,0xD8,, +4.93,0x22,, +4.930087,0x60,, +4.930174,0x22,, +4.930261,0x60,, +4.930347,0x3B,, +4.930434,0x60,, +4.930521,0x22,, +4.930608,0x60,, +4.930695,0x22,, +4.930782,0x60,, +4.930869,0x3B,, +4.930956,0x60,, +4.931043,0x2E,, +4.93113,0xE0,, +4.931217,0x2E,, +4.931304,0xE0,, +4.931391,0x65,, +4.931478,0xAD,, +4.93909,0xA8,, +4.939177,0x01,, +4.939264,0x0C,, +4.939351,0x22,, +4.939438,0x78,, +4.939525,0x2E,, +4.939612,0xD8,, +4.939699,0x2E,, +4.939786,0xE0,, +4.939873,0x2E,, +4.93996,0xD8,, +4.940046,0x22,, +4.940133,0x60,, +4.94022,0x22,, +4.940307,0x60,, +4.940394,0x3B,, +4.940481,0x60,, +4.940568,0x22,, +4.940655,0x60,, +4.940742,0x22,, +4.940829,0x60,, +4.940916,0x3B,, +4.941003,0x60,, +4.94109,0x2E,, +4.941177,0xE0,, +4.941264,0x2E,, +4.941351,0xE0,, +4.941438,0x65,, +4.941525,0xAD,, +4.949138,0xA8,, +4.949225,0x01,, +4.949312,0x0C,, +4.949399,0x22,, +4.949485,0x74,, +4.949572,0x2E,, +4.949659,0xD8,, +4.949746,0x2E,, +4.949833,0xE4,, +4.94992,0x2E,, +4.950007,0xD8,, +4.950094,0x22,, +4.950181,0x60,, +4.950268,0x22,, +4.950355,0x60,, +4.950442,0x3B,, +4.950529,0x60,, +4.950616,0x22,, +4.950703,0x60,, +4.95079,0x22,, +4.950877,0x60,, +4.950963,0x3B,, +4.95105,0x60,, +4.951137,0x2E,, +4.951224,0xE0,, +4.951311,0x2E,, +4.951398,0xE0,, +4.951485,0xB1,, +4.951572,0x84,, +4.959185,0xA8,, +4.959272,0x01,, +4.959359,0x0C,, +4.959446,0x22,, +4.959533,0x74,, +4.95962,0x2E,, +4.959707,0xD8,, +4.959794,0x2E,, +4.959881,0xE0,, +4.959967,0x2E,, +4.960054,0xD8,, +4.960141,0x22,, +4.960228,0x60,, +4.960315,0x22,, +4.960402,0x60,, +4.960489,0x3B,, +4.960576,0x60,, +4.960663,0x22,, +4.96075,0x60,, +4.960837,0x22,, +4.960924,0x60,, +4.961011,0x3B,, +4.961098,0x60,, +4.961185,0x2E,, +4.961272,0xE0,, +4.961359,0x2E,, +4.961445,0xE0,, +4.961532,0xF9,, +4.961619,0x9B,, +4.969231,0xA8,, +4.969318,0x01,, +4.969405,0x0C,, +4.969492,0x22,, +4.969579,0x74,, +4.969666,0x2E,, +4.969753,0xD8,, +4.96984,0x2E,, +4.969927,0xE0,, +4.970014,0x2E,, +4.970101,0xD8,, +4.970188,0x22,, +4.970275,0x60,, +4.970362,0x22,, +4.970449,0x60,, +4.970535,0x3B,, +4.970622,0x60,, +4.970709,0x22,, +4.970796,0x60,, +4.970883,0x22,, +4.97097,0x60,, +4.971057,0x3B,, +4.971144,0x60,, +4.971231,0x2E,, +4.971318,0xE0,, +4.971405,0x2E,, +4.971492,0xE0,, +4.971579,0xF9,, +4.971666,0x9B,, +4.979278,0xA8,, +4.979365,0x01,, +4.979452,0x0C,, +4.979539,0x22,, +4.979626,0x74,, +4.979713,0x2E,, +4.9798,0xD8,, +4.979887,0x2E,, +4.979974,0xE4,, +4.980061,0x2E,, +4.980148,0xD8,, +4.980235,0x22,, +4.980322,0x60,, +4.980409,0x22,, +4.980495,0x60,, +4.980582,0x3B,, +4.980669,0x60,, +4.980756,0x22,, +4.980843,0x60,, +4.98093,0x22,, +4.981017,0x60,, +4.981104,0x3B,, +4.981191,0x60,, +4.981278,0x2E,, +4.981365,0xE0,, +4.981452,0x2E,, +4.981539,0xE0,, +4.981626,0xB1,, +4.981713,0x84,, +4.989326,0xA8,, +4.989413,0x01,, +4.9895,0x0C,, +4.989587,0x22,, +4.989674,0x74,, +4.989761,0x2E,, +4.989848,0xD8,, +4.989935,0x2E,, +4.990021,0xE4,, +4.990108,0x2E,, +4.990195,0xD8,, +4.990282,0x22,, +4.990369,0x60,, +4.990456,0x22,, +4.990543,0x60,, +4.99063,0x3B,, +4.990717,0x60,, +4.990804,0x22,, +4.990891,0x60,, +4.990978,0x22,, +4.991065,0x60,, +4.991152,0x3B,, +4.991239,0x60,, +4.991326,0x2E,, +4.991413,0xE0,, +4.991499,0x2E,, +4.991586,0xE0,, +4.991673,0xB1,, +4.99176,0x84,, +4.999373,0xA8,, +4.99946,0x01,, +4.999547,0x0C,, +4.999634,0x22,, +4.999721,0x74,, +4.999807,0x2E,, +4.999894,0xD8,, +4.999981,0x2E,, +5.000068,0xE4,, +5.000155,0x2E,, +5.000242,0xD8,, +5.000329,0x22,, +5.000416,0x60,, +5.000503,0x22,, +5.00059,0x60,, +5.000677,0x3B,, +5.000764,0x60,, +5.000851,0x22,, +5.000938,0x60,, +5.001025,0x22,, +5.001112,0x60,, +5.001199,0x3B,, +5.001285,0x60,, +5.001372,0x2E,, +5.001459,0xE0,, +5.001546,0x2E,, +5.001633,0xE0,, +5.00172,0xB1,, +5.001807,0x84,, +5.00942,0xA8,, +5.009507,0x01,, +5.009594,0x0C,, +5.009681,0x22,, +5.009768,0x74,, +5.009855,0x2E,, +5.009942,0xD8,, +5.010029,0x2E,, +5.010116,0xE4,, +5.010203,0x2E,, +5.01029,0xDC,, +5.010377,0x22,, +5.010464,0x60,, +5.010551,0x22,, +5.010637,0x60,, +5.010724,0x3B,, +5.010811,0x60,, +5.010898,0x22,, +5.010985,0x60,, +5.011072,0x22,, +5.011159,0x60,, +5.011246,0x3B,, +5.011333,0x60,, +5.01142,0x2E,, +5.011507,0xE0,, +5.011594,0x2E,, +5.011681,0xE0,, +5.011768,0xF2,, +5.011855,0x0C,, diff --git a/unittests/uorb_stub.cpp b/unittests/uorb_stub.cpp index fc039a408..e4269afc3 100644 --- a/unittests/uorb_stub.cpp +++ b/unittests/uorb_stub.cpp @@ -10,11 +10,13 @@ * TODO: use googlemock ******************************************/ -orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data) { +orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data) +{ return (orb_advert_t)0; } -int orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data) { +int orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data) +{ return 0; } |