Add CRC checking to python tools

Fix using full version of protobuf

fix issue with lora config defaults

fix a few things in cmake files

fix cmake flags

minor protobuf build fixes

.clang-format

@@ -3,8 +3,8 @@ Language:        Cpp
 # BasedOnStyle:  Google
 AccessModifierOffset: -1
 AlignAfterOpenBracket: Align
-AlignConsecutiveAssignments: true
-AlignConsecutiveDeclarations: true
+AlignConsecutiveAssignments: false
+AlignConsecutiveDeclarations: false
 AlignEscapedNewlines: Left
 AlignOperands:   true
 AlignTrailingComments: true

.gitignore

@@ -1,5 +1,12 @@
 /.vscode
+/.vs
+/.idea
 /sdkconfig
 /sdkconfig.old
 /build
-/cmake-build*
+/cmake-build*
+
+__pycache__
+*.pyc
+/venv
+/tools/venv

.gitmodules

@@ -13,3 +13,6 @@
 [submodule "components/minmea/minmea"]
 	path = components/minmea/minmea
 	url = https://github.com/kosma/minmea.git
+[submodule "components/protobuf/protobuf"]
+	path = components/protobuf/protobuf
+	url = https://github.com/protocolbuffers/protobuf.git

Dockerfile

@@ -9,10 +9,16 @@ flex \
 bison \
 gperf \
 python \
+python-pip \
+python-setuptools \
 python-serial \
+python-future \
+python-cryptography \
+python-pyparsing \
+python-pyelftools \
 cmake \
 ninja-build \
-protobuf-compiler \
+g++ \
 python-protobuf
 
 RUN mkdir /esp
@@ -22,7 +28,9 @@ RUN wget https://dl.espressif.com/dl/xtensa-esp32-elf-linux64-1.22.0-80-g6c4433a
 -O xtensa-esp32-elf.tar.gz
 RUN tar --extract --verbose --file xtensa-esp32-elf.tar.gz && rm xtensa-esp32-elf.tar.gz
 
-RUN git clone --branch v3.1.1 --depth 1 --recursive https://github.com/espressif/esp-idf.git
+RUN git clone --branch v3.2 --depth 1 --recursive https://github.com/espressif/esp-idf.git
 
 ENV PATH=/usr/local/sbin:/usr/local/bin:/usr/sbin:/usr/bin:/sbin:/bin:/esp/xtensa-esp32-elf/bin \
 IDF_PATH=/esp/esp-idf
+
+RUN python2.7 -m pip install --user -r $IDF_PATH/requirements.txt

build.sh

@@ -21,7 +21,7 @@ get_docker_run_flags() {
         if [ -t 0 ]; then
             DOCKER_RUN_FLAGS+=(--tty)
         fi
-        USB_SERIAL_DEVICES=$(find /dev -name 'ttyUSB0' -o -name 'ttyACM*')
+        USB_SERIAL_DEVICES=$(find /dev -name 'ttyUSB0' -o -name 'ttyACM*' || true)
         for device in $USB_SERIAL_DEVICES; do
             DOCKER_RUN_FLAGS+=(--device "$device")
             echo_status "Adding device $device to container"

calib/calib.py

@@ -0,0 +1,49 @@
+#!/usr/bin/env python3
+import serial
+import re
+import datetime
+import sys
+
+def capture_data():
+    num_re = r"([\-0-9\.]+)"
+    line_re = re.compile(r".*inputs: acc=\({0}, {0}, {0}\) gyro=\({0}, {0}, {0}\) mag=\({0}, {0}, {0}\)".format(num_re))
+
+    if len(sys.argv) >= 2:
+        ser_url = sys.argv[1]
+    else:
+        ser_url = "hwgrep://"
+    if len(sys.argv) >= 3:
+        fname = sys.argv[2]
+        f = open(fname, "w")
+    else:
+        f = sys.stdout
+        # timestr = datetime.datetime.now().strftime("%Y-%m-%d %H-%M-%S")
+        # fname = "UGVDATA_{}.csv".format(timestr)
+    ser = serial.serial_for_url(ser_url, baudrate=115200, parity=serial.PARITY_NONE,
+                                stopbits=serial.STOPBITS_ONE, bytesize=serial.EIGHTBITS)
+    with ser, f:
+        try:
+            f.write("AX,AY,AZ,GX,GY,GZ,MX,MY,MZ\n")
+            while True:
+                try:
+                    line = ser.read_until().decode("utf-8")
+                except Exception as e:
+                    print("line decode error: ", e, file=sys.stderr)
+                    continue
+                matches = line_re.match(line)
+                if not matches:
+                    print("line did not match: ", line, file=sys.stderr)
+                    continue
+                nums = [str(numstr) for numstr in matches.groups()]
+                if len(nums) != 9:
+                    continue
+                f.write(",".join(nums))
+                f.write("\n")
+                f.flush()
+        except KeyboardInterrupt:
+            print("interrupt", file=sys.stderr)
+        finally:
+            f.flush()
+
+if __name__ == "__main__":
+    capture_data()

calib/plot.sh

@@ -0,0 +1,12 @@
+#!/usr/bin/env bash
+
+file=$1
+
+gnuplot -persist <<EOF
+  set datafile separator ","
+  set xzeroaxis
+  set yzeroaxis
+  set style data points
+  set pointsize 0.5
+  plot "$file" using 7:8 with points title "X-Y", "$file" using 8:9 with points title "Y-Z", "$file" using 9:7 with points title "Z-X"
+EOF

components/MPU-driver

@@ -1 +1 @@
-Subproject commit ccc440bfd1c37f53ee275c1b7abac085677b5cc9
+Subproject commit c82b00502eb4c101a3f6b8134cd9b4a13f88e016

components/nanopb/functions.cmake

@@ -9,8 +9,8 @@ function(nanopb_generate PROTO_FILE PB_OUT HDR_VAR SRC_VAR)
     set(${HDR_VAR} ${${HDR_VAR}} ${PROTO_HDR} PARENT_SCOPE)
     set(${SRC_VAR} ${${SRC_VAR}} ${PROTO_SRC} PARENT_SCOPE)
     if(NOT CMAKE_SCRIPT_MODE_FILE)
-        add_custom_command(OUTPUT ${PROTO_HDR} ${PROTO_SRC} 
-            COMMAND protoc 
+        add_custom_command(OUTPUT ${PROTO_HDR} ${PROTO_SRC}
+            COMMAND protoc
                 --plugin=protoc-gen-nanopb=${NANOPB_PLUGIN}
                 --nanopb_out=${PB_OUT}
                 --proto_path ${PROTO_FILE_DIR}
@@ -18,7 +18,6 @@ function(nanopb_generate PROTO_FILE PB_OUT HDR_VAR SRC_VAR)
             DEPENDS ${PROTO_FILE_PATH}
             COMMENT "Generating nanopb sources for ${PROTO_FILE}")
         set_property(SOURCE ${PROTO_HDR} ${PROTO_SRC} PROPERTY GENERATED TRUE)
+        set_property(DIRECTORY . APPEND PROPERTY ADDITIONAL_MAKE_CLEAN_FILES ${PROTO_HDR} ${PROTO_SRC})
     endif()
-    list(APPEND ADDITIONAL_MAKE_CLEAN_FILES ${PROTO_HDR} ${PROTO_SRC})
 endfunction()
-

components/protobuf/CMakeLists.txt

@@ -0,0 +1,35 @@
+set(protobuf_source_dir ${CMAKE_CURRENT_LIST_DIR}/protobuf)
+
+set(libprotobuf_lite_files
+  ${protobuf_source_dir}/src/google/protobuf/arena.cc
+  ${protobuf_source_dir}/src/google/protobuf/arenastring.cc
+  ${protobuf_source_dir}/src/google/protobuf/extension_set.cc
+  ${protobuf_source_dir}/src/google/protobuf/generated_message_util.cc
+  ${protobuf_source_dir}/src/google/protobuf/io/coded_stream.cc
+  ${protobuf_source_dir}/src/google/protobuf/io/zero_copy_stream.cc
+  ${protobuf_source_dir}/src/google/protobuf/io/zero_copy_stream_impl_lite.cc
+  ${protobuf_source_dir}/src/google/protobuf/message_lite.cc
+  ${protobuf_source_dir}/src/google/protobuf/repeated_field.cc
+  ${protobuf_source_dir}/src/google/protobuf/stubs/atomicops_internals_x86_gcc.cc
+  ${protobuf_source_dir}/src/google/protobuf/stubs/atomicops_internals_x86_msvc.cc
+  ${protobuf_source_dir}/src/google/protobuf/stubs/bytestream.cc
+  ${protobuf_source_dir}/src/google/protobuf/stubs/common.cc
+  ${protobuf_source_dir}/src/google/protobuf/stubs/int128.cc
+  ${protobuf_source_dir}/src/google/protobuf/stubs/once.cc
+  ${protobuf_source_dir}/src/google/protobuf/stubs/status.cc
+  ${protobuf_source_dir}/src/google/protobuf/stubs/statusor.cc
+  ${protobuf_source_dir}/src/google/protobuf/stubs/stringpiece.cc
+  ${protobuf_source_dir}/src/google/protobuf/stubs/stringprintf.cc
+  ${protobuf_source_dir}/src/google/protobuf/stubs/structurally_valid.cc
+  ${protobuf_source_dir}/src/google/protobuf/stubs/strutil.cc
+  ${protobuf_source_dir}/src/google/protobuf/stubs/time.cc
+  ${protobuf_source_dir}/src/google/protobuf/wire_format_lite.cc
+)
+
+set(COMPONENT_SRCS ${libprotobuf_lite_files})
+set(COMPONENT_ADD_INCLUDEDIRS ${protobuf_source_dir}/src)
+
+register_component()
+
+component_compile_options(PUBLIC -DGOOGLE_PROTOBUF_NO_RTTI -Wno-error=maybe-uninitialized
+  PRIVATE -DHAVE_PTHREAD)

components/protobuf/functions.cmake

@@ -0,0 +1,56 @@
+if(NOT CMAKE_SCRIPT_MODE_FILE)
+
+set(PROTOBUF_DIR ${CMAKE_CURRENT_LIST_DIR}/protobuf)
+
+set(PROTOBUF_BUILD ${PROJECT_BINARY_DIR}/protobuf-build)
+set(PROTOBUF_BINARY ${PROJECT_BINARY_DIR}/protobuf-binary)
+set(PROTOBUF_SRC ${PROJECT_BINARY_DIR}/protobuf-src)
+
+configure_file(${CMAKE_CURRENT_LIST_DIR}/protobuf-CMakeLists.cmake ${PROTOBUF_BUILD}/CMakeLists.txt)
+if(NOT CMAKE_GENERATOR)
+    set(CMAKE_GENERATOR "Ninja")
+endif()
+execute_process(COMMAND ${CMAKE_COMMAND} -G "${CMAKE_GENERATOR}" .
+        RESULT_VARIABLE result
+        WORKING_DIRECTORY ${PROTOBUF_BUILD} )
+if(result)
+    message(FATAL_ERROR "CMake step for protobuf failed: ${result}")
+endif()
+# execute_process(COMMAND ${CMAKE_COMMAND} --build .
+#         RESULT_VARIABLE result
+#         WORKING_DIRECTORY ${PROTOBUF_BUILD})
+# if(result)
+#     message(FATAL_ERROR "Build step for protobuf failed: ${result}")
+# endif()
+
+set(PROTOC ${PROTOBUF_BINARY}/protoc)
+
+add_custom_command(OUTPUT ${PROTOC}
+    COMMENT "Building protoc binary"
+    COMMAND ${CMAKE_COMMAND} --build .
+    USES_TERMINAL
+    WORKING_DIRECTORY ${PROTOBUF_BUILD})
+endif()
+
+# find_program(PROTOC protoc PATHS ${CMAKE_CURRENT_LIST_DIR}/protobuf/cmake/build/)
+
+function(proto_generate_cpp PROTO_FILE PB_OUT HDR_VAR SRC_VAR)
+    get_filename_component(PROTO_FILE_NAME ${PROTO_FILE} NAME_WE)
+    get_filename_component(PROTO_FILE_PATH ${PROTO_FILE} ABSOLUTE BASE_DIR ${CMAKE_CURRENT_SOURCE_DIR})
+    get_filename_component(PROTO_FILE_DIR ${PROTO_FILE_PATH} DIRECTORY)
+    set(PROTO_HDR "${PB_OUT}/${PROTO_FILE_NAME}.pb.h")
+    set(PROTO_SRC "${PB_OUT}/${PROTO_FILE_NAME}.pb.cc")
+    set(${HDR_VAR} ${${HDR_VAR}} ${PROTO_HDR} PARENT_SCOPE)
+    set(${SRC_VAR} ${${SRC_VAR}} ${PROTO_SRC} PARENT_SCOPE)
+    if(NOT CMAKE_SCRIPT_MODE_FILE)
+        add_custom_command(OUTPUT ${PROTO_HDR} ${PROTO_SRC}
+            COMMAND ${PROTOC}
+                --cpp_out=${PB_OUT}
+                --proto_path ${PROTO_FILE_DIR}
+                ${PROTO_FILE_PATH}
+            DEPENDS ${PROTO_FILE_PATH} ${PROTOC}
+            COMMENT "Generating nanopb sources for ${PROTO_FILE}")
+        set_property(SOURCE ${PROTO_HDR} ${PROTO_SRC} PROPERTY GENERATED TRUE)
+        set_property(DIRECTORY . APPEND PROPERTY ADDITIONAL_MAKE_CLEAN_FILES ${PROTO_HDR} ${PROTO_SRC})
+    endif()
+endfunction()

components/protobuf/protobuf-CMakeLists.cmake

@@ -0,0 +1,17 @@
+cmake_minimum_required(VERSION 2.8.2)
+
+project(protobuf-build NONE)
+
+include(ExternalProject)
+ExternalProject_Add(protobuf
+  URL               "${PROTOBUF_DIR}"
+  SOURCE_DIR        "${PROTOBUF_SRC}"
+  BINARY_DIR        "${PROTOBUF_BINARY}"
+  SOURCE_SUBDIR     "cmake"
+  CMAKE_GENERATOR   "${CMAKE_GENERATOR}"
+  CMAKE_ARGS        -Dprotobuf_BUILD_TESTS:BOOL=OFF
+  INSTALL_COMMAND   ""
+  TEST_COMMAND      ""
+  USES_TERMINAL_CONFIGURE ON
+  USES_TERMINAL_BUILD ON
+)

components/sx127x_driver/sx127x_driver.h

@@ -75,15 +75,15 @@ typedef struct sx127x_config {
 // clang-format on
 
 typedef struct sx127x_packet_rx {
-  char *  data;
-  size_t  data_len;
+  uint8_t *data;
+  size_t   data_len;
   // rssi value
   int32_t rssi;
   // snr value in steps of .25
-  int8_t  snr;
+  int8_t snr;
 } sx127x_rx_packet_t;
 
-typedef struct sx127x* sx127x_hndl;
+typedef struct sx127x *sx127x_hndl;
 
 sx127x_config_t sx127x_config_default();
 

components/sx127x_driver/sx127x_internal.h

@@ -70,7 +70,7 @@ typedef enum sx127x_task_state {
 } sx127x_task_state_t;
 
 typedef struct sx127x_packet {
-  char * data;
+  uint8_t * data;
   size_t data_len;
 } sx127x_packet_t;
 

components/sx127x_driver/sx127x_pb.c

@@ -13,7 +13,7 @@ esp_err_t sx127x_send_packet_pb(sx127x_hndl hndl, const pb_field_t fields[],
                "task not running", ESP_ERR_INVALID_STATE);
 
   size_t data_len;
-  bool pb_ret = pb_get_encoded_size(&data_len, fields, src_struct);
+  bool   pb_ret = pb_get_encoded_size(&data_len, fields, src_struct);
   SX127X_CHECK(pb_ret == true, "pb_get_encoded_size fail", ESP_ERR_INVALID_ARG);
   SX127X_CHECK(data_len < SX127_MAX_PACKET_LEN, "packet len too long: %d",
                ESP_ERR_INVALID_ARG, data_len);
@@ -26,8 +26,8 @@ esp_err_t sx127x_send_packet_pb(sx127x_hndl hndl, const pb_field_t fields[],
   ESP_LOGV(SX127X_TAG, "encoded pb message %d bytes", data_len);
 
   sx127x_packet_t packet;
-  packet.data_len = data_len;
-  packet.data     = (char *)data;
+  packet.data_len  = data_len;
+  packet.data      = (uint8_t *)data;
   BaseType_t pdRet = xQueueSend(hndl->tx_packet_queue, &packet, ticks_to_wait);
   SX127X_CHECK(pdRet == pdTRUE, "tx queue full", ESP_ERR_TIMEOUT);
   return ESP_OK;

components/sx127x_driver/sx127x_registers.c

@@ -252,7 +252,7 @@ esp_err_t sx127x_read_lna_gain(sx127x_t *hndl, uint8_t *lna_gain) {
   return ESP_OK;
 }
 
-esp_err_t sx127x_write_fifo(sx127x_t *hndl, const char *data, size_t data_len) {
+esp_err_t sx127x_write_fifo(sx127x_t *hndl, const uint8_t *data, size_t data_len) {
   spi_transaction_t trans;
   memset(&trans, 0, sizeof(trans));
   trans.flags     = 0;
@@ -274,7 +274,7 @@ esp_err_t sx127x_write_fifo(sx127x_t *hndl, const char *data, size_t data_len) {
   return ESP_OK;
 }
 
-esp_err_t sx127x_read_fifo(sx127x_t *hndl, char *data_out, size_t data_len) {
+esp_err_t sx127x_read_fifo(sx127x_t *hndl, uint8_t *data_out, size_t data_len) {
   spi_transaction_t trans;
   memset(&trans, 0, sizeof(trans));
   trans.flags     = 0;

components/sx127x_driver/sx127x_registers.h

@@ -99,9 +99,9 @@ esp_err_t sx127x_read_pkt_snr(sx127x_hndl hdnl, int8_t *snr);
 esp_err_t sx127x_read_rssi(sx127x_hndl hdnl, int32_t *rssi);
 esp_err_t sx127x_read_lna_gain(sx127x_hndl hdnl, uint8_t *lna_gain);
 
-esp_err_t sx127x_write_fifo(sx127x_hndl hdnl, const char *data,
+esp_err_t sx127x_write_fifo(sx127x_hndl hdnl, const uint8_t *data,
                             size_t data_len);
-esp_err_t sx127x_read_fifo(sx127x_hndl hdnl, char *data_out, size_t data_len);
+esp_err_t sx127x_read_fifo(sx127x_hndl hdnl, uint8_t *data_out, size_t data_len);
 
 #ifdef __cplusplus
 }

main/CMakeLists.txt

@@ -1,21 +1,43 @@
-include(${CMAKE_CURRENT_LIST_DIR}/../components/nanopb/functions.cmake)
-
-set(PB_OUT ${CMAKE_CURRENT_SOURCE_DIR}/pb_out)
+include(${CMAKE_CURRENT_LIST_DIR}/../components/protobuf/functions.cmake)
 
+set(PB_OUT ${CMAKE_CURRENT_LIST_DIR}/pb_out)
 make_directory(${PB_OUT})
-nanopb_generate(messages.proto ${PB_OUT} PROTO_HDRS PROTO_SRCS)
 
-list(APPEND COMPONENT_SRCS "ugv_main.cc"
-    "ugv_comms.c"
+set(COMPONENT_SRCS
+    "ugv_main.cc"
+    "ugv_comms.hh"
+    "ugv_comms.cc"
+    "ugv_display.hh"
+    "ugv_display.cc"
+    "ugv_io.hh"
     "ugv_io.cc"
+    "ugv_io_gps.hh"
     "ugv_io_gps.cc"
+    "ugv_io_mpu.hh"
     "ugv_io_mpu.cc"
+    "u8g2_esp32_hal.h"
     "u8g2_esp32_hal.c"
+    "Print.h"
     "Print.cpp"
-    ${PROTO_SRCS})
+    "MadgwickAHRS.h"
+    "MadgwickAHRS.cpp"
+    "e32_driver.hh"
+    "e32_driver.cc"
+    "pid_controller.hh"
+    "pid_controller.cc"
+    "lat_long.hh"
+    "lat_long.cc"
+    "ugv.hh"
+    "ugv.cc"
+)
 set(COMPONENT_PRIV_INCLUDEDIRS "." ${PB_OUT})
-set(COMPONENT_REQUIRES "u8g2" "sx127x_driver" "nanopb" "MPU-driver" "minmea")
+set(COMPONENT_REQUIRES "u8g2" "sx127x_driver" "protobuf" "MPU-driver" "minmea" "mbedtls")
+
+proto_generate_cpp(messages.proto ${PB_OUT} PROTO_HDRS PROTO_SRCS)
+proto_generate_cpp(config.proto ${PB_OUT} PROTO_HDRS PROTO_SRCS)
+list(APPEND COMPONENT_SRCS ${PROTO_SRCS} ${PROTO_HDRS})
 
 register_component()
 
-component_compile_options("-Werror=incompatible-pointer-types")
+component_compile_options("-Werror=incompatible-pointer-types" "-std=c++14")
+component_compile_options(-I${PB_OUT})

main/MadgwickAHRS.cpp

@@ -0,0 +1,291 @@
+//=============================================================================================
+// MadgwickAHRS.c
+//=============================================================================================
+//
+// Implementation of Madgwick's IMU and AHRS algorithms.
+// See: http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/
+//
+// From the x-io website "Open-source resources available on this website are
+// provided under the GNU General Public Licence unless an alternative licence
+// is provided in source."
+//
+// Date			Author          Notes
+// 29/09/2011	SOH Madgwick    Initial release
+// 02/10/2011	SOH Madgwick	Optimised for reduced CPU load
+// 19/02/2012	SOH Madgwick	Magnetometer measurement is normalised
+//
+//=============================================================================================
+
+//-------------------------------------------------------------------------------------------
+// Header files
+
+#include "MadgwickAHRS.h"
+#include <math.h>
+#include <string.h>
+
+//-------------------------------------------------------------------------------------------
+// Definitions
+
+#define sampleFreqDef 512.0f  // sample frequency in Hz
+#define betaDef 0.2f          // 2 * proportional gain
+
+//============================================================================================
+// Functions
+
+//-------------------------------------------------------------------------------------------
+// AHRS algorithm update
+
+Madgwick::Madgwick() {
+  beta = betaDef;
+  q0 = 1.0f;
+  q1 = 0.0f;
+  q2 = 0.0f;
+  q3 = 0.0f;
+  invSampleFreq = 1.0f / sampleFreqDef;
+  anglesComputed = 0;
+}
+
+void Madgwick::update(float gx, float gy, float gz, float ax, float ay,
+                      float az, float mx, float my, float mz) {
+  float recipNorm;
+  float s0, s1, s2, s3;
+  float qDot1, qDot2, qDot3, qDot4;
+  float hx, hy;
+  float _2q0mx, _2q0my, _2q0mz, _2q1mx, _2bx, _2bz, _4bx, _4bz, _2q0, _2q1,
+      _2q2, _2q3, _2q0q2, _2q2q3, q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3,
+      q2q2, q2q3, q3q3;
+
+  // Use IMU algorithm if magnetometer measurement invalid (avoids NaN in
+  // magnetometer normalisation)
+  if ((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) {
+    updateIMU(gx, gy, gz, ax, ay, az);
+    return;
+  }
+
+  // Convert gyroscope degrees/sec to radians/sec
+  gx *= 0.0174533f;
+  gy *= 0.0174533f;
+  gz *= 0.0174533f;
+
+  // Rate of change of quaternion from gyroscope
+  qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
+  qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);
+  qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);
+  qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);
+
+  // Compute feedback only if accelerometer measurement valid (avoids NaN in
+  // accelerometer normalisation)
+  if (!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
+    // Normalise accelerometer measurement
+    recipNorm = invSqrt(ax * ax + ay * ay + az * az);
+    ax *= recipNorm;
+    ay *= recipNorm;
+    az *= recipNorm;
+
+    // Normalise magnetometer measurement
+    recipNorm = invSqrt(mx * mx + my * my + mz * mz);
+    mx *= recipNorm;
+    my *= recipNorm;
+    mz *= recipNorm;
+
+    // Auxiliary variables to avoid repeated arithmetic
+    _2q0mx = 2.0f * q0 * mx;
+    _2q0my = 2.0f * q0 * my;
+    _2q0mz = 2.0f * q0 * mz;
+    _2q1mx = 2.0f * q1 * mx;
+    _2q0 = 2.0f * q0;
+    _2q1 = 2.0f * q1;
+    _2q2 = 2.0f * q2;
+    _2q3 = 2.0f * q3;
+    _2q0q2 = 2.0f * q0 * q2;
+    _2q2q3 = 2.0f * q2 * q3;
+    q0q0 = q0 * q0;
+    q0q1 = q0 * q1;
+    q0q2 = q0 * q2;
+    q0q3 = q0 * q3;
+    q1q1 = q1 * q1;
+    q1q2 = q1 * q2;
+    q1q3 = q1 * q3;
+    q2q2 = q2 * q2;
+    q2q3 = q2 * q3;
+    q3q3 = q3 * q3;
+
+    // Reference direction of Earth's magnetic field
+    hx = mx * q0q0 - _2q0my * q3 + _2q0mz * q2 + mx * q1q1 + _2q1 * my * q2 +
+         _2q1 * mz * q3 - mx * q2q2 - mx * q3q3;
+    hy = _2q0mx * q3 + my * q0q0 - _2q0mz * q1 + _2q1mx * q2 - my * q1q1 +
+         my * q2q2 + _2q2 * mz * q3 - my * q3q3;
+    _2bx = sqrtf(hx * hx + hy * hy);
+    _2bz = -_2q0mx * q2 + _2q0my * q1 + mz * q0q0 + _2q1mx * q3 - mz * q1q1 +
+           _2q2 * my * q3 - mz * q2q2 + mz * q3q3;
+    _4bx = 2.0f * _2bx;
+    _4bz = 2.0f * _2bz;
+
+    // Gradient decent algorithm corrective step
+    s0 = -_2q2 * (2.0f * q1q3 - _2q0q2 - ax) +
+         _2q1 * (2.0f * q0q1 + _2q2q3 - ay) -
+         _2bz * q2 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) +
+         (-_2bx * q3 + _2bz * q1) *
+             (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) +
+         _2bx * q2 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
+    s1 = _2q3 * (2.0f * q1q3 - _2q0q2 - ax) +
+         _2q0 * (2.0f * q0q1 + _2q2q3 - ay) -
+         4.0f * q1 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) +
+         _2bz * q3 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) +
+         (_2bx * q2 + _2bz * q0) *
+             (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) +
+         (_2bx * q3 - _4bz * q1) *
+             (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
+    s2 = -_2q0 * (2.0f * q1q3 - _2q0q2 - ax) +
+         _2q3 * (2.0f * q0q1 + _2q2q3 - ay) -
+         4.0f * q2 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) +
+         (-_4bx * q2 - _2bz * q0) *
+             (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) +
+         (_2bx * q1 + _2bz * q3) *
+             (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) +
+         (_2bx * q0 - _4bz * q2) *
+             (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
+    s3 = _2q1 * (2.0f * q1q3 - _2q0q2 - ax) +
+         _2q2 * (2.0f * q0q1 + _2q2q3 - ay) +
+         (-_4bx * q3 + _2bz * q1) *
+             (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) +
+         (-_2bx * q0 + _2bz * q2) *
+             (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) +
+         _2bx * q1 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
+    recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 +
+                        s3 * s3);  // normalise step magnitude
+    s0 *= recipNorm;
+    s1 *= recipNorm;
+    s2 *= recipNorm;
+    s3 *= recipNorm;
+
+    // Apply feedback step
+    qDot1 -= beta * s0;
+    qDot2 -= beta * s1;
+    qDot3 -= beta * s2;
+    qDot4 -= beta * s3;
+  }
+
+  // Integrate rate of change of quaternion to yield quaternion
+  q0 += qDot1 * invSampleFreq;
+  q1 += qDot2 * invSampleFreq;
+  q2 += qDot3 * invSampleFreq;
+  q3 += qDot4 * invSampleFreq;
+
+  // Normalise quaternion
+  recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
+  q0 *= recipNorm;
+  q1 *= recipNorm;
+  q2 *= recipNorm;
+  q3 *= recipNorm;
+  anglesComputed = 0;
+}
+
+//-------------------------------------------------------------------------------------------
+// IMU algorithm update
+
+void Madgwick::updateIMU(float gx, float gy, float gz, float ax, float ay,
+                         float az) {
+  float recipNorm;
+  float s0, s1, s2, s3;
+  float qDot1, qDot2, qDot3, qDot4;
+  float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2, _8q1, _8q2, q0q0, q1q1, q2q2,
+      q3q3;
+
+  // Convert gyroscope degrees/sec to radians/sec
+  gx *= 0.0174533f;
+  gy *= 0.0174533f;
+  gz *= 0.0174533f;
+
+  // Rate of change of quaternion from gyroscope
+  qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
+  qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);
+  qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);
+  qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);
+
+  // Compute feedback only if accelerometer measurement valid (avoids NaN in
+  // accelerometer normalisation)
+  if (!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
+    // Normalise accelerometer measurement
+    recipNorm = invSqrt(ax * ax + ay * ay + az * az);
+    ax *= recipNorm;
+    ay *= recipNorm;
+    az *= recipNorm;
+
+    // Auxiliary variables to avoid repeated arithmetic
+    _2q0 = 2.0f * q0;
+    _2q1 = 2.0f * q1;
+    _2q2 = 2.0f * q2;
+    _2q3 = 2.0f * q3;
+    _4q0 = 4.0f * q0;
+    _4q1 = 4.0f * q1;
+    _4q2 = 4.0f * q2;
+    _8q1 = 8.0f * q1;
+    _8q2 = 8.0f * q2;
+    q0q0 = q0 * q0;
+    q1q1 = q1 * q1;
+    q2q2 = q2 * q2;
+    q3q3 = q3 * q3;
+
+    // Gradient decent algorithm corrective step
+    s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay;
+    s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * q1 - _2q0 * ay - _4q1 +
+         _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az;
+    s2 = 4.0f * q0q0 * q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 +
+         _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az;
+    s3 = 4.0f * q1q1 * q3 - _2q1 * ax + 4.0f * q2q2 * q3 - _2q2 * ay;
+    recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 +
+                        s3 * s3);  // normalise step magnitude
+    s0 *= recipNorm;
+    s1 *= recipNorm;
+    s2 *= recipNorm;
+    s3 *= recipNorm;
+
+    // Apply feedback step
+    qDot1 -= beta * s0;
+    qDot2 -= beta * s1;
+    qDot3 -= beta * s2;
+    qDot4 -= beta * s3;
+  }
+
+  // Integrate rate of change of quaternion to yield quaternion
+  q0 += qDot1 * invSampleFreq;
+  q1 += qDot2 * invSampleFreq;
+  q2 += qDot3 * invSampleFreq;
+  q3 += qDot4 * invSampleFreq;
+
+  // Normalise quaternion
+  recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
+  q0 *= recipNorm;
+  q1 *= recipNorm;
+  q2 *= recipNorm;
+  q3 *= recipNorm;
+  anglesComputed = 0;
+}
+
+//-------------------------------------------------------------------------------------------
+// Fast inverse square-root
+// See: http://en.wikipedia.org/wiki/Fast_inverse_square_root
+
+float Madgwick::invSqrt(float x) {
+  float halfx = 0.5f * x;
+  float y = x;
+  // long  i     = *(long *)&y;
+  long i;
+  memcpy(&i, &y, sizeof(float));
+  i = 0x5f3759df - (i >> 1);
+  // y           = *(float *)&i;
+  memcpy(&y, &i, sizeof(float));
+  y = y * (1.5f - (halfx * y * y));
+  y = y * (1.5f - (halfx * y * y));
+  return y;
+}
+
+//-------------------------------------------------------------------------------------------
+
+void Madgwick::computeAngles() {
+  roll = atan2f(q0 * q1 + q2 * q3, 0.5f - q1 * q1 - q2 * q2);
+  pitch = asinf(-2.0f * (q1 * q3 - q0 * q2));
+  yaw = atan2f(q1 * q2 + q0 * q3, 0.5f - q2 * q2 - q3 * q3);
+  anglesComputed = 1;
+}

main/MadgwickAHRS.h

@@ -0,0 +1,88 @@
+//=============================================================================================
+// MadgwickAHRS.h
+//=============================================================================================
+//
+// Implementation of Madgwick's IMU and AHRS algorithms.
+// See: http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/
+//
+// From the x-io website "Open-source resources available on this website are
+// provided under the GNU General Public Licence unless an alternative licence
+// is provided in source."
+//
+// Date			Author          Notes
+// 29/09/2011	SOH Madgwick    Initial release
+// 02/10/2011	SOH Madgwick	Optimised for reduced CPU load
+//
+//=============================================================================================
+#ifndef MadgwickAHRS_h
+#define MadgwickAHRS_h
+
+#include <math.h>
+
+//--------------------------------------------------------------------------------------------
+// Variable declaration
+class Madgwick {
+ private:
+  static float invSqrt(float x);
+
+  float beta;  // algorithm gain
+  float q0;
+  float q1;
+  float q2;
+  float q3;  // quaternion of sensor frame relative to auxiliary frame
+  float invSampleFreq;
+  float roll;
+  float pitch;
+  float yaw;
+  char anglesComputed;
+
+  void computeAngles();
+
+  //-------------------------------------------------------------------------------------------
+  // Function declarations
+ public:
+  Madgwick(void);
+
+  void begin(float sampleFrequency) { invSampleFreq = 1.0f / sampleFrequency; }
+
+  void update(float gx, float gy, float gz, float ax, float ay, float az,
+              float mx, float my, float mz);
+
+  void updateIMU(float gx, float gy, float gz, float ax, float ay, float az);
+
+  // float getPitch(){return atan2f(2.0f * q2 * q3 - 2.0f * q0 * q1, 2.0f * q0 *
+  // q0 + 2.0f * q3 * q3 - 1.0f);}; float getRoll(){return -1.0f * asinf(2.0f *
+  // q1 * q3 + 2.0f * q0 * q2);}; float getYaw(){return atan2f(2.0f * q1 * q2
+  // - 2.0f * q0 * q3, 2.0f * q0 * q0 + 2.0f * q1 * q1 - 1.0f);};
+  float getRoll() {
+    if (!anglesComputed) computeAngles();
+    return roll * 57.29578f;
+  }
+
+  float getPitch() {
+    if (!anglesComputed) computeAngles();
+    return pitch * 57.29578f;
+  }
+
+  float getYaw() {
+    if (!anglesComputed) computeAngles();
+    return yaw * 57.29578f + 180.0f;
+  }
+
+  float getRollRadians() {
+    if (!anglesComputed) computeAngles();
+    return roll;
+  }
+
+  float getPitchRadians() {
+    if (!anglesComputed) computeAngles();
+    return pitch;
+  }
+
+  float getYawRadians() {
+    if (!anglesComputed) computeAngles();
+    return yaw;
+  }
+};
+
+#endif

main/Print.cpp

@@ -41,8 +41,8 @@ size_t Print::write(const uint8_t *buffer, size_t size) {
 }
 
 size_t Print::printf(const char *format, ...) {
-  char    loc_buf[64];
-  char *  temp = loc_buf;
+  char loc_buf[64];
+  char *temp = loc_buf;
   va_list arg;
   va_list copy;
   va_start(arg, format);
@@ -66,7 +66,9 @@ size_t Print::printf(const char *format, ...) {
 
 // size_t Print::print(const String &s) { return write(s.c_str(), s.length()); }
 
-size_t Print::print(const std::string &s) { return write(s.c_str(), s.length()); }
+size_t Print::print(const std::string &s) {
+  return write(s.c_str(), s.length());
+}
 
 size_t Print::print(const char str[]) { return write(str); }
 
@@ -88,7 +90,7 @@ size_t Print::print(long n, int base) {
   } else if (base == 10) {
     if (n < 0) {
       int t = print('-');
-      n     = -n;
+      n = -n;
       return printNumber(n, 10) + t;
     }
     return printNumber(n, 10);
@@ -114,7 +116,7 @@ size_t Print::print(struct tm *timeinfo, const char *format) {
   if (!f) {
     f = "%c";
   }
-  char   buf[64];
+  char buf[64];
   size_t written = strftime(buf, 64, f, timeinfo);
   print(buf);
   return written;
@@ -197,7 +199,7 @@ size_t Print::println(struct tm *timeinfo, const char *format) {
 // Private Methods /////////////////////////////////////////////////////////////
 
 size_t Print::printNumber(unsigned long n, uint8_t base) {
-  char  buf[8 * sizeof(long) + 1];  // Assumes 8-bit chars plus zero byte.
+  char buf[8 * sizeof(long) + 1];  // Assumes 8-bit chars plus zero byte.
   char *str = &buf[sizeof(buf) - 1];
 
   *str = '\0';
@@ -248,8 +250,8 @@ size_t Print::printFloat(double number, uint8_t digits) {
   number += rounding;
 
   // Extract the integer part of the number and print it
-  unsigned long int_part  = (unsigned long)number;
-  double        remainder = number - (double)int_part;
+  unsigned long int_part = (unsigned long)number;
+  double remainder = number - (double)int_part;
   n += print(int_part);
 
   // Print the decimal point, but only if there are digits beyond

main/Print.h

@@ -17,8 +17,9 @@
 #ifndef Print_h
 #define Print_h
 
-#include <stdint.h>
 #include <stddef.h>
+#include <stdint.h>
+#include <string.h>
 #include <string>
 
 // #include "Printable.h"
@@ -28,73 +29,60 @@
 #define OCT 8
 #define BIN 2
 
-class Print
-{
-private:
-    int write_error;
-    size_t printNumber(unsigned long, uint8_t);
-    size_t printFloat(double, uint8_t);
-protected:
-    void setWriteError(int err = 1)
-    {
-        write_error = err;
-    }
-public:
-    Print() :
-        write_error(0)
-    {
-    }
-    virtual ~Print() {}
-    int getWriteError()
-    {
-        return write_error;
-    }
-    void clearWriteError()
-    {
-        setWriteError(0);
-    }
+class Print {
+ private:
+  int write_error;
+  size_t printNumber(unsigned long, uint8_t);
+  size_t printFloat(double, uint8_t);
 
-    virtual size_t write(uint8_t) = 0;
-    size_t write(const char *str)
-    {
-        if(str == NULL) {
-            return 0;
-        }
-        return write((const uint8_t *) str, strlen(str));
-    }
-    virtual size_t write(const uint8_t *buffer, size_t size);
-    size_t write(const char *buffer, size_t size)
-    {
-        return write((const uint8_t *) buffer, size);
+ protected:
+  void setWriteError(int err = 1) { write_error = err; }
+
+ public:
+  Print() : write_error(0) {}
+  virtual ~Print() {}
+  int getWriteError() { return write_error; }
+  void clearWriteError() { setWriteError(0); }
+
+  virtual size_t write(uint8_t) = 0;
+  size_t write(const char *str) {
+    if (str == NULL) {
+      return 0;
     }
+    return write((const uint8_t *)str, strlen(str));
+  }
+  virtual size_t write(const uint8_t *buffer, size_t size);
+  size_t write(const char *buffer, size_t size) {
+    return write((const uint8_t *)buffer, size);
+  }
 
-    size_t printf(const char * format, ...)  __attribute__ ((format (printf, 2, 3)));
-    // size_t print(const String &);
-    size_t print(const std::string &);
-    size_t print(const char[]);
-    size_t print(char);
-    size_t print(unsigned char, int = DEC);
-    size_t print(int, int = DEC);
-    size_t print(unsigned int, int = DEC);
-    size_t print(long, int = DEC);
-    size_t print(unsigned long, int = DEC);
-    size_t print(double, int = 2);
-    // size_t print(const Printable&);
-    size_t print(struct tm * timeinfo, const char * format = NULL);
+  size_t printf(const char *format, ...) __attribute__((format(printf, 2, 3)));
+  // size_t print(const String &);
+  size_t print(const std::string &);
+  size_t print(const char[]);
+  size_t print(char);
+  size_t print(unsigned char, int = DEC);
+  size_t print(int, int = DEC);
+  size_t print(unsigned int, int = DEC);
+  size_t print(long, int = DEC);
+  size_t print(unsigned long, int = DEC);
+  size_t print(double, int = 2);
+  // size_t print(const Printable&);
+  size_t print(struct tm *timeinfo, const char *format = NULL);
 
-    // size_t println(const String &s);
-    size_t println(const std::string &s);
-    size_t println(const char[]);
-    size_t println(char);
-    size_t println(unsigned char, int = DEC);
-    size_t println(int, int = DEC);
-    size_t println(unsigned int, int = DEC);
-    size_t println(long, int = DEC);
-    size_t println(unsigned long, int = DEC);
-    size_t println(double, int = 2);
-    // size_t println(const Printable&);
-    size_t println(struct tm * timeinfo, const char * format = NULL);
-    size_t println(void);
+  // size_t println(const String &s);
+  size_t println(const std::string &s);
+  size_t println(const char[]);
+  size_t println(char);
+  size_t println(unsigned char, int = DEC);
+  size_t println(int, int = DEC);
+  size_t println(unsigned int, int = DEC);
+  size_t println(long, int = DEC);
+  size_t println(unsigned long, int = DEC);
+  size_t println(double, int = 2);
+  // size_t println(const Printable&);
+  size_t println(struct tm *timeinfo, const char *format = NULL);
+  size_t println(void);
 };
 
 #endif

main/U8g2lib.hh

@@ -6,7 +6,7 @@
 
 class U8G2 : public Print {
  protected:
-  u8g2_t       u8g2;
+  u8g2_t u8g2;
   u8x8_char_cb cpp_next_cb; /*  the cpp interface has its own decoding function
                                for the Arduino print command */
  public:
@@ -20,7 +20,7 @@ class U8G2 : public Print {
   u8g2_t *getU8g2(void) { return &u8g2; }
 
   uint32_t getBusClock(void) { return u8g2_GetU8x8(&u8g2)->bus_clock; }
-  void     setBusClock(uint32_t clock_speed) {
+  void setBusClock(uint32_t clock_speed) {
     u8g2_GetU8x8(&u8g2)->bus_clock = clock_speed;
   }
 
@@ -32,13 +32,13 @@ class U8G2 : public Print {
   /* u8x8 interface */
   uint8_t getCols(void) { return u8x8_GetCols(u8g2_GetU8x8(&u8g2)); }
   uint8_t getRows(void) { return u8x8_GetRows(u8g2_GetU8x8(&u8g2)); }
-  void    drawTile(uint8_t x, uint8_t y, uint8_t cnt, uint8_t *tile_ptr) {
+  void drawTile(uint8_t x, uint8_t y, uint8_t cnt, uint8_t *tile_ptr) {
     u8x8_DrawTile(u8g2_GetU8x8(&u8g2), x, y, cnt, tile_ptr);
   }
 
 #ifdef U8X8_WITH_USER_PTR
   void *getUserPtr() { return u8g2_GetUserPtr(&u8g2); }
-  void  setUserPtr(void *p) { u8g2_SetUserPtr(&u8g2, p); }
+  void setUserPtr(void *p) { u8g2_SetUserPtr(&u8g2, p); }
 #endif
 
 #ifdef U8X8_USE_PINS
@@ -118,13 +118,13 @@ class U8G2 : public Print {
   void sendBuffer(void) { u8g2_SendBuffer(&u8g2); }
   void clearBuffer(void) { u8g2_ClearBuffer(&u8g2); }
 
-  void    firstPage(void) { u8g2_FirstPage(&u8g2); }
+  void firstPage(void) { u8g2_FirstPage(&u8g2); }
   uint8_t nextPage(void) { return u8g2_NextPage(&u8g2); }
 
   uint8_t *getBufferPtr(void) { return u8g2_GetBufferPtr(&u8g2); }
-  uint8_t  getBufferTileHeight(void) { return u8g2_GetBufferTileHeight(&u8g2); }
-  uint8_t  getBufferTileWidth(void) { return u8g2_GetBufferTileWidth(&u8g2); }
-  uint8_t  getPageCurrTileRow(void) {
+  uint8_t getBufferTileHeight(void) { return u8g2_GetBufferTileHeight(&u8g2); }
+  uint8_t getBufferTileWidth(void) { return u8g2_GetBufferTileWidth(&u8g2); }
+  uint8_t getPageCurrTileRow(void) {
     return u8g2_GetBufferCurrTileRow(&u8g2);
   }  // obsolete
   void setPageCurrTileRow(uint8_t row) {
@@ -339,25 +339,31 @@ class U8G2 : public Print {
   void setColorIndex(uint8_t color_index) {
     u8g2_SetDrawColor(&u8g2, color_index);
   }
-  uint8_t     getColorIndex(void) { return u8g2_GetDrawColor(&u8g2); }
-  int8_t      getFontAscent(void) { return u8g2_GetAscent(&u8g2); }
-  int8_t      getFontDescent(void) { return u8g2_GetDescent(&u8g2); }
-  int8_t      getMaxCharHeight(void) { return u8g2_GetMaxCharHeight(&u8g2); }
-  int8_t      getMaxCharWidth(void) { return u8g2_GetMaxCharWidth(&u8g2); }
+  uint8_t getColorIndex(void) { return u8g2_GetDrawColor(&u8g2); }
+  int8_t getFontAscent(void) { return u8g2_GetAscent(&u8g2); }
+  int8_t getFontDescent(void) { return u8g2_GetDescent(&u8g2); }
+  int8_t getMaxCharHeight(void) { return u8g2_GetMaxCharHeight(&u8g2); }
+  int8_t getMaxCharWidth(void) { return u8g2_GetMaxCharWidth(&u8g2); }
   u8g2_uint_t getHeight() { return u8g2_GetDisplayHeight(&u8g2); }
   u8g2_uint_t getWidth() { return u8g2_GetDisplayWidth(&u8g2); }
 };
 
-void u8x8_SetPin_HW_I2C(u8x8_t *u8x8, uint8_t reset, uint8_t clock, uint8_t data)
-{
+void u8x8_SetPin_HW_I2C(u8x8_t *u8x8, uint8_t reset, uint8_t clock,
+                        uint8_t data) {
   u8x8_SetPin(u8x8, U8X8_PIN_RESET, reset);
   u8x8_SetPin(u8x8, U8X8_PIN_I2C_CLOCK, clock);
   u8x8_SetPin(u8x8, U8X8_PIN_I2C_DATA, data);
 }
 
 class U8G2_SSD1306_128X64_NONAME_F_HW_I2C : public U8G2 {
-  public: U8G2_SSD1306_128X64_NONAME_F_HW_I2C(const u8g2_cb_t *rotation, uint8_t reset = U8X8_PIN_NONE, uint8_t clock = U8X8_PIN_NONE, uint8_t data = U8X8_PIN_NONE) : U8G2() {
-    u8g2_Setup_ssd1306_i2c_128x64_noname_f(&u8g2, rotation, u8g2_esp32_i2c_byte_cb, u8g2_esp32_gpio_and_delay_cb);
+ public:
+  U8G2_SSD1306_128X64_NONAME_F_HW_I2C(const u8g2_cb_t *rotation,
+                                      uint8_t reset = U8X8_PIN_NONE,
+                                      uint8_t clock = U8X8_PIN_NONE,
+                                      uint8_t data = U8X8_PIN_NONE)
+      : U8G2() {
+    u8g2_Setup_ssd1306_i2c_128x64_noname_f(
+        &u8g2, rotation, u8g2_esp32_i2c_byte_cb, u8g2_esp32_gpio_and_delay_cb);
     u8x8_SetPin_HW_I2C(getU8x8(), reset, clock, data);
   }
 };

main/config.proto

@@ -0,0 +1,21 @@
+syntax = "proto3";
+
+package ugv.config;
+
+option optimize_for = LITE_RUNTIME;
+
+message PidParams {
+  float kp = 1;
+  float ki = 2;
+  float kd = 3;
+  float max_output = 4;
+  float max_i_error = 5;
+}
+
+message Config {
+  float min_target_dist = 1;
+  float drive_power = 2;
+  PidParams angle_pid = 3;
+  float min_flip_pitch = 4;
+  float mag_declination = 5;
+}

main/e32_driver.cc

@@ -0,0 +1,359 @@
+#include "e32_driver.hh"
+
+#include <esp_log.h>
+
+namespace ugv {
+namespace e32 {
+
+static constexpr size_t E32_BUF_SIZE = 1024;
+static constexpr size_t E32_UART_RX_BUF_SIZE = 1024;
+static constexpr size_t E32_UART_TX_BUF_SIZE = 1024;
+
+static constexpr size_t PARAMS_LEN = 6;
+static const uint8_t CMD_WRITE_PARAMS_SAVE = 0xC0;
+static const uint8_t CMD_READ_PARAMS[] = {0xC1, 0xC1, 0xC1};
+static const uint8_t CMD_WRITE_PARAMS_NO_SAVE = 0xC2;
+static const uint8_t CMD_READ_VERSION[] = {0xC3, 0xC3, 0xC3};
+static const uint8_t CMD_RESET[] = {0xC4, 0xC4, 0xC4};
+
+static const char* TAG = "e32_driver";
+
+Config::Config() {
+  uart_port = UART_NUM_1;
+  uart_parity = UART_PARITY_DISABLE;
+  uart_tx_pin = UART_PIN_NO_CHANGE;
+  uart_rx_pin = UART_PIN_NO_CHANGE;
+  uart_baud = 9600;
+}
+
+Params::Params() {
+  // These are defaults for the 433T30D
+  save_params = true;
+  address = 0x0000;
+  uart_partity = UART_PARITY_DISABLE;
+  uart_baud = 9600;      // bps
+  air_data_rate = 2400;  // bps
+  comm_channel = 0x17;
+  tx_mode = TxTransparent;
+  io_mode = IoPushPull;
+  wake_up_time = 250;  // ms
+  fec_enabled = true;
+  tx_power = 30;
+}
+
+E32_Driver::E32_Driver() : initialized_(false), config_(), params_() {}
+
+E32_Driver::~E32_Driver() { Free(); }
+
+esp_err_t E32_Driver::Init(Config config) {
+  config_ = config;
+  uart_config_t uart_config;
+  uart_config.baud_rate = config_.uart_baud;
+  uart_config.data_bits = UART_DATA_8_BITS;
+  uart_config.parity = config_.uart_parity;
+  uart_config.stop_bits = UART_STOP_BITS_1;
+  uart_config.flow_ctrl = UART_HW_FLOWCTRL_DISABLE;
+  uart_config.rx_flow_ctrl_thresh = 122;
+  uart_config.use_ref_tick = false;
+
+  esp_err_t ret;
+  ret = uart_param_config(config_.uart_port, &uart_config);
+  if (ret != ESP_OK) {
+    goto error;
+  }
+
+  ret =
+      uart_set_pin(config_.uart_port, config_.uart_tx_pin, config_.uart_rx_pin,
+                   UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE);
+  if (ret != ESP_OK) {
+    goto error;
+  }
+
+  ret = uart_driver_install(config_.uart_port, E32_UART_RX_BUF_SIZE,
+                            E32_UART_TX_BUF_SIZE, 0, NULL, 0);
+  if (ret != ESP_OK) {
+    goto error;
+  }
+  initialized_ = true;
+
+  // ReadParams(params_);
+  // if (ret != ESP_OK) {
+  // goto error;
+  // }
+
+  return ESP_OK;
+
+error:
+  const char* error_name = esp_err_to_name(ret);
+  ESP_LOGE(TAG, "E32_Driver::Init error: %s (%d)", error_name, ret);
+  return ret;
+}
+
+esp_err_t E32_Driver::Free() {
+  esp_err_t ret;
+  if (initialized_) {
+    ret = uart_driver_delete(config_.uart_port);
+    initialized_ = false;
+  } else {
+    ret = ESP_ERR_INVALID_STATE;
+    ESP_LOGE(TAG, "Free called when not initialized");
+  }
+  return ret;
+}
+
+esp_err_t E32_Driver::ReadParams(Params& params) {
+  esp_err_t ret;
+  ret = RawWrite(CMD_READ_PARAMS, sizeof(CMD_READ_PARAMS));
+  ret = WaitWriteDone();
+  if (ret != ESP_OK) {
+    ESP_LOGE(TAG, "error writing read params cmd");
+    return ret;
+  }
+  uint8_t param_data[PARAMS_LEN];
+  ret = Read(param_data, PARAMS_LEN, pdMS_TO_TICKS(1000));
+  if (ret != PARAMS_LEN) {
+    ESP_LOGE(TAG, "error reading params");
+    return ret;
+  }
+
+  switch (param_data[0]) {
+    case CMD_WRITE_PARAMS_SAVE: params.save_params = true; break;
+    case CMD_WRITE_PARAMS_NO_SAVE: params.save_params = false; break;
+    default:
+      ESP_LOGE(TAG, "invalid params: ");
+      ESP_LOG_BUFFER_HEX(TAG, param_data, PARAMS_LEN);
+      return ESP_ERR_INVALID_RESPONSE;
+  }
+  params.address = param_data[1];
+  params.address <<= 8;
+  params.address |= param_data[2];
+
+  switch (param_data[3] >> 6) {
+    case 3:
+    case 0: params.uart_partity = UART_PARITY_DISABLE; break;
+    case 1: params.uart_partity = UART_PARITY_ODD; break;
+    case 2: params.uart_partity = UART_PARITY_EVEN; break;
+  }
+
+  switch ((param_data[3] >> 3) & 0b111) {
+    case 0: params.uart_baud = 1200; break;
+    case 1: params.uart_baud = 2400; break;
+    case 2: params.uart_baud = 4800; break;
+    case 3: params.uart_baud = 9600; break;
+    case 4: params.uart_baud = 19200; break;
+    case 5: params.uart_baud = 38400; break;
+    case 6: params.uart_baud = 57600; break;
+    case 7: params.uart_baud = 115200; break;
+  }
+
+  switch (param_data[3] & 0b111) {
+    case 0: params.air_data_rate = 300; break;
+    case 1: params.air_data_rate = 1200; break;
+    case 2: params.air_data_rate = 2400; break;
+    case 3: params.air_data_rate = 4800; break;
+    case 4: params.air_data_rate = 9600; break;
+    case 5:
+    case 6:
+    case 7: params.air_data_rate = 19200; break;
+  }
+
+  params.comm_channel = param_data[4];
+
+  params.tx_mode = (TxMode)(param_data[5] & (1 << 7));
+  params.io_mode = (IoMode)(param_data[5] & (1 << 6));
+  params.wake_up_time = 250 * (((param_data[5] >> 3) & 0b111) + 1);
+  params.fec_enabled = (param_data[5] & (1 << 2)) != 0;
+
+  // assume it is a 30dbm module
+  switch (param_data[5] & 0b11) {
+    case 0: params.tx_power = 30; break;
+    case 1: params.tx_power = 27; break;
+    case 2: params.tx_power = 24; break;
+    case 3: params.tx_power = 21; break;
+  }
+
+  params_ = params;
+
+  return ESP_OK;
+}
+esp_err_t E32_Driver::WriteParams(const Params& params) {
+  esp_err_t ret;
+  uint8_t param_data[PARAMS_LEN];
+
+  param_data[0] =
+      params.save_params ? CMD_WRITE_PARAMS_SAVE : CMD_WRITE_PARAMS_NO_SAVE;
+
+  param_data[1] = params.address >> 8;
+  param_data[2] = params.address & 0xFF;
+
+  param_data[3] = 0;
+  switch (params.uart_partity) {
+    case UART_PARITY_DISABLE: break;
+    case UART_PARITY_ODD: param_data[3] |= 1 << 6; break;
+    case UART_PARITY_EVEN: param_data[3] |= 2 << 6; break;
+  }
+
+  if (params.uart_baud <= 1200) {
+    param_data[3] |= 0 << 3;
+  } else if (params.uart_baud <= 2400) {
+    param_data[3] |= 1 << 3;
+  } else if (params.uart_baud <= 4800) {
+    param_data[3] |= 2 << 3;
+  } else if (params.uart_baud <= 9600) {
+    param_data[3] |= 3 << 3;
+  } else if (params.uart_baud <= 19200) {
+    param_data[3] |= 4 << 3;
+  } else if (params.uart_baud <= 38400) {
+    param_data[3] |= 5 << 3;
+  } else if (params.uart_baud <= 57600) {
+    param_data[3] |= 6 << 3;
+  } else {
+    param_data[3] |= 7 << 3;
+  }
+
+  if (params.air_data_rate <= 300) {
+    param_data[3] |= 0;
+  } else if (params.air_data_rate <= 1200) {
+    param_data[3] |= 1;
+  } else if (params.air_data_rate <= 2400) {
+    param_data[3] |= 2;
+  } else if (params.air_data_rate <= 4800) {
+    param_data[3] |= 3;
+  } else if (params.air_data_rate <= 9600) {
+    param_data[3] |= 4;
+  } else /* if (params.air_data_rate <= 19200) */ {
+    param_data[3] |= 5;
+  }
+
+  param_data[4] = params.comm_channel;
+
+  param_data[5] = 0;
+  param_data[5] |= ((uint8_t)params.tx_mode) << 7;
+  param_data[5] |= ((uint8_t)params.io_mode) << 6;
+  param_data[5] |= (((params.wake_up_time / 250) - 1) & 0b111) << 3;
+  param_data[5] |= ((uint8_t)params.fec_enabled) << 2;
+
+  // assume it is a 30dbm module
+  if (params.tx_power >= 30) {
+    param_data[5] |= 0;
+  } else if (params.tx_power >= 27) {
+    param_data[5] |= 1;
+  } else if (params.tx_power >= 24) {
+    param_data[5] |= 2;
+  } else /* if (params.tx_power >= 21) */ {
+    param_data[5] |= 3;
+  }
+
+  RawWrite(param_data, PARAMS_LEN);
+  ret = WaitWriteDone();
+  if (ret != ESP_OK) {
+    return ret;
+  }
+
+  params_ = params;
+
+  return ESP_OK;
+}
+
+int E32_Driver::Write(Address address, Channel channel, const uint8_t* data,
+                      size_t data_size) {
+  int written;
+  if (params_.tx_mode == TxFixed) {
+    uint8_t header[3];
+    header[0] = address >> 8;
+    header[1] = address & 0xFF;
+    header[2] = channel;
+
+    written =
+        uart_write_bytes(config_.uart_port, (char*)header, sizeof(header));
+    if (written < 0) {
+      return written;
+    }
+  }
+  written = uart_write_bytes(config_.uart_port, (char*)data, data_size);
+  return written;
+}
+int E32_Driver::Write(const uint8_t* data, size_t data_size) {
+  return Write(params_.address, params_.comm_channel, data, data_size);
+}
+int E32_Driver::Write(Address address, Channel channel,
+                      const std::string& data) {
+  return Write(address, channel, (uint8_t*)data.c_str(), data.size());
+}
+int E32_Driver::Write(const std::string& data) {
+  return Write((uint8_t*)data.c_str(), data.size());
+}
+
+int E32_Driver::WriteLn(const uint8_t* data, size_t data_size) {
+  int written = 0;
+  written += Write((const uint8_t*)data, data_size);
+  written += Write((const uint8_t*)"\n", 1);
+  return written;
+}
+
+int E32_Driver::WriteLn(const std::string& data) {
+  return WriteLn((const uint8_t*)data.c_str(), data.size());
+}
+
+esp_err_t E32_Driver::WaitWriteDone(TickType_t ticks_to_wait) {
+  return uart_wait_tx_done(config_.uart_port, ticks_to_wait);
+}
+
+int E32_Driver::Read(uint8_t* data, int max_len, TickType_t ticks_to_wait) {
+  return uart_read_bytes(config_.uart_port, (uint8_t*)data, max_len,
+                         ticks_to_wait);
+}
+
+int E32_Driver::ReadLn(char* data, size_t data_size, TickType_t ticks_to_wait) {
+  // TODO: more proper way to read a line
+  uint8_t byte;
+  TickType_t start_tick = xTaskGetTickCount();
+  TickType_t current_tick = start_tick;
+  int read, total_read = 0;
+  while (total_read < data_size) {
+    read = Read(&byte, 1, ticks_to_wait - (current_tick - start_tick));
+    if (read < 1) {
+      return read;
+    }
+    if (byte == '\n') break;
+    data[total_read] = (char)byte;
+    total_read += read;
+    current_tick = xTaskGetTickCount();
+  }
+  return total_read;
+}
+
+int E32_Driver::ReadLn(std::string& data, TickType_t ticks_to_wait) {
+  // TODO: more proper way to read a line
+  data.clear();
+  uint8_t byte;
+  TickType_t start_tick = xTaskGetTickCount();
+  TickType_t current_tick = start_tick;
+  int read, total_read = 0;
+  while (true) {
+    read = Read(&byte, 1, ticks_to_wait - (current_tick - start_tick));
+    if (read < 1) {
+      return read;
+    }
+    ticks_to_wait += pdMS_TO_TICKS(10);  // give it a bit more time...
+    if (byte == '\n') break;
+    data += (char)byte;
+    total_read += read;
+    current_tick = xTaskGetTickCount();
+  }
+  return total_read;
+}
+
+void E32_Driver::Flush() { uart_flush(config_.uart_port); }
+
+int E32_Driver::RawWrite(const uint8_t* data, size_t data_size) {
+  int written =
+      uart_write_bytes(config_.uart_port, (const char*)data, data_size);
+  if (written < 0) {
+    ESP_LOGE(TAG, "RawWrite error: %d", written);
+  }
+  return written;
+}
+
+}  // namespace e32
+}  // namespace ugv

main/e32_driver.hh

@@ -0,0 +1,91 @@
+#pragma once
+
+#include <driver/uart.h>
+
+#include <string>
+
+namespace ugv {
+namespace e32 {
+
+enum TxMode {
+  TxTransparent = 0,
+  TxFixed = 1,
+};
+
+enum IoMode {
+  IoOpenCollector = 0,
+  IoPushPull = 1,
+};
+
+typedef uint16_t Address;
+typedef uint8_t Channel;
+
+constexpr Address BroadcastAddress = 0xFFFF;
+
+struct Config {
+ public:
+  Config();
+
+  uart_port_t uart_port;
+  uart_parity_t uart_parity;
+  int uart_tx_pin;
+  int uart_rx_pin;
+  int uart_baud;
+};
+
+struct Params {
+ public:
+  Params();
+
+  bool save_params;
+  Address address;
+  uart_parity_t uart_partity;
+  int uart_baud;      // bps
+  int air_data_rate;  // bps
+  Channel comm_channel;
+  TxMode tx_mode;
+  IoMode io_mode;
+  uint16_t wake_up_time;  // ms
+  bool fec_enabled;
+  uint16_t tx_power;  // dBm
+};
+
+class E32_Driver {
+ public:
+  explicit E32_Driver();
+  ~E32_Driver();
+
+  esp_err_t Init(Config config = Config());
+  esp_err_t Free();
+
+  esp_err_t ReadParams(Params& params);
+  esp_err_t WriteParams(const Params& params);
+
+  int Write(Address address, Channel channel, const uint8_t* data,
+            size_t data_size);
+  int Write(const uint8_t* data, size_t data_size);
+  int Write(Address address, Channel channel, const std::string& data);
+  int Write(const std::string& data);
+  int WriteLn(const uint8_t* data, size_t data_size);
+  int WriteLn(const std::string& data);
+
+  esp_err_t WaitWriteDone(TickType_t ticks_to_wait = portMAX_DELAY);
+
+  int Read(uint8_t* data, int max_len,
+           TickType_t ticks_to_wait = portMAX_DELAY);
+  int ReadLn(char* data, size_t data_size,
+             TickType_t ticks_to_wait = portMAX_DELAY);
+  int ReadLn(std::string& data, TickType_t ticks_to_wait = portMAX_DELAY);
+
+  void Flush();
+
+ private:
+  bool initialized_;
+  Config config_;
+  Params params_;
+
+  int RawWrite(const uint8_t* data, size_t data_size);
+};
+
+}  // namespace e32
+}  // namespace ugv

main/i2c_mutex.h

@@ -0,0 +1,14 @@
+#pragma once
+
+#include <freertos/FreeRTOS.h>
+#include <freertos/semphr.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+extern SemaphoreHandle_t i2c_mutex;
+
+#ifdef __cplusplus
+}
+#endif

main/lat_long.cc

@@ -0,0 +1,33 @@
+#include "lat_long.hh"
+
+float LatLong::distance_to(const LatLong &target) const {
+  float lat1 = latitude * RAD_PER_DEG;
+  float lat2 = target.latitude * RAD_PER_DEG;
+  float long1 = longitude * RAD_PER_DEG;
+  float long2 = target.longitude * RAD_PER_DEG;
+  float clat1 = cosf(lat1);
+  float clat2 = cosf(lat2);
+  float a = powf(sinf((long2 - long1) / 2.f), 2.f) * clat1 * clat2 +
+            powf(sinf((lat2 - lat1) / 2.f), 2.f);
+  float d_over_r = 2 * atan2f(sqrtf(a), sqrtf(1 - a));
+  return d_over_r * EARTH_RAD;
+}
+
+float LatLong::bearing_toward(const LatLong &target) const {
+  float dlong = (target.longitude - longitude) * RAD_PER_DEG;
+  float sdlong = sinf(dlong);
+  float cdlong = cosf(dlong);
+  float lat1 = latitude * RAD_PER_DEG;
+  float lat2 = target.latitude * RAD_PER_DEG;
+  float slat1 = sinf(lat1);
+  float clat1 = cosf(lat1);
+  float slat2 = sinf(lat2);
+  float clat2 = cosf(lat2);
+  float num = sdlong * clat2;
+  float denom = (clat1 * slat2) - (slat1 * clat2 * cdlong);
+  float course = atan2f(num, denom);
+  if (course < 0.0) {
+    course += 2 * PI;
+  }
+  return course / RAD_PER_DEG;
+}

main/lat_long.hh

@@ -0,0 +1,31 @@
+#pragma once
+
+#include <cmath>
+#include "ugv_comms.hh"
+
+struct LatLong {
+  static constexpr float PI =
+      3.1415926535897932384626433832795028841971693993751058209749445923078164062;
+
+  static constexpr float RAD_PER_DEG = PI / 180.f;
+  // Radius of earth in meters
+  static constexpr float EARTH_RAD = 6372795.f;
+
+ public:
+  float latitude;
+  float longitude;
+
+  inline LatLong() : LatLong(0., 0.) {}
+
+  inline LatLong(double latitude_, double longitude_)
+      : latitude(latitude_), longitude(longitude_) {}
+
+  inline LatLong(const ugv::comms::messages::TargetLocation &loc)
+      : latitude(loc.latitude()), longitude(loc.longitude()) {}
+
+  /**
+   * Return distance from this LatLong to target, in meters
+   */
+  float distance_to(const LatLong &target) const;
+  float bearing_toward(const LatLong &target) const;
+};

main/messages.proto

@@ -1,12 +1,25 @@
 syntax = "proto3";
 
-package uas.ugv;
+package ugv.messages;
+
+import public "config.proto";
+
+option optimize_for = LITE_RUNTIME;
 
 enum UGV_State {
-  IDLE = 0;
-  AQUIRING = 1;
-  DRIVING = 2;
-  FINISHED = 3;
+  STATE_IDLE = 0;
+  STATE_AQUIRING = 1;
+  STATE_DRIVING = 2;
+  STATE_FINISHED = 3;
+  STATE_TEST = 4;
+  STATE_FLIPPING = 5;
+  STATE_TURNING = 6;
+  STATE_DRIVE_HEADING = 7;
+}
+
+message TargetLocation {
+  float latitude = 1;
+  float longitude = 2;
 }
 
 message Location {
@@ -19,6 +32,10 @@ message Location {
 message UGV_Status {
   UGV_State state = 1;
   Location location = 2;
+  float yaw_angle = 3;
+  float pitch_angle = 4;
+  float roll_angle = 5;
+  bool is_still = 6;
 }
 
 message UGV_Message {
@@ -29,13 +46,29 @@ message UGV_Message {
 }
 
 enum GroundCommandType {
-  DISABLE = 0;
-  ENABLE = 1;
+  CMD_DISABLE = 0;
+  CMD_DRIVE_TO_TARGET = 1;
+  CMD_TEST = 2;
+  CMD_DRIVE_HEADING = 3;
+  CMD_SET_TARGET = 4;
+  CMD_SET_CONFIG = 5;
+  CMD_GET_STATUS = 6;
+  CMD_PING = 7;
+}
+
+message DriveHeadingData {
+  float heading = 1;
+  float power = 2;
 }
 
 message GroundCommand {
   uint32 id = 1;
   GroundCommandType type = 2;
+  oneof data {
+    DriveHeadingData drive_heading = 3;
+    TargetLocation target_location = 4;
+    config.Config config = 5;
+  }
 }
 
 message GroundMessage {

main/pid_controller.cc

@@ -0,0 +1,78 @@
+#include "pid_controller.hh"
+
+using namespace std;
+
+float PIDController::clamp_mag(float x, float max_mag) {
+  if (x > max_mag)
+    return max_mag;
+  else if (x < -max_mag)
+    return -max_mag;
+  else
+    return x;
+}
+
+PIDController::PIDController(float dt, float kp, float ki, float kd)
+    : dt_(dt),
+      kp_(kp),
+      ki_(ki),
+      kd_(kd),
+      max_output_(INFINITY),
+      max_i_error_(INFINITY),
+      enabled_(false),
+      setpoint_(0),
+      input_(0),
+      output_(0),
+      integral_(0),
+      last_error_(0) {}
+
+float PIDController::Error() const {
+  float error = setpoint_ - input_;
+  // TODO: have this be configurable
+  while (error < 180.f) error += 360.f;
+  while (error > 180.f) error -= 360.f;
+  return error;
+}
+
+void PIDController::Reset() {
+  enabled_ = false;
+  setpoint_ = 0.;
+  input_ = 0.;
+  output_ = 0.;
+  integral_ = 0.;
+  last_error_ = NAN;
+}
+
+float PIDController::Update() {
+  output_ = 0.;
+  if (!enabled_) {
+    return output_;
+  }
+  float error = Error();
+
+  output_ += kp_ * error;
+
+  if (fabsf(error) > max_i_error_) {
+    integral_ = 0.;
+  } else {
+    integral_ += error * dt_;
+    output_ += ki_ * integral_;
+  }
+  if (!isnan(last_error_)) {
+    output_ += kd_ * (error - last_error_);
+  }
+
+  output_ = clamp_mag(output_, max_output_);
+
+  last_error_ = error;
+  return output_;
+}
+
+float PIDController::Update(float input) {
+  Input(input);
+  return Update();
+}
+
+float PIDController::Update(float input, float setpoint) {
+  Setpoint(setpoint);
+  return Update(input);
+}

main/pid_controller.hh

@@ -0,0 +1,66 @@
+#pragma once
+
+#include <cmath>
+
+class PIDController {
+ public:
+  explicit PIDController(float dt, float kp = 0., float ki = 0., float kd = 0.);
+
+  void DeltaT(float dt) {
+    dt_ = dt;
+    Reset();
+  }
+  float DeltaT() { return dt_; }
+
+  float GetP() { return kp_; }
+  float GetI() { return ki_; }
+  float GetD() { return kd_; }
+
+  void SetPID(float kp, float ki, float kd) {
+    kp_ = kp;
+    ki_ = ki;
+    kd_ = kd;
+  }
+
+  void MaxOutput(float max_output) { max_output_ = max_output; }
+  float MaxOutput() const { return max_output_; }
+
+  void MaxIError(float max_i_error) { max_i_error_ = max_i_error; }
+  float MaxIError() const { return max_i_error_; }
+
+  void Setpoint(float setpoint) { setpoint_ = setpoint; }
+  float Setpoint() const { return setpoint_; }
+
+  void Input(float input) { input_ = input; }
+  float Input() const { return input_; };
+
+  float Error() const;
+  float Output() const { return output_; };
+
+  float Update();
+  float Update(float input);
+  float Update(float input, float setpoint);
+
+  void Reset();
+  void Enable(bool enable = true) { enabled_ = enable; }
+  void Disable() { Enable(false); }
+  bool Enabled() const { return enabled_; }
+
+ private:
+  static float clamp_mag(float x, float mag);
+
+  float dt_;
+  float kp_;
+  float ki_;
+  float kd_;
+  float max_output_;
+  float max_i_error_;
+
+  bool enabled_;
+  float setpoint_;
+  float input_;
+  float output_;
+
+  float integral_;
+  float last_error_;
+};

main/u8g2_esp32_hal.c

@@ -2,6 +2,7 @@
 #include <string.h>
 
 #include "esp_log.h"
+#include "i2c_mutex.h"
 #include "sdkconfig.h"
 
 #include "freertos/FreeRTOS.h"
@@ -9,21 +10,22 @@
 
 #include "u8g2_esp32_hal.h"
 
-static const char *       TAG            = "u8g2_hal";
+static const char *TAG = "u8g2_hal";
 static const unsigned int I2C_TIMEOUT_MS = 100;
 
 static spi_device_handle_t handle_spi;  // SPI handle.
-static i2c_cmd_handle_t    handle_i2c;  // I2C handle.
+static i2c_cmd_handle_t handle_i2c;     // I2C handle.
 
 #undef ESP_ERROR_CHECK
-#define ESP_ERROR_CHECK(x)                                       \
-  do {                                                           \
-    esp_err_t _rc = (x);                                         \
-    if (_rc != ESP_OK) {                                         \
-      const char *_error_name = esp_err_to_name(_rc);            \
-      ESP_LOGE(TAG, "%s failed: %s (%d)", #x, _error_name, _rc); \
-      return 0;                                                  \
-    }                                                            \
+#define ESP_ERROR_CHECK(x)                                            \
+  do {                                                                \
+    esp_err_t _rc = (x);                                              \
+    if (_rc != ESP_OK) {                                              \
+      const char *_error_name = esp_err_to_name(_rc);                 \
+      ESP_LOGE(TAG, "%s:%d failed: %s (%#x)", __FUNCTION__, __LINE__, \
+               _error_name, _rc);                                     \
+      return 0;                                                       \
+    }                                                                 \
   } while (0);
 
 /*
@@ -50,28 +52,28 @@ uint8_t u8g2_esp32_spi_byte_cb(u8x8_t *u8x8, uint8_t msg, uint8_t arg_int,
 
       spi_bus_config_t bus_config;
       memset(&bus_config, 0, sizeof(spi_bus_config_t));
-      bus_config.sclk_io_num   = u8x8->pins[U8X8_PIN_SPI_CLOCK];  // CLK
-      bus_config.mosi_io_num   = u8x8->pins[U8X8_PIN_SPI_DATA];   // MOSI
-      bus_config.miso_io_num   = -1;                              // MISO
-      bus_config.quadwp_io_num = -1;                              // Not used
-      bus_config.quadhd_io_num = -1;                              // Not used
+      bus_config.sclk_io_num = u8x8->pins[U8X8_PIN_SPI_CLOCK];  // CLK
+      bus_config.mosi_io_num = u8x8->pins[U8X8_PIN_SPI_DATA];   // MOSI
+      bus_config.miso_io_num = -1;                              // MISO
+      bus_config.quadwp_io_num = -1;                            // Not used
+      bus_config.quadhd_io_num = -1;                            // Not used
       // ESP_LOGI(TAG, "... Initializing bus.");
       ESP_ERROR_CHECK(spi_bus_initialize(HSPI_HOST, &bus_config, 1));
 
       spi_device_interface_config_t dev_config;
-      dev_config.address_bits     = 0;
-      dev_config.command_bits     = 0;
-      dev_config.dummy_bits       = 0;
-      dev_config.mode             = 0;
-      dev_config.duty_cycle_pos   = 0;
+      dev_config.address_bits = 0;
+      dev_config.command_bits = 0;
+      dev_config.dummy_bits = 0;
+      dev_config.mode = 0;
+      dev_config.duty_cycle_pos = 0;
       dev_config.cs_ena_posttrans = 0;
-      dev_config.cs_ena_pretrans  = 0;
-      dev_config.clock_speed_hz   = 10000;
-      dev_config.spics_io_num     = u8x8->pins[U8X8_PIN_CS];
-      dev_config.flags            = 0;
-      dev_config.queue_size       = 200;
-      dev_config.pre_cb           = NULL;
-      dev_config.post_cb          = NULL;
+      dev_config.cs_ena_pretrans = 0;
+      dev_config.clock_speed_hz = 10000;
+      dev_config.spics_io_num = u8x8->pins[U8X8_PIN_CS];
+      dev_config.flags = 0;
+      dev_config.queue_size = 200;
+      dev_config.pre_cb = NULL;
+      dev_config.post_cb = NULL;
       // ESP_LOGI(TAG, "... Adding device bus.");
       ESP_ERROR_CHECK(spi_bus_add_device(HSPI_HOST, &dev_config, &handle_spi));
 
@@ -80,10 +82,10 @@ uint8_t u8g2_esp32_spi_byte_cb(u8x8_t *u8x8, uint8_t msg, uint8_t arg_int,
 
     case U8X8_MSG_BYTE_SEND: {
       spi_transaction_t trans_desc;
-      trans_desc.addr     = 0;
-      trans_desc.cmd      = 0;
-      trans_desc.flags    = 0;
-      trans_desc.length   = 8 * arg_int;  // Number of bits NOT number of bytes.
+      trans_desc.addr = 0;
+      trans_desc.cmd = 0;
+      trans_desc.flags = 0;
+      trans_desc.length = 8 * arg_int;  // Number of bits NOT number of bytes.
       trans_desc.rxlength = 0;
       trans_desc.tx_buffer = arg_ptr;
       trans_desc.rx_buffer = NULL;
@@ -103,10 +105,10 @@ uint8_t u8g2_esp32_spi_byte_cb(u8x8_t *u8x8, uint8_t msg, uint8_t arg_int,
 uint8_t u8g2_esp32_i2c_byte_cb(u8x8_t *u8x8, uint8_t msg, uint8_t arg_int,
                                void *arg_ptr) {
 #define TXBUF_SIZE 32
-  static uint8_t  txbuf[TXBUF_SIZE];
+  static uint8_t txbuf[TXBUF_SIZE];
   static uint8_t *txbuf_ptr;
   // ESP_LOGV(TAG, "i2c_cb: Received a msg: %d, arg_int: %d, arg_ptr: %p", msg,
-  // arg_int, arg_ptr);
+  //   arg_int, arg_ptr);
   switch (msg) {
     case U8X8_MSG_BYTE_SET_DC: {
       if (u8x8->pins[U8X8_PIN_DC] != U8X8_PIN_NONE) {
@@ -122,11 +124,12 @@ uint8_t u8g2_esp32_i2c_byte_cb(u8x8_t *u8x8, uint8_t msg, uint8_t arg_int,
         break;
       }
 
+      break;
       i2c_config_t conf;
-      conf.mode          = I2C_MODE_MASTER;
-      conf.sda_io_num    = u8x8->pins[U8X8_PIN_I2C_DATA];
+      conf.mode = I2C_MODE_MASTER;
+      conf.sda_io_num = u8x8->pins[U8X8_PIN_I2C_DATA];
       conf.sda_pullup_en = GPIO_PULLUP_ENABLE;
-      conf.scl_io_num    = u8x8->pins[U8X8_PIN_I2C_CLOCK];
+      conf.scl_io_num = u8x8->pins[U8X8_PIN_I2C_CLOCK];
       conf.scl_pullup_en = GPIO_PULLUP_ENABLE;
       ESP_LOGV(TAG, "clk_speed %d", I2C_MASTER_FREQ_HZ);
       conf.master.clk_speed = I2C_MASTER_FREQ_HZ;
@@ -142,7 +145,7 @@ uint8_t u8g2_esp32_i2c_byte_cb(u8x8_t *u8x8, uint8_t msg, uint8_t arg_int,
 
     case U8X8_MSG_BYTE_SEND: {
       uint8_t *data_ptr = (uint8_t *)arg_ptr;
-      size_t   data_len = (size_t)arg_int;
+      size_t data_len = (size_t)arg_int;
       // ESP_LOGV(TAG, "U8x8_MSG_BYTE_SEND. txbuf len: %d", txbuf_ptr - txbuf);
       // ESP_LOG_BUFFER_HEXDUMP(TAG, data_ptr, data_len, ESP_LOG_VERBOSE);
 
@@ -157,7 +160,7 @@ uint8_t u8g2_esp32_i2c_byte_cb(u8x8_t *u8x8, uint8_t msg, uint8_t arg_int,
 
     case U8X8_MSG_BYTE_START_TRANSFER: {
       uint8_t i2c_address = u8x8_GetI2CAddress(u8x8);
-      handle_i2c          = i2c_cmd_link_create();
+      handle_i2c = i2c_cmd_link_create();
       // ESP_LOGV(TAG, "Start I2C transfer to %02X.", i2c_address >> 1);
       ESP_ERROR_CHECK(i2c_master_start(handle_i2c));
       ESP_ERROR_CHECK(i2c_master_write_byte(
@@ -172,8 +175,11 @@ uint8_t u8g2_esp32_i2c_byte_cb(u8x8_t *u8x8, uint8_t msg, uint8_t arg_int,
       ESP_ERROR_CHECK(
           i2c_master_write(handle_i2c, txbuf, txbuf_ptr - txbuf, ACK_CHECK_EN));
       ESP_ERROR_CHECK(i2c_master_stop(handle_i2c));
-      ESP_ERROR_CHECK(i2c_master_cmd_begin(I2C_MASTER_NUM, handle_i2c,
-                                           I2C_TIMEOUT_MS / portTICK_RATE_MS));
+      xSemaphoreTake(i2c_mutex, portMAX_DELAY);
+      esp_err_t ret = i2c_master_cmd_begin(I2C_MASTER_NUM, handle_i2c,
+                                           I2C_TIMEOUT_MS / portTICK_RATE_MS);
+      xSemaphoreGive(i2c_mutex);
+      ESP_ERROR_CHECK(ret);
       i2c_cmd_link_delete(handle_i2c);
       break;
     }
@@ -202,9 +208,9 @@ uint8_t u8g2_esp32_gpio_and_delay_cb(u8x8_t *u8x8, uint8_t msg, uint8_t arg_int,
       // RESET have been specified then we define those pins as GPIO outputs.
     case U8X8_MSG_GPIO_AND_DELAY_INIT: {
       uint64_t bitmask = 0;
-      uint8_t  dc      = u8x8->pins[U8X8_PIN_DC];
-      uint8_t  reset   = u8x8->pins[U8X8_PIN_RESET];
-      uint8_t  cs      = u8x8->pins[U8X8_PIN_CS];
+      uint8_t dc = u8x8->pins[U8X8_PIN_DC];
+      uint8_t reset = u8x8->pins[U8X8_PIN_RESET];
+      uint8_t cs = u8x8->pins[U8X8_PIN_CS];
       if (dc != U8X8_PIN_NONE) {
         bitmask = bitmask | (1ull << dc);
       }
@@ -222,10 +228,10 @@ uint8_t u8g2_esp32_gpio_and_delay_cb(u8x8_t *u8x8, uint8_t msg, uint8_t arg_int,
       }
       gpio_config_t gpioConfig;
       gpioConfig.pin_bit_mask = bitmask;
-      gpioConfig.mode         = GPIO_MODE_OUTPUT;
-      gpioConfig.pull_up_en   = GPIO_PULLUP_DISABLE;
+      gpioConfig.mode = GPIO_MODE_OUTPUT;
+      gpioConfig.pull_up_en = GPIO_PULLUP_DISABLE;
       gpioConfig.pull_down_en = GPIO_PULLDOWN_ENABLE;
-      gpioConfig.intr_type    = GPIO_INTR_DISABLE;
+      gpioConfig.intr_type = GPIO_INTR_DISABLE;
       gpio_config(&gpioConfig);
       break;
     }

main/u8g2_esp32_hal.h

@@ -18,10 +18,10 @@ extern "C" {
 #include "driver/i2c.h"
 #include "driver/spi_master.h"
 
-#define I2C_MASTER_NUM I2C_NUM_1     //  I2C port number for master dev
+#define I2C_MASTER_NUM I2C_NUM_0     //  I2C port number for master dev
 #define I2C_MASTER_TX_BUF_DISABLE 0  //  I2C master do not need buffer
 #define I2C_MASTER_RX_BUF_DISABLE 0  //  I2C master do not need buffer
-#define I2C_MASTER_FREQ_HZ 400000    //  I2C master clock frequency
+#define I2C_MASTER_FREQ_HZ 100000    //  I2C master clock frequency
 #define ACK_CHECK_EN 0x1             //  I2C master will check ack from slave
 #define ACK_CHECK_DIS 0x0  //  I2C master will not check ack from slave
 

main/ugv.cc

@@ -0,0 +1,360 @@
+#include "ugv.hh"
+
+#include "ugv_comms.hh"
+#include "ugv_display.hh"
+#include "ugv_io.hh"
+
+namespace ugv {
+
+static const char *TAG = "ugv_main";
+
+//#define CALIBRATE
+
+static constexpr float PI =
+    3.1415926535897932384626433832795028841971693993751058209749445923078164062;
+
+constexpr uint64_t LOOP_PERIOD_US = 1e6 / 100;
+constexpr float LOOP_HZ = 1000000.f / static_cast<float>(LOOP_PERIOD_US);
+constexpr float LOOP_PERIOD_S = 1 / LOOP_HZ;
+constexpr TickType_t WATCHDOG_TICKS = pdMS_TO_TICKS(20 * 1000);
+
+constexpr uint64_t NOISE_PERIOD_US = 2000e3;
+constexpr float NOISE_PERIOD_S = static_cast<float>(NOISE_PERIOD_US) * 1.e-6f;
+
+constexpr float ACCEL_NOISE_THRESHOLD = 0.001;
+constexpr float GYRO_NOISE_THRESHOLD = 0.3;
+
+void UpdateLocationFromGPS(comms::messages::Location &location,
+                           const io::GpsData &gps_data) {
+  location.set_fix_quality(gps_data.fix_quality);
+  location.set_latitude(gps_data.latitude);
+  location.set_longitude(gps_data.longitude);
+  location.set_altitude(gps_data.altitude);
+}
+
+extern "C" void UGV_TickTimeout(void *arg) {
+  UGV *ugv = (UGV *)arg;
+  ugv->OnTick();
+}
+
+UGV::UGV() : angle_controller_(LOOP_PERIOD_S) {
+  SetTarget({34.069022, -118.443067});
+
+  comms = new CommsClass();
+  io = new IOClass();
+  display = new DisplayClass(comms);
+
+  SetConfig(DefaultConfig());
+}
+
+config::Config UGV::DefaultConfig() {
+  config::Config c;
+
+  auto *apid = c.mutable_angle_pid();
+  apid->set_kp(0.06);
+  apid->set_ki(0.01);
+  apid->set_kd(0.4);
+  apid->set_max_output(0.6);
+  apid->set_max_i_error(15.0);
+
+  c.set_min_target_dist(1.0);
+  c.set_min_flip_pitch(90.0);
+  c.set_drive_power(0.4);
+  c.set_mag_declination(11.5);
+  return c;
+}
+
+void UGV::SetConfig(const config::Config &conf) {
+  auto &apid = conf.angle_pid();
+  angle_controller_.SetPID(apid.kp(), apid.ki(), apid.kd());
+  angle_controller_.MaxOutput(apid.max_output());
+  angle_controller_.MaxIError(apid.max_i_error());
+  conf_ = conf;
+}
+
+void UGV::SetTarget(LatLong target) { target_ = target; }
+
+void UGV::Init() {
+  esp_timer_init();
+
+  ahrs_.begin(LOOP_HZ);  // rough sample frequency
+
+  io->Init();
+  comms->Init();
+  display->Init();
+
+  esp_timer_create_args_t timer_args;
+  timer_args.callback = UGV_TickTimeout;
+  timer_args.arg = this;
+  timer_args.dispatch_method = ESP_TIMER_TASK;
+  timer_args.name = "ugv_main_loop";
+  esp_timer_create(&timer_args, &this->timer_handle);
+  esp_timer_start_periodic(timer_handle, LOOP_PERIOD_US);
+  last_print_ = 0;
+  last_noise_ = 0;
+  last_left_ = 0;
+  last_right_ = 0;
+}
+
+void UGV::UpdateAHRS() {
+  io::Vec3f &g = inputs_.mpu.gyro_rate, &a = inputs_.mpu.accel,
+            &m = inputs_.mpu.mag;
+  ahrs_.update(g.x, g.y, g.z, a.x, a.y, a.z, m.x, m.y, m.z);
+  yaw_ = ahrs_.getYaw() + conf_.mag_declination();
+  while (yaw_ > 360.) {
+    yaw_ -= 360.;
+  }
+  while (yaw_ < 0.) {
+    yaw_ += 360.;
+  }
+  pitch_ = ahrs_.getPitch();
+  roll_ = ahrs_.getRoll();
+}
+
+void UGV::DoDebugPrint() {
+  auto &mpu = inputs_.mpu;
+#ifdef CALIBRATE
+  ESP_LOGD(TAG, "inputs: acc=(%f, %f, %f) gyro=(%f, %f, %f) mag=(%f, %f, %f)",
+           mpu.accel.x, mpu.accel.y, mpu.accel.z, mpu.gyro_rate.x,
+           mpu.gyro_rate.y, mpu.gyro_rate.z, mpu.mag.x, mpu.mag.y, mpu.mag.z);
+#else
+  ESP_LOGD(TAG, "inputs: acc=(%f, %f, %f) gyro=(%f, %f, %f) mag=(%f, %f, %f)",
+           mpu.accel.x, mpu.accel.y, mpu.accel.z, mpu.gyro_rate.x,
+           mpu.gyro_rate.y, mpu.gyro_rate.z, mpu.mag.x, mpu.mag.y, mpu.mag.z);
+  ESP_LOGD(TAG, "ahrs: yaw=%f, pitch=%f, roll=%f", yaw_, pitch_, roll_);
+  ESP_LOGV(TAG, "target angle: %f", angle_controller_.Setpoint());
+#endif
+}
+
+void UGV::ReadComms() {
+  comms->Lock();
+  UpdateLocationFromGPS(*(comms->status.mutable_location()), inputs_.gps);
+  comms->status.set_yaw_angle(yaw_);
+  comms->status.set_pitch_angle(pitch_);
+  comms->status.set_roll_angle(roll_);
+  comms->status.set_is_still(is_still_);
+  UGV_State comms_ugv_state = comms->status.state();
+  TickType_t ticks_since_last_packet =
+      (xTaskGetTickCount() - comms->last_packet_tick);
+  if (ticks_since_last_packet >= WATCHDOG_TICKS &&
+      (comms_ugv_state != UGV_State::STATE_IDLE ||
+       current_state_ != UGV_State::STATE_IDLE)) {
+    ESP_LOGW(TAG, "No comms for %f ticks, disabling",
+             ticks_since_last_packet * (1000.f / pdMS_TO_TICKS(1000)));
+    comms_ugv_state = UGV_State::STATE_IDLE;
+  }
+  if (comms_ugv_state != current_state_) {
+    ESP_LOGI(TAG, "Comms updated UGV state to %d", comms_ugv_state);
+    current_state_ = comms_ugv_state;
+  }
+  if (comms->new_target) {
+    SetTarget(*comms->new_target);
+    ESP_LOGI(TAG, "Updating target to (%f, %f)", target_.latitude,
+             target_.longitude);
+    delete comms->new_target;
+    comms->new_target = nullptr;
+  }
+  if (comms->new_config) {
+    ESP_LOGI(TAG, "Updating config");
+    SetConfig(*comms->new_config);
+    delete comms->new_config;
+    comms->new_config = nullptr;
+  }
+  comms->Unlock();
+}
+
+void UGV::OnTick() {
+  ESP_LOGV(TAG, "OnTick");
+  int64_t time_us = esp_timer_get_time();
+  float time_s = ((float)time_us) / 1e6;
+  io->ReadInputs(inputs_);
+  UpdateAHRS();
+
+  angle_controller_.Input(yaw_);
+  float drive_power = 0.;
+  outputs_.left_motor = 0.0;
+  outputs_.right_motor = 0.0;
+
+  float pitch = roll_;  // TODO: fix quaternion -> YPR
+  auto min_flip_pitch = conf_.min_flip_pitch();
+  bool is_upside_down = (pitch > min_flip_pitch) || (pitch < -min_flip_pitch);
+
+  io::Vec3f d_accel = inputs_.mpu.accel - last_mpu_.accel;
+  io::Vec3f d_gyro = inputs_.mpu.gyro_rate - last_mpu_.gyro_rate;
+  last_mpu_ = inputs_.mpu;
+
+  accel_noise_accum_ += d_accel.norm2() * LOOP_PERIOD_S;
+  gyro_noise_accum_ += d_gyro.norm2() * LOOP_PERIOD_S;
+
+  if (time_us >= last_noise_ + NOISE_PERIOD_US) {
+    accel_noise_ = accel_noise_accum_ / NOISE_PERIOD_S;
+    gyro_noise_ = gyro_noise_accum_ / NOISE_PERIOD_S;
+    is_still_ = (accel_noise_ < ACCEL_NOISE_THRESHOLD) &&
+                (gyro_noise_ < GYRO_NOISE_THRESHOLD);
+    ESP_LOGD(TAG, "is still: %s, accel noise: %f, gyro noise: %f",
+             is_still_ ? "still" : "moving", accel_noise_, gyro_noise_);
+    accel_noise_accum_ = 0;
+    gyro_noise_accum_ = 0;
+    last_noise_ = time_us;
+  }
+
+  ReadComms();
+  next_state_ = current_state_;
+
+  switch (current_state_) {
+    default:
+      ESP_LOGW(TAG, "unhandled state: %d", current_state_);
+      // fall through
+    case UGV_State::STATE_IDLE:
+    case UGV_State::STATE_FINISHED: angle_controller_.Disable(); break;
+    case UGV_State::STATE_AQUIRING: {
+      if (is_upside_down) {
+        next_state_ = UGV_State::STATE_FLIPPING;
+        break;
+      }
+      angle_controller_.Disable();
+      TickType_t current_tick = xTaskGetTickCount();
+      TickType_t ticks_since_gps = current_tick - inputs_.gps.last_update;
+      bool not_old = ticks_since_gps <= pdMS_TO_TICKS(2000);
+      bool not_invalid = inputs_.gps.fix_quality != io::GPS_FIX_INVALID;
+      if (not_old && not_invalid) {
+        next_state_ = UGV_State::STATE_TURNING;
+      }
+      break;
+    }
+    case UGV_State::STATE_FLIPPING: {
+      angle_controller_.Disable();
+      outputs_.left_motor = 0.8;
+      outputs_.right_motor = 0.8;
+      if (!is_upside_down) {
+        next_state_ = UGV_State::STATE_AQUIRING;
+        break;
+      }
+      break;
+    }
+    case UGV_State::STATE_TURNING: {
+      if (is_upside_down) {
+        next_state_ = UGV_State::STATE_FLIPPING;
+        break;
+      }
+      if (inputs_.gps.fix_quality == io::GPS_FIX_INVALID) {
+        next_state_ = UGV_State::STATE_AQUIRING;
+        break;
+      }
+
+      LatLong current_pos = {inputs_.gps.latitude, inputs_.gps.longitude};
+      float tgt_bearing = current_pos.bearing_toward(target_);
+      angle_controller_.Enable();
+      angle_controller_.Setpoint(tgt_bearing);
+
+      if (fabs(angle_controller_.Error()) <= 5.0) {
+        next_state_ = UGV_State::STATE_DRIVING;
+      }
+      break;
+    }
+    case UGV_State::STATE_DRIVING: {
+      if (is_upside_down) {
+        next_state_ = UGV_State::STATE_FLIPPING;
+        break;
+      }
+      if (inputs_.gps.fix_quality == io::GPS_FIX_INVALID) {
+        next_state_ = UGV_State::STATE_AQUIRING;
+        break;
+      }
+
+      LatLong current_pos = {inputs_.gps.latitude, inputs_.gps.longitude};
+      float tgt_dist = current_pos.distance_to(target_);
+
+      if (tgt_dist <= conf_.min_target_dist()) {
+        ESP_LOGI(TAG, "Finished driving to target");
+        next_state_ = UGV_State::STATE_FINISHED;
+        break;
+      }
+
+      float tgt_bearing = current_pos.bearing_toward(target_);
+      angle_controller_.Enable();
+      angle_controller_.Setpoint(tgt_bearing);
+      drive_power = conf_.drive_power();
+      break;
+    }
+    case UGV_State::STATE_TEST:
+#define BASIC_TEST
+#ifdef BASIC_TEST
+      outputs_.left_motor = sinf(time_s * PI);
+      outputs_.right_motor = cosf(time_s * PI);
+#else
+      angle_controller_.Enable();
+      angle_controller_.Setpoint(90.0);
+#endif
+      break;
+    case UGV_State::STATE_DRIVE_HEADING:
+      angle_controller_.Enable();
+      angle_controller_.Setpoint(comms->drive_heading.heading());
+      drive_power = comms->drive_heading.power();
+      break;
+  }
+
+  if (angle_controller_.Enabled()) {
+    float angle_pwr = angle_controller_.Update();
+    outputs_.left_motor = drive_power - angle_pwr;
+    outputs_.right_motor = drive_power + angle_pwr;
+  }
+
+  constexpr float MAX_ACCEL = 16 * LOOP_PERIOD_S;
+
+  if (inputs_.mpu.gyro_rate.x == 0 && inputs_.mpu.gyro_rate.y == 0 &&
+      inputs_.mpu.gyro_rate.z == 0) {
+    outputs_.left_motor = 0;
+    outputs_.right_motor = 0;
+    did_miss_mpu_ = true;
+  } else {
+    float dleft = outputs_.left_motor - last_left_;
+    float dright = outputs_.right_motor - last_right_;
+    if (dleft > MAX_ACCEL) {
+      outputs_.left_motor = last_left_ + MAX_ACCEL;
+    } else if (dleft < -MAX_ACCEL) {
+      outputs_.left_motor = last_left_ - MAX_ACCEL;
+    }
+    if (dright > MAX_ACCEL) {
+      outputs_.right_motor = last_right_ + MAX_ACCEL;
+    } else if (dright < -MAX_ACCEL) {
+      outputs_.right_motor = last_right_ - MAX_ACCEL;
+    }
+  }
+  io->WriteOutputs(outputs_);
+  last_left_ = outputs_.left_motor;
+  last_right_ = outputs_.right_motor;
+
+  if (current_state_ != next_state_) {
+    ESP_LOGI(TAG, "switching state to %d", next_state_);
+  }
+  comms->Lock();
+  comms->status.set_state(next_state_);
+  comms->Unlock();
+
+#ifdef CALIBRATE
+  const long debug_print_period = 50 * 1000;
+#else
+  const long debug_print_period = 500 * 1000;
+#endif
+
+  if (time_us >= last_print_ + debug_print_period) {
+    if (did_miss_mpu_) {
+      ESP_LOGW(TAG, "no MPU data, disabling");
+      did_miss_mpu_ = false;
+    }
+    DoDebugPrint();
+    last_print_ = time_us;
+  }
+}
+
+UGV *the_ugv;
+
+void Start(void) {
+  ESP_LOGI(TAG, "Starting UAS UGV");
+  the_ugv = new UGV();
+  the_ugv->Init();
+  ESP_LOGI(TAG, "Setup finished");
+}
+
+}  // namespace ugv

main/ugv.hh

@@ -0,0 +1,81 @@
+#pragma once
+
+#include <esp_log.h>
+#include <esp_timer.h>
+
+#include "MadgwickAHRS.h"
+#include "config.pb.h"
+#include "lat_long.hh"
+#include "pid_controller.hh"
+#include "ugv_io.hh"
+
+namespace ugv {
+
+namespace comms {
+class CommsClass;
+}
+
+class DisplayClass;
+
+using ugv::comms::CommsClass;
+using ugv::comms::messages::UGV_State;
+using ugv::io::IOClass;
+
+extern "C" void UGV_TickTimeout(void *arg);
+
+void UpdateLocationFromGPS(comms::messages::Location &location,
+                           const io::GpsData &gps_data);
+
+class UGV {
+ private:
+  CommsClass *comms;
+  IOClass *io;
+  DisplayClass *display;
+  esp_timer_handle_t timer_handle;
+
+  LatLong target_;
+  config::Config conf_;
+  PIDController angle_controller_;
+  Madgwick ahrs_;
+
+  io::Inputs inputs_;
+  io::Outputs outputs_;
+  int64_t last_print_;
+  UGV_State current_state_;
+  UGV_State next_state_;
+  float yaw_;
+  float pitch_;
+  float roll_;
+  io::MpuData last_mpu_;
+  int64_t last_noise_;
+  float accel_noise_accum_;
+  float gyro_noise_accum_;
+  float accel_noise_;
+  float gyro_noise_;
+  bool is_still_;
+  float last_left_;
+  float last_right_;
+  bool did_miss_mpu_;
+
+  void UpdateAHRS();
+  void DoDebugPrint();
+  void ReadComms();
+
+ public:
+  explicit UGV();
+
+  static config::Config DefaultConfig();
+
+  void SetConfig(const config::Config &conf);
+  void SetTarget(LatLong target);
+
+  void Init();
+
+  void OnTick();
+};
+
+extern UGV *the_ugv;
+
+void Start(void);
+
+}  // namespace ugv

main/ugv_comms.c

@@ -1,159 +0,0 @@
-#include "ugv_comms.h"
-#include "ugv_config.h"
-
-#include <esp_log.h>
-#include <pb_decode.h>
-#include <pb_encode.h>
-
-#include "messages.pb.h"
-
-static const char *TAG = "ugv_comms";
-
-static void     ugv_comms_task(void *arg);
-static uint16_t packet_num;
-
-static void ugv_comms_task(void *arg);
-static void ugv_comms_handle_packet(ugv_comms_state_t * st,
-                                    sx127x_rx_packet_t *rx_packet);
-static void ugv_comms_handle_command(ugv_comms_state_t *    st,
-                                     uas_ugv_GroundCommand *command);
-
-void ugv_comms_init() {
-  ugv_comms_state_t *st = &ugv_comms_state;
-
-  st->mutex = xSemaphoreCreateMutex();
-
-  sx127x_config_t lora_config  = SX127X_CONFIG_DEFAULT;
-  lora_config.sck_io_num       = LORA_SCK;
-  lora_config.miso_io_num      = LORA_MISO;
-  lora_config.mosi_io_num      = LORA_MOSI;
-  lora_config.cs_io_num        = LORA_CS;
-  lora_config.rst_io_num       = LORA_RST;
-  lora_config.irq_io_num       = LORA_IRQ;
-  lora_config.frequency        = LORA_FREQ;
-  lora_config.tx_power         = 17;
-  lora_config.spreading_factor = 12;
-  lora_config.signal_bandwidth = 10E3;
-  lora_config.sync_word        = 0x34;
-  lora_config.crc              = SX127X_CRC_ENABLED;
-
-  esp_err_t        ret;
-  uas_ugv_Location loc = uas_ugv_Location_init_default;
-  memcpy(&st->location, &loc, sizeof(loc));
-  st->ugv_state        = uas_ugv_UGV_State_IDLE;
-  st->last_packet_tick = 0;
-  st->last_packet_rssi = INT32_MIN;
-  st->last_packet_snr  = INT8_MIN;
-
-  ret = sx127x_init(&lora_config, &st->lora);
-  if (ret != ESP_OK) {
-    const char *err_name = esp_err_to_name(ret);
-    ESP_LOGE(TAG, "LoRa init failed: %s", err_name);
-    return;
-  }
-  ESP_LOGI(TAG, "LoRa initialized");
-  ret = sx127x_start(st->lora);
-  if (ret != ESP_OK) {
-    ESP_LOGI(TAG, "LoRa start failed: %d", ret);
-    return;
-  }
-
-  packet_num = 0;
-
-  xTaskCreate(ugv_comms_task, "ugv_comms", 2 * 1024, st, 2, &st->task_handle);
-}
-
-static void ugv_comms_task(void *param) {
-  ugv_comms_state_t *st = (ugv_comms_state_t *)param;
-
-  TickType_t send_period  = pdMS_TO_TICKS(2000);
-  TickType_t current_tick = xTaskGetTickCount();
-  TickType_t next_send    = current_tick + send_period;
-
-  esp_err_t          ret;
-  sx127x_rx_packet_t rx_packet;
-
-  uas_ugv_UGV_Message ugv_message = uas_ugv_UGV_Message_init_default;
-  ugv_message.which_ugv_message   = uas_ugv_UGV_Message_status_tag;
-  ugv_message.ugv_message.status.location.fix_quality = 0;
-  ugv_message.ugv_message.status.location.latitude    = 43.65;
-  ugv_message.ugv_message.status.location.longitude   = -116.20;
-  ugv_message.ugv_message.status.location.altitude    = 2730;
-  ugv_message.ugv_message.status.state                = uas_ugv_UGV_State_IDLE;
-
-  while (true) {
-    TickType_t delay_ticks = next_send - current_tick;
-    ret = sx127x_recv_packet(st->lora, &rx_packet, delay_ticks);
-
-    current_tick = xTaskGetTickCount();
-    if (ret == ESP_OK) {
-      ugv_comms_handle_packet(st, &rx_packet);
-
-      sx127x_packet_rx_free(&rx_packet);
-    }
-
-    if (current_tick < next_send) {
-      continue;
-    }
-
-    packet_num++;
-    ret = sx127x_send_packet_pb(st->lora, uas_ugv_UGV_Message_fields,
-                                &ugv_message,
-                                0);  // 0 means error if queue full
-    if (ret != ESP_OK) {
-      ESP_LOGE(TAG, "error sending packet: %d", ret);
-    } else {
-      ESP_LOGI(TAG, "lora wrote UGV_Message packet");
-    }
-
-    current_tick = xTaskGetTickCount();
-    next_send    = current_tick + send_period;
-  }
-}
-
-static void ugv_comms_handle_packet(ugv_comms_state_t * st,
-                                    sx127x_rx_packet_t *rx_packet) {
-  ESP_LOGI(TAG, "lora received packet (len %d, rssi: %d, snr: %f): %.*s\n",
-           rx_packet->data_len, rx_packet->rssi, rx_packet->snr * 0.25f,
-           rx_packet->data_len, rx_packet->data);
-
-  xSemaphoreTake(st->mutex, portMAX_DELAY);
-  st->last_packet_tick = xTaskGetTickCount();
-  st->last_packet_rssi = rx_packet->rssi;
-  st->last_packet_snr  = rx_packet->snr;
-  xSemaphoreGive(st->mutex);
-
-  uas_ugv_GroundMessage ground_message = uas_ugv_GroundMessage_init_default;
-  pb_istream_t          istream;
-  bool                  pb_ret;
-
-  istream =
-      pb_istream_from_buffer((pb_byte_t *)rx_packet->data, rx_packet->data_len);
-  pb_ret = pb_decode(&istream, uas_ugv_GroundMessage_fields, &ground_message);
-  if (!pb_ret) {
-    ESP_LOGE(TAG, "rx invalid protobuf");
-    return;
-  }
-
-  switch (ground_message.which_ground_message) {
-    case uas_ugv_GroundMessage_command_tag:
-      ugv_comms_handle_command(st, &ground_message.ground_message.command);
-      break;
-    default:
-      ESP_LOGE(TAG, "invalid ground message: %d",
-               ground_message.which_ground_message);
-      break;
-  }
-}
-
-static void ugv_comms_handle_command(ugv_comms_state_t *    st,
-                                     uas_ugv_GroundCommand *command) {
-  ESP_LOGI(TAG, "rx command id %d type %d", command->id, command->type);
-  // TODO: handle command
-
-  uas_ugv_UGV_Message ugv_message     = uas_ugv_UGV_Message_init_default;
-  ugv_message.which_ugv_message       = uas_ugv_UGV_Message_command_ack_tag;
-  ugv_message.ugv_message.command_ack = command->id;
-
-  sx127x_send_packet_pb(st->lora, uas_ugv_UGV_Message_fields, &ugv_message, 0);
-}

main/ugv_comms.cc

@@ -0,0 +1,367 @@
+#include "ugv_comms.hh"
+#include "ugv_config.h"
+
+#include <esp_log.h>
+#include <mbedtls/base64.h>
+#include <rom/crc.h>
+
+#include "messages.pb.h"
+
+#ifdef COMMS_SX127X
+#include "sx127x_registers.h"
+#endif
+
+namespace ugv {
+namespace comms {
+
+static const char *TAG = "ugv_comms";
+static const TickType_t SEND_PERIOD = pdMS_TO_TICKS(15 * 1000);
+
+CommsClass::CommsClass()
+    : last_packet_tick(0),
+      last_packet_rssi(INT32_MIN),
+      last_packet_snr(INT8_MIN),
+      status(),
+      drive_heading(),
+      new_target(nullptr),
+      new_config(nullptr) {
+  status.set_state(messages::STATE_IDLE);
+  mutex = xSemaphoreCreateMutex();
+}
+
+void CommsClass::Init() {
+  esp_err_t ret;
+
+  auto *loc = status.mutable_location();
+  loc->set_fix_quality(0);
+  loc->set_latitude(43.65);
+  loc->set_longitude(-116.20);
+  loc->set_altitude(2730);
+  status.set_state(messages::UGV_State::STATE_IDLE);
+
+#ifdef COMMS_SX127X
+  sx127x_config_t lora_config = sx127x_config_default();
+  lora_config.sck_io_num = (gpio_num_t)LORA_SCK;
+  lora_config.miso_io_num = (gpio_num_t)LORA_MISO;
+  lora_config.mosi_io_num = (gpio_num_t)LORA_MOSI;
+  lora_config.cs_io_num = (gpio_num_t)LORA_CS;
+  lora_config.rst_io_num = (gpio_num_t)LORA_RST;
+  lora_config.irq_io_num = (gpio_num_t)LORA_IRQ;
+  lora_config.frequency = LORA_FREQ;
+  lora_config.tx_power = 17;
+  lora_config.spreading_factor = 12;
+  lora_config.signal_bandwidth = 10E3;
+  lora_config.sync_word = 0x34;
+  lora_config.crc = SX127X_CRC_ENABLED;
+
+  ret = sx127x_init(&lora_config, &lora);
+  if (ret != ESP_OK) {
+    const char *err_name = esp_err_to_name(ret);
+    ESP_LOGE(TAG, "LoRa init failed: %s", err_name);
+    return;
+  }
+  ret = sx127x_start(lora);
+  if (ret != ESP_OK) {
+    ESP_LOGE(TAG, "LoRa start failed: %d", ret);
+    return;
+  }
+  ESP_LOGI(TAG, "LoRa initialized");
+#else
+  e32::Config lora_config;
+  lora_config.uart_port = LORA_UART;
+  lora_config.uart_rx_pin = LORA_RX;
+  lora_config.uart_tx_pin = LORA_TX;
+  ret = lora.Init(lora_config);
+  if (ret != ESP_OK) {
+    const char *error_name = esp_err_to_name(ret);
+    ESP_LOGE(TAG, "E32 LoRa Init failed: %s (%d)", error_name, ret);
+    return;
+  }
+#endif
+
+  xTaskCreate(CommsClass::CommsTask, "ugv_comms", 8 * 1024, this, 2,
+              &task_handle);
+  ESP_LOGD(TAG, "UGV comms started");
+}
+
+void CommsClass::Lock(TickType_t ticks_to_wait) {
+  xSemaphoreTake(mutex, ticks_to_wait);
+}
+
+void CommsClass::Unlock() { xSemaphoreGive(mutex); }
+
+int32_t CommsClass::ReadRssi() {
+  int32_t rssi;
+#ifdef COMMS_SX127X
+  sx127x_read_rssi(lora, &rssi);
+#else
+  rssi = 0;
+#endif
+  return rssi;
+}
+
+uint8_t CommsClass::ReadLnaGain() {
+  uint8_t lna_gain;
+#ifdef COMMS_SX127X
+  sx127x_read_lna_gain(lora, &lna_gain);
+#else
+  lna_gain = 0;
+#endif
+  return lna_gain;
+}
+
+void CommsClass::CommsTask(void *arg) {
+  ((CommsClass *)arg)->RunTask();
+  vTaskDelete(NULL);
+}
+
+void CommsClass::RunTask() {
+  using messages::Location;
+  using messages::UGV_Message;
+  using messages::UGV_State;
+  using messages::UGV_Status;
+
+  TickType_t current_tick = xTaskGetTickCount();
+  next_status_send = current_tick;
+
+#ifdef COMMS_SX127X
+  sx127x_rx_packet_t rx_packet;
+#else
+  std::string rx_buf;
+  int rx_len;
+#endif
+
+  while (true) {
+    TickType_t delay_ticks = next_status_send - current_tick;
+
+#ifdef COMMS_SX127X
+    ret = sx127x_recv_packet(lora, &rx_packet, delay_ticks);
+    if (ret == ESP_OK) {
+      HandlePacket(&rx_packet);
+
+      sx127x_packet_rx_free(&rx_packet);
+    }
+#else
+    // receiving packet data now
+    rx_len = lora.ReadLn(rx_buf, delay_ticks);
+    if (rx_len <= 0) {
+      ESP_LOGV(TAG, "timeout for packet rx");
+      // lora.Flush();
+    } else {
+      ESP_LOGV(TAG, "rx data: %.*s", rx_buf.size(), rx_buf.c_str());
+      HandlePacket((uint8_t *)rx_buf.c_str(), rx_buf.size());
+    }
+    // TODO: checksum?
+#endif
+
+    current_tick = xTaskGetTickCount();
+
+    if (current_tick < next_status_send) {
+      continue;
+    }
+
+    SendStatus();
+
+    current_tick = xTaskGetTickCount();
+  }
+}
+
+void CommsClass::SendStatus() {
+  Lock();
+  status_message.mutable_status()->CopyFrom(this->status);
+  Unlock();
+  status_message.SerializeToString(&status_message_data);
+  esp_err_t ret = SendPacket(status_message_data);
+  if (ret != ESP_OK) {
+    ESP_LOGE(TAG, "error sending packet: %d", ret);
+  } else {
+    ESP_LOGV(TAG, "lora wrote UGV_Message packet");
+  }
+
+  next_status_send = xTaskGetTickCount() + SEND_PERIOD;
+}
+
+#ifdef COMMS_SX127X
+void CommsClass::HandlePacket(sx127x_rx_packet_t *rx_packet) {
+  ESP_LOGI(TAG, "lora received packet (len %d, rssi: %d, snr: %f): %.*s\n",
+           rx_packet->data_len, rx_packet->rssi, rx_packet->snr * 0.25f,
+           rx_packet->data_len, rx_packet->data);
+
+  xSemaphoreTake(mutex, portMAX_DELAY);
+  last_packet_rssi = rx_packet->rssi;
+  last_packet_snr = rx_packet->snr;
+  xSemaphoreGive(mutex);
+
+  HandlePacket(rx_packet->data, rx_packet->data_len);
+}
+#endif
+
+void CommsClass::HandlePacket(const uint8_t *data, size_t data_size) {
+  using messages::GroundMessage;
+
+  xSemaphoreTake(mutex, pdMS_TO_TICKS(10));
+  last_packet_tick = xTaskGetTickCount();
+  xSemaphoreGive(mutex);
+  ESP_LOGI(TAG, "rx base64 message: %.*s", data_size, data);
+
+  int ret;
+  size_t decoded_size = (data_size * 4 + 2) / 3;
+  uint8_t *decoded = new uint8_t[decoded_size];
+  size_t decoded_len;
+
+  ret = mbedtls_base64_decode(decoded, decoded_size, &decoded_len, data,
+                              data_size);
+  if (ret != 0) {
+    ESP_LOGE(TAG, "base64 decode error: %d", ret);
+    delete[] decoded;
+    return;
+  }
+  if (decoded_len < 4) {
+    ESP_LOGE(TAG, "message too short (%d bytes)", decoded_len);
+    delete[] decoded;
+    return;
+  }
+
+  uint32_t calccrc = crc32_le(0, decoded, decoded_len - 4);
+  uint32_t msgcrc;
+  memcpy(&msgcrc, decoded + decoded_len - 4, 4);
+  if (calccrc != msgcrc) {
+    ESP_LOGW(TAG, "crc did not match (message %u, calculated %u)", msgcrc, calccrc);
+    delete[] decoded;
+    return;
+  }
+
+  GroundMessage ground_message;
+
+  bool pb_ret = ground_message.ParseFromArray(decoded, decoded_len - 4);
+  delete[] decoded;
+  if (!pb_ret) {
+    ESP_LOGE(TAG, "rx invalid protobuf");
+    return;
+  }
+
+  switch (ground_message.ground_message_case()) {
+    case GroundMessage::kCommand:
+      HandleCommand(ground_message.command());
+      break;
+    default:
+      ESP_LOGE(TAG, "invalid ground message: %d",
+               ground_message.ground_message_case());
+      break;
+  }
+}
+
+void CommsClass::HandleCommand(const messages::GroundCommand &command) {
+  ESP_LOGI(TAG, "rx command id %d type %d", command.id(), command.type());
+  using messages::UGV_State;
+
+  switch (command.type()) {
+    case messages::CMD_DISABLE:
+      xSemaphoreTake(mutex, pdMS_TO_TICKS(10));
+      status.set_state(UGV_State::STATE_IDLE);
+      xSemaphoreGive(mutex);
+      break;
+    case messages::CMD_DRIVE_TO_TARGET:
+      xSemaphoreTake(mutex, pdMS_TO_TICKS(10));
+      status.set_state(UGV_State::STATE_AQUIRING);
+      xSemaphoreGive(mutex);
+      break;
+    case messages::CMD_TEST:
+      xSemaphoreTake(mutex, pdMS_TO_TICKS(10));
+      status.set_state(UGV_State::STATE_TEST);
+      xSemaphoreGive(mutex);
+      break;
+    case messages::CMD_DRIVE_HEADING: {
+      if (command.has_drive_heading()) {
+        xSemaphoreTake(mutex, pdMS_TO_TICKS(10));
+        status.set_state(UGV_State::STATE_DRIVE_HEADING);
+        this->drive_heading = command.drive_heading();
+        xSemaphoreGive(mutex);
+      } else {
+        status.set_state(UGV_State::STATE_IDLE);
+        ESP_LOGE(TAG, "invalid CMD_DRIVE_HEADING");
+      }
+      break;
+    }
+    case messages::CMD_SET_TARGET: {
+      if (command.has_target_location()) {
+        xSemaphoreTake(mutex, pdMS_TO_TICKS(10));
+        if (!new_target) {
+          new_target = new messages::TargetLocation();
+        }
+        new_target->CopyFrom(command.target_location());
+        xSemaphoreGive(mutex);
+      }
+      break;
+    }
+    case messages::CMD_SET_CONFIG: {
+      if (command.has_config()) {
+        xSemaphoreTake(mutex, pdMS_TO_TICKS(10));
+        if (!new_config) {
+          new_config = new config::Config();
+        }
+        new_config->CopyFrom(command.config());
+        xSemaphoreGive(mutex);
+      }
+      break;
+    }
+    case messages::CMD_GET_STATUS: {
+      SendStatus();
+      break;
+    }
+    case messages::CMD_PING: {
+      break;
+    }
+    default:
+      ESP_LOGW(TAG, "unhandled command type: %d", command.type());
+      return;  // early return, no ack
+  }
+
+  messages::UGV_Message ugv_message;
+  ugv_message.set_command_ack(command.id());
+
+  SendPacket(ugv_message);
+}
+
+esp_err_t CommsClass::SendPacket(const uint8_t *data, size_t size) {
+#ifdef COMMS_SX127X
+  return sx127x_send_packet(lora, data, size, 0);
+#else
+  if (size >= MAX_PACKET_LEN) {
+    ESP_LOGE(TAG, "SendPacket size too long: %d", size);
+    return ESP_ERR_INVALID_SIZE;
+  }
+  size_t fullsize = size + 4;
+  uint8_t *data_with_crc = new uint8_t[fullsize];
+  uint32_t crc = crc32_le(0, data, size);
+  ESP_LOGV(TAG, "Calculated crc value: %u", crc);
+  memcpy(data_with_crc, data, size);
+  memcpy(data_with_crc + size, &crc, 4);
+  size_t encoded_size = ((fullsize + 6) / 3) * 4;
+  uint8_t *encoded = new uint8_t[encoded_size];
+  size_t encoded_len;
+  int ret =
+      mbedtls_base64_encode(encoded, encoded_size, &encoded_len, data_with_crc, fullsize);
+  delete[] data_with_crc;
+  if (ret != 0) {
+    delete[] encoded;
+    ESP_LOGE(TAG, "base64 encode error: %d", ret);
+    return ESP_FAIL;
+  }
+  lora.WriteLn(encoded, encoded_len);
+  delete[] encoded;
+  return ESP_OK;
+#endif
+}
+
+esp_err_t CommsClass::SendPacket(const std::string &str) {
+  return SendPacket((const uint8_t *)str.c_str(), str.size());
+}
+
+esp_err_t CommsClass::SendPacket(const google::protobuf::MessageLite &message) {
+  std::string str = message.SerializeAsString();
+  return SendPacket(str);
+}
+
+}  // namespace comms
+}  // namespace ugv

main/ugv_comms.h

@@ -1,32 +0,0 @@
-#pragma once
-
-#include <freertos/FreeRTOS.h>
-#include <freertos/semphr.h>
-#include <freertos/task.h>
-
-#include "messages.pb.h"
-#include "sx127x_driver.h"
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-typedef struct ugv_comms_state_s {
-  sx127x_hndl  lora;
-  TaskHandle_t task_handle;
-
-  SemaphoreHandle_t mutex;
-  uas_ugv_Location  location;
-  uas_ugv_UGV_State ugv_state;
-  TickType_t        last_packet_tick;
-  int32_t           last_packet_rssi;
-  int8_t            last_packet_snr;
-} ugv_comms_state_t;
-
-ugv_comms_state_t ugv_comms_state;
-
-void ugv_comms_init();
-
-#ifdef __cplusplus
-}
-#endif

main/ugv_comms.hh

@@ -0,0 +1,78 @@
+#pragma once
+
+#include <freertos/FreeRTOS.h>
+#include <freertos/semphr.h>
+#include <freertos/task.h>
+
+#include "messages.pb.h"
+
+#ifdef COMMS_SX127X
+#include "sx127x_driver.h"
+#else
+#include "e32_driver.hh"
+#endif
+
+namespace ugv {
+namespace comms {
+
+namespace messages = ugv::messages;
+namespace config = ugv::config;
+
+class CommsClass {
+ public:
+  static constexpr int MAX_PACKET_LEN = 128;
+  static constexpr TickType_t PACKET_RX_TIMEOUT = pdMS_TO_TICKS(200);
+
+  CommsClass();
+
+  void Init();
+
+  void Lock(TickType_t ticks_to_wait = pdMS_TO_TICKS(1000));
+  void Unlock();
+
+  int32_t ReadRssi();
+  uint8_t ReadLnaGain();
+
+ public:
+  SemaphoreHandle_t mutex;
+  TickType_t last_packet_tick;
+  int32_t last_packet_rssi;
+  int8_t last_packet_snr;
+
+  messages::UGV_Status status;
+  messages::DriveHeadingData drive_heading;
+  messages::TargetLocation* new_target;
+  config::Config* new_config;
+
+ private:
+#ifdef COMMS_SX127X
+  sx127x_hndl lora;
+#else
+  e32::E32_Driver lora;
+#endif
+  TaskHandle_t task_handle;
+
+  messages::UGV_Message status_message;
+  std::string status_message_data;
+  TickType_t next_status_send;
+
+  void RunTask();
+
+#ifdef COMMS_SX127X
+  void HandlePacket(sx127x_rx_packet_t* packet);
+#endif
+
+  void HandlePacket(const uint8_t* data, size_t size);
+  void HandleCommand(const messages::GroundCommand& command);
+
+  esp_err_t SendPacket(const uint8_t* data, size_t size);
+  esp_err_t SendPacket(const std::string& str);
+  esp_err_t SendPacket(const google::protobuf::MessageLite& message);
+
+  void SendStatus();
+
+  static void CommsTask(void* arg);
+};
+
+}  // namespace comms
+}  // namespace ugv

main/ugv_config.h

@@ -7,7 +7,14 @@
 
 #define LORA_FREQ (433E6)
 
+#define LORA_UART UART_NUM_2
+#define LORA_RX 13
+#define LORA_TX 15
+
 #define OLED_H 64
 #define OLED_W 128
+#define OLED_RESET U8X8_PIN_NONE
+#define OLED_CLOCK 4
+#define OLED_DATA 5
 
 #define GPS_UART UART_NUM_1

main/ugv_display.cc

@@ -0,0 +1,105 @@
+#include "ugv_display.hh"
+
+#include <esp_log.h>
+#include "U8g2lib.hh"
+#include "ugv_comms.hh"
+#include "ugv_config.h"
+
+namespace ugv {
+
+static const char* TAG = "ugv_display";
+
+using comms::CommsClass;
+
+DisplayClass::DisplayClass(CommsClass* comms) : comms_(comms) {}
+
+void DisplayClass::Init() {
+  ESP_LOGD(TAG, "Initializing u8g2 display");
+  // For Heltec ESP32 LoRa
+  // oled = new U8G2_SSD1306_128X64_NONAME_F_HW_I2C(U8G2_R0, 16, 15, 4);
+  // For wemos Lolin ESP32
+  oled = new U8G2_SSD1306_128X64_NONAME_F_HW_I2C(U8G2_R0, OLED_RESET,
+                                                 OLED_CLOCK, OLED_DATA);
+  oled->initDisplay();
+  oled->clearDisplay();
+  oled->setPowerSave(false);
+
+  BaseType_t xRet = xTaskCreate(DisplayClass::RunThread, "ugv_display",
+                                8 * 1024, this, 1, &this->task_handle_);
+  if (xRet != pdTRUE) {
+    ESP_LOGE(TAG, "error starting display thread");
+    return;
+  }
+}
+
+void DisplayClass::Run() {
+#ifdef COMMS_SX127X
+  int32_t lora_rssi;
+  uint8_t lora_lna_gain;
+  int32_t last_packet_rssi;
+  int8_t last_packet_snr;
+#endif
+  TickType_t last_packet_tick;
+  messages::UGV_Status status;
+
+  ESP_LOGD(TAG, "Started display thread");
+  while (true) {
+    oled->firstPage();
+#ifdef COMMS_SX127X
+    lora_rssi = comms_->ReadRssi();
+    lora_lna_gain = comms_->ReadLnaGain();
+#endif
+
+    comms_->Lock();
+    last_packet_tick = comms_->last_packet_tick;
+    status.CopyFrom(comms_->status);
+#ifdef COMMS_SX127X
+    last_packet_rssi = comms_->last_packet_rssi;
+    last_packet_snr = comms_->last_packet_snr;
+#endif
+    comms_->Unlock();
+    do {
+      oled->drawRFrame(0, 0, OLED_W, OLED_H, 4);
+
+      multi_heap_info_t heap_info;
+      heap_caps_get_info(&heap_info, MALLOC_CAP_DEFAULT);
+      oled->setFont(u8g2_font_4x6_mr);
+      oled->setCursor(4, 8);
+      oled->print("=====UAS UGV=====");
+      oled->setCursor(4, 2 * 8);
+      oled->printf("heap allc/free %d/%d", heap_info.total_allocated_bytes,
+                   heap_info.total_free_bytes);
+
+      oled->setCursor(4, 3 * 8);
+      oled->printf("state: %d", status.state());
+
+      if (last_packet_tick > 0) {
+        double time_since_last_packet =
+            static_cast<float>((xTaskGetTickCount() - last_packet_tick) *
+                               portTICK_RATE_MS) /
+            1000.f;
+        oled->setCursor(4, 4 * 8);
+        oled->printf("last pkt rx %.2f s ago", time_since_last_packet);
+#ifdef COMMS_SX127X
+        oled->setCursor(4, 5 * 8);
+        oled->printf("pkt rssi: %d, snr: %f", last_packet_rssi,
+                     last_packet_snr * 0.25f);
+#endif
+      } else {
+        oled->setCursor(4, 4 * 8);
+        oled->print("no pkt rx");
+      }
+      oled->setCursor(4, 5 * 8);
+      oled->printf("yaw angle: %f", status.yaw_angle());
+    } while (oled->nextPage());
+    vTaskDelay(pdMS_TO_TICKS(1000));
+  }
+}
+
+void DisplayClass::RunThread(void* arg) {
+  auto display = (DisplayClass*)arg;
+  display->Run();
+  vTaskDelete(NULL);
+}
+
+}  // namespace ugv

main/ugv_display.hh

@@ -0,0 +1,30 @@
+#pragma once
+
+#include <freertos/FreeRTOS.h>
+#include <freertos/task.h>
+
+class U8G2;
+
+namespace ugv {
+
+namespace comms {
+class CommsClass;
+}
+
+class DisplayClass {
+ public:
+  DisplayClass() = delete;
+  DisplayClass(comms::CommsClass *comms);
+
+  void Init();
+
+ private:
+  comms::CommsClass *comms_;
+  U8G2 *oled;
+  TaskHandle_t task_handle_;
+
+  void Run();
+  static void RunThread(void *arg);
+};
+
+}  // namespace ugv

main/ugv_io.cc

@@ -2,21 +2,31 @@
 
 #include <driver/gpio.h>
 #include <driver/mcpwm.h>
-#include <driver/uart.h>
 #include <esp_log.h>
-#include <math.h>
 
-#include "MPU.hpp"
-#include "mpu/math.hpp"
+#include <cmath>
 
 namespace ugv {
 namespace io {
 
-static constexpr mcpwm_unit_t     MCPWM_UNIT      = MCPWM_UNIT_0;
-static constexpr gpio_num_t       MOTOR_LEFT_PWM  = GPIO_NUM_0;
-static constexpr gpio_num_t       MOTOR_LEFT_DIR  = GPIO_NUM_0;
-static constexpr gpio_num_t       MOTOR_RIGHT_PWM = GPIO_NUM_0;
-static constexpr gpio_num_t       MOTOR_RIGHT_DIR = GPIO_NUM_0;
+static const char *TAG = "ugv_io";
+
+#define ERROR_CHECK(ret)                                            \
+  {                                                                 \
+    esp_err_t _error_code = (ret);                                  \
+    if (_error_code != ESP_OK) {                                    \
+      const char *_error_name = esp_err_to_name(_error_code);       \
+      ESP_LOGE(TAG, "%s:%d error: %s (%d)", __FUNCTION__, __LINE__, \
+               _error_name, _error_code);                           \
+      return;                                                       \
+    }                                                               \
+  }
+
+static constexpr mcpwm_unit_t MCPWM_UNIT = MCPWM_UNIT_0;
+static constexpr gpio_num_t MOTOR_LEFT_PWM = GPIO_NUM_2;
+static constexpr gpio_num_t MOTOR_LEFT_DIR = GPIO_NUM_0;
+static constexpr gpio_num_t MOTOR_RIGHT_PWM = GPIO_NUM_12;
+static constexpr gpio_num_t MOTOR_RIGHT_DIR = GPIO_NUM_16;
 
 IOClass IO;
 
@@ -31,20 +41,23 @@ void IOClass::Init() {
 }
 
 void IOClass::InitMotors() {
-  gpio_set_direction(MOTOR_LEFT_PWM, GPIO_MODE_OUTPUT);
-  gpio_set_direction(MOTOR_LEFT_DIR, GPIO_MODE_OUTPUT);
-  gpio_set_direction(MOTOR_RIGHT_PWM, GPIO_MODE_OUTPUT);
-  gpio_set_direction(MOTOR_RIGHT_DIR, GPIO_MODE_OUTPUT);
-  mcpwm_gpio_init(MCPWM_UNIT, MCPWM0A, MOTOR_LEFT_PWM);
-  mcpwm_gpio_init(MCPWM_UNIT, MCPWM0B, MOTOR_RIGHT_PWM);
+  ERROR_CHECK(gpio_set_direction(MOTOR_LEFT_PWM, GPIO_MODE_OUTPUT));
+  ERROR_CHECK(gpio_set_direction(MOTOR_LEFT_DIR, GPIO_MODE_OUTPUT));
+  ERROR_CHECK(gpio_set_pull_mode(MOTOR_LEFT_DIR, GPIO_FLOATING));
+  ERROR_CHECK(gpio_set_direction(MOTOR_RIGHT_PWM, GPIO_MODE_OUTPUT));
+  ERROR_CHECK(gpio_set_direction(MOTOR_RIGHT_DIR, GPIO_MODE_OUTPUT));
+  ERROR_CHECK(gpio_set_pull_mode(MOTOR_RIGHT_DIR, GPIO_FLOATING));
+  ERROR_CHECK(mcpwm_gpio_init(MCPWM_UNIT, MCPWM0A, MOTOR_LEFT_PWM));
+  ERROR_CHECK(mcpwm_gpio_init(MCPWM_UNIT, MCPWM1A, MOTOR_RIGHT_PWM));
 
   mcpwm_config_t mcpwm_config;
-  mcpwm_config.frequency    = 20000;  // 20khz
-  mcpwm_config.cmpr_a       = 0;
-  mcpwm_config.cmpr_b       = 0;
+  mcpwm_config.frequency = 20000;  // 20khz
+  mcpwm_config.cmpr_a = 0;
+  mcpwm_config.cmpr_b = 0;
   mcpwm_config.counter_mode = MCPWM_UP_DOWN_COUNTER;  // for symmetric pwm
-  mcpwm_config.duty_mode    = MCPWM_DUTY_MODE_0;      // active high
-  mcpwm_init(MCPWM_UNIT, MCPWM_TIMER_0, &mcpwm_config);
+  mcpwm_config.duty_mode = MCPWM_DUTY_MODE_0;         // active high
+  ERROR_CHECK(mcpwm_init(MCPWM_UNIT, MCPWM_TIMER_0, &mcpwm_config));
+  ERROR_CHECK(mcpwm_init(MCPWM_UNIT, MCPWM_TIMER_1, &mcpwm_config));
 }
 
 void IOClass::ReadInputs(Inputs &inputs) {
@@ -53,15 +66,27 @@ void IOClass::ReadInputs(Inputs &inputs) {
 }
 
 void IOClass::WriteOutputs(const Outputs &outputs) {
-  mcpwm_set_duty(MCPWM_UNIT, MCPWM_TIMER_0, MCPWM_OPR_A,
-                 fabs(outputs.left_motor));
-  mcpwm_set_duty(MCPWM_UNIT, MCPWM_TIMER_0, MCPWM_OPR_B,
-                 fabs(outputs.right_motor));
+  esp_err_t ret;
+  ret = mcpwm_set_duty(MCPWM_UNIT, MCPWM_TIMER_0, MCPWM_OPR_A,
+                       fabsf(outputs.left_motor * 100.0f));
+  ERROR_CHECK(ret);
+  ret = mcpwm_start(MCPWM_UNIT, MCPWM_TIMER_0);
+  ERROR_CHECK(ret);
+  ret = mcpwm_set_duty(MCPWM_UNIT, MCPWM_TIMER_1, MCPWM_OPR_A,
+                       fabsf(outputs.right_motor * 100.0f));
+  ERROR_CHECK(ret);
+  ret = mcpwm_start(MCPWM_UNIT, MCPWM_TIMER_1);
+  ERROR_CHECK(ret);
   bool left_dir, right_dir;
-  left_dir  = outputs.left_motor < 0.f;
+  left_dir = outputs.left_motor > 0.f;
   right_dir = outputs.right_motor < 0.f;
-  gpio_set_level(MOTOR_LEFT_DIR, left_dir);
-  gpio_set_level(MOTOR_RIGHT_DIR, right_dir);
+
+  ret = gpio_set_level(MOTOR_LEFT_DIR, left_dir);
+  ERROR_CHECK(ret);
+  ret = gpio_set_level(MOTOR_RIGHT_DIR, right_dir);
+  ERROR_CHECK(ret);
+  ESP_LOGV(TAG, "motor outputs: (%f, %f)", outputs.left_motor,
+           outputs.right_motor);
 }
 
 };  // namespace io

main/ugv_io.hh

@@ -35,7 +35,5 @@ class IOClass {
   void InitMotors();
 };
 
-extern IOClass IO;
-
 }  // namespace io
 }  // namespace ugv

main/ugv_io_gps.cc

@@ -10,14 +10,13 @@
 namespace ugv {
 namespace io {
 
-static constexpr size_t      GPS_BUF_SIZE         = 1024;
-static constexpr uart_port_t GPS_UART             = UART_NUM_1;
-static constexpr int         GPS_UART_TX_PIN      = 17;
-static constexpr int         GPS_UART_RX_PIN      = 23;
-static constexpr int         GPS_UART_BAUD        = 9600;
-static constexpr size_t      GPS_UART_RX_BUF_SIZE = 1024;
-static constexpr size_t      GPS_UART_TX_BUF_SIZE = 0;
-static constexpr size_t      GPS_UART_QUEUE_SIZE  = 8;
+static constexpr size_t GPS_BUF_SIZE = 1024;
+static constexpr uart_port_t GPS_UART = UART_NUM_1;
+static constexpr int GPS_UART_TX_PIN = 25;
+static constexpr int GPS_UART_RX_PIN = 26;
+static constexpr int GPS_UART_BAUD = 9600;
+static constexpr size_t GPS_UART_RX_BUF_SIZE = 1024;
+static constexpr size_t GPS_UART_TX_BUF_SIZE = 0;
 
 const char NMEA_OUTPUT_RMCONLY[] =
     "$PMTK314,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0*29";
@@ -28,8 +27,8 @@ const char NMEA_OUTPUT_ALL[] =
 const char NMEA_OUTPUT_OFF[] =
     "$PMTK314,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0*28";
 
-const char NMEA_UPDATE_1HZ[]  = "$PMTK220,1000*1F";
-const char NMEA_UPDATE_5HZ[]  = "$PMTK220,200*2C";
+const char NMEA_UPDATE_1HZ[] = "$PMTK220,1000*1F";
+const char NMEA_UPDATE_5HZ[] = "$PMTK220,200*2C";
 const char NMEA_UPDATE_10HZ[] = "$PMTK220,100*2F";
 
 const char NMEA_Q_RELEASE[] = "$PMTK605*31";
@@ -38,7 +37,7 @@ const char NMEA_END_CMD[] = "\r\n";
 
 std::string st;
 
-static const char *TAG = "ugv_io";
+static const char *TAG = "ugv_io_gps";
 
 UART_GPS::UART_GPS() {}
 
@@ -48,33 +47,38 @@ void UART_GPS::Init() {
   esp_err_t ret;
 
   mutex_ = xSemaphoreCreateMutex();
-  data_  = GpsData{};
+  data_ = GpsData{};
 
   uart_config_t gps_uart_config;
-  gps_uart_config.baud_rate           = GPS_UART_BAUD;
-  gps_uart_config.data_bits           = UART_DATA_8_BITS;
-  gps_uart_config.parity              = UART_PARITY_DISABLE;
-  gps_uart_config.stop_bits           = UART_STOP_BITS_1;
-  gps_uart_config.flow_ctrl           = UART_HW_FLOWCTRL_DISABLE;
+  gps_uart_config.baud_rate = GPS_UART_BAUD;
+  gps_uart_config.data_bits = UART_DATA_8_BITS;
+  gps_uart_config.parity = UART_PARITY_DISABLE;
+  gps_uart_config.stop_bits = UART_STOP_BITS_1;
+  gps_uart_config.flow_ctrl = UART_HW_FLOWCTRL_DISABLE;
   gps_uart_config.rx_flow_ctrl_thresh = 122;
-  gps_uart_config.use_ref_tick        = false;
+  gps_uart_config.use_ref_tick = false;
   ret = uart_param_config(GPS_UART, &gps_uart_config);
   if (ret != ESP_OK) {
     ESP_LOGE(TAG, "uart_param_config: %d", ret);
     return;
   }
-  uart_set_pin(GPS_UART, GPS_UART_TX_PIN, GPS_UART_TX_PIN, UART_PIN_NO_CHANGE,
+  uart_set_pin(GPS_UART, GPS_UART_TX_PIN, GPS_UART_RX_PIN, UART_PIN_NO_CHANGE,
                UART_PIN_NO_CHANGE);
   ret = uart_driver_install(GPS_UART, GPS_UART_RX_BUF_SIZE,
-                            GPS_UART_TX_BUF_SIZE, GPS_UART_QUEUE_SIZE, NULL, 0);
+                            GPS_UART_TX_BUF_SIZE, 8, &this->uart_queue_, 0);
   if (ret != ESP_OK) {
     ESP_LOGE(TAG, "uart_driver_install: %d", ret);
     return;
   }
+  uart_enable_pattern_det_intr(GPS_UART, '\n', 1, 10000, 10, 10);
+  uart_pattern_queue_reset(GPS_UART, 8);
+  uart_flush(GPS_UART);
   ESP_LOGI(TAG, "gps uart configured");
 
-  BaseType_t xRet = xTaskCreate(UART_GPS::GPS_Task, "ugv_io_gps", 2 * 1024,
-                                this, 1, &this->task_);
+  buffer_ = new uint8_t[GPS_BUF_SIZE];
+
+  BaseType_t xRet = xTaskCreate(UART_GPS::TaskEntry, "ugv_io_gps", 8 * 1024,
+                                this, 3, &this->task_);
   if (xRet != pdTRUE) {
     ESP_LOGE(TAG, "error creating GPS task");
     return;
@@ -103,7 +107,6 @@ err:
 }
 
 void UART_GPS::ProcessLine(const char *line, size_t len) {
-  ESP_LOGI(TAG, "gps data: %.*s", len, line);
   enum minmea_sentence_id id = minmea_sentence_id(line, false);
   switch (id) {
     case MINMEA_INVALID:
@@ -112,101 +115,117 @@ void UART_GPS::ProcessLine(const char *line, size_t len) {
       return;
     case MINMEA_SENTENCE_RMC: {
       minmea_sentence_rmc rmc;
-      bool                parse_success;
+      bool parse_success;
       parse_success = minmea_parse_rmc(&rmc, line);
       if (!parse_success) {
         goto invalid;
       }
       xSemaphoreTake(mutex_, pdMS_TO_TICKS(1000));
-      data_.valid       = rmc.valid;
-      data_.latitude    = minmea_tofloat(&rmc.latitude);
-      data_.longitude   = minmea_tofloat(&rmc.longitude);
-      data_.speed       = minmea_tofloat(&rmc.speed);
-      data_.course      = minmea_tofloat(&rmc.course);
+      data_.valid = rmc.valid;
+      data_.latitude = minmea_tocoord(&rmc.latitude);
+      data_.longitude = minmea_tocoord(&rmc.longitude);
+      data_.speed = minmea_tofloat(&rmc.speed);
+      data_.course = minmea_tofloat(&rmc.course);
       data_.last_update = xTaskGetTickCount();
+      ESP_LOGV(TAG, "RMC: %s, (%f,%f), speed=%f, course=%f",
+               data_.valid ? "valid" : "invalid", data_.latitude,
+               data_.longitude, data_.speed, data_.course);
       xSemaphoreGive(mutex_);
+      break;
     }
     case MINMEA_SENTENCE_GGA: {
       minmea_sentence_gga gga;
-      bool                parse_success;
+      bool parse_success;
       parse_success = minmea_parse_gga(&gga, line);
       if (!parse_success) {
         goto invalid;
       }
       xSemaphoreTake(mutex_, pdMS_TO_TICKS(1000));
-      data_.fix_quality    = (GpsFixQual)gga.fix_quality;
+      data_.fix_quality = (GpsFixQual)gga.fix_quality;
       data_.num_satellites = gga.satellites_tracked;
-      data_.latitude       = minmea_tofloat(&gga.latitude);
-      data_.longitude      = minmea_tofloat(&gga.longitude);
-      if (gga.altitude_units != 'M') {
+      data_.latitude = minmea_tocoord(&gga.latitude);
+      data_.longitude = minmea_tocoord(&gga.longitude);
+      if (gga.fix_quality != 0 && gga.altitude_units != 'M') {
         ESP_LOGW(TAG, "unknown altitude units: %c", gga.altitude_units);
       }
-      data_.altitude    = minmea_tofloat(&gga.altitude);
+      data_.altitude = minmea_tofloat(&gga.altitude);
       data_.last_update = xTaskGetTickCount();
+      ESP_LOGV(TAG, "GGA: qual: %d, num_sat=%d, (%f,%f), alt=%f",
+               data_.fix_quality, data_.num_satellites, data_.latitude,
+               data_.longitude, data_.altitude);
       xSemaphoreGive(mutex_);
+      break;
     }
-    default: ESP_LOGW(TAG, "unsupported nmea sentence: %s", line);
+    case MINMEA_SENTENCE_GSA: {
+      minmea_sentence_gsa gsa;
+      bool parse_success;
+      parse_success = minmea_parse_gsa(&gsa, line);
+      if (!parse_success) {
+        goto invalid;
+      }
+      xSemaphoreTake(mutex_, pdMS_TO_TICKS(1000));
+      data_.pdop = minmea_tofloat(&gsa.pdop);
+      data_.hdop = minmea_tofloat(&gsa.hdop);
+      data_.vdop = minmea_tofloat(&gsa.vdop);
+      ESP_LOGV(TAG, "GSA: pdop=%f, hdop=%f, vdop=%f", data_.pdop, data_.hdop,
+               data_.vdop);
+      xSemaphoreGive(mutex_);
+      break;
+    }
+    default: ESP_LOGV(TAG, "unsupported nmea sentence: %s", line);
   }
 }
 
-void UART_GPS::GPS_Task(void *arg) {
-  UART_GPS *gps = (UART_GPS *)arg;
-  (void)gps;
+void UART_GPS::HandleUartPattern() {
+  int pos = uart_pattern_pop_pos(GPS_UART);
+  if (pos < 0) {
+    ESP_LOGE(TAG, "uart_pattern_pop_pos: %d", pos);
+    return;
+  }
+  int read_bytes =
+      uart_read_bytes(GPS_UART, this->buffer_, pos + 1, pdMS_TO_TICKS(100));
+  if (read_bytes <= 0) {
+    ESP_LOGW(TAG, "uart_read_bytes error: %d", read_bytes);
+    return;
+  }
+  ESP_LOGV(TAG, "GPS bytes received: %.*s", read_bytes, buffer_);
+  buffer_[read_bytes] = '\0';
+  this->ProcessLine((const char *)buffer_,
+                    read_bytes + 1);  // process that line
+}
 
+void UART_GPS::DoTask() {
   ESP_LOGI(TAG, "gps_task start");
-  char * buf1    = (char *)malloc(GPS_BUF_SIZE);
-  char * buf2    = (char *)malloc(GPS_BUF_SIZE);
-  char * buf_end = buf1;
-  size_t read_bytes;
+  uart_event_t uart_event;
 
   esp_err_t ret;
-  ret = gps->WriteCommand(NMEA_OUTPUT_RMCGGA, sizeof(NMEA_OUTPUT_RMCGGA));
-  ESP_LOGI(TAG, "sent output rmc and gga: %d", ret);
-  ret = gps->WriteCommand(NMEA_UPDATE_1HZ, sizeof(NMEA_UPDATE_1HZ));
-  ESP_LOGI(TAG, "sent update 1hz: %d", ret);
+  ret = WriteCommand(NMEA_OUTPUT_RMCGGA, sizeof(NMEA_OUTPUT_RMCGGA));
+  ESP_LOGV(TAG, "sent output rmc and gga: %d", ret);
+  ret = WriteCommand(NMEA_UPDATE_1HZ, sizeof(NMEA_UPDATE_1HZ));
+  ESP_LOGV(TAG, "sent update 1hz: %d", ret);
 
   vTaskDelay(pdMS_TO_TICKS(100));
-  ret = gps->WriteCommand(NMEA_Q_RELEASE, sizeof(NMEA_Q_RELEASE));
-  ESP_LOGI(TAG, "sent q release: %d", ret);
+  ret = WriteCommand(NMEA_Q_RELEASE, sizeof(NMEA_Q_RELEASE));
+  ESP_LOGV(TAG, "sent q release: %d", ret);
 
   while (true) {
-    size_t buf_remaining = GPS_BUF_SIZE - (buf_end - buf1);
-    read_bytes = uart_read_bytes(GPS_UART, (uint8_t *)buf_end, buf_remaining,
-                                 portMAX_DELAY);
-    if (read_bytes <= 0) {
-      ESP_LOGW(TAG, "GPS error: %d", read_bytes);
+    BaseType_t pdRet = xQueueReceive(uart_queue_, &uart_event, portMAX_DELAY);
+    if (!pdRet) {
       continue;
     }
-    ESP_LOGW(TAG, "GPS bytes received: %.*s", read_bytes, buf_end);
-    buf_end += read_bytes;
-    int lines_received = 0;
-    for (char *c = buf1; c < buf_end; c++) {
-      if (*c != '\n') {  // wait for \n, if not found loop again
-        continue;
-      }
-      lines_received++;
-      c++;  // now c is one past the end of the whole string including \n
-
-      size_t remaining = buf_end - c;
-      memcpy(buf2, c, remaining);  // copy remaining text from buf1 to buf2
-      buf_end = buf2 + remaining;  // update buf_end to point to the end of the
-                                   // remaining text in buf2
-
-      char * str_ptr = buf1;
-      size_t str_len = c - buf1;
-      str_ptr[str_len] =
-          '\0';  // append the NULL byte (safe because old data was copied)
-      gps->ProcessLine(str_ptr, str_len);  // process that line
-
-      std::swap(buf1, buf2);  // swap buffers
-      c = buf1;  // start over on text which was copied to buf2 (now buf1)
-    }
-    if (lines_received == 0) {
-      ESP_LOGW(TAG, "no lines received, waiting for more data");
+    switch (uart_event.type) {
+      case UART_DATA: break;
+      case UART_PATTERN_DET: this->HandleUartPattern(); break;
+      default:
+        ESP_LOGW(TAG, "Unhandled UART event type: %d", uart_event.type);
+        break;
     }
   }
-  free(buf1);
-  free(buf2);
+}
+
+void UART_GPS::TaskEntry(void *arg) {
+  UART_GPS *gps = (UART_GPS *)arg;
+  gps->DoTask();
   vTaskDelete(NULL);
 }
 

main/ugv_io_gps.hh

@@ -9,28 +9,31 @@ namespace ugv {
 namespace io {
 
 enum GpsFixQual {
-  GPS_FIX_INVALID   = 0,  // invalid gps fix
-  GPS_FIX_GPS       = 1,  // GPS fix
-  GPS_FIX_DGPS      = 2,  // differential GPS fix
-  GPS_FIX_PPS       = 3,  // PPS fix
-  GPS_FIX_RTK       = 4,  // Real Time Kinematic fix
-  GPS_FIX_FRTK      = 5,  // Float Real Time Kinematic fix
+  GPS_FIX_INVALID = 0,    // invalid gps fix
+  GPS_FIX_GPS = 1,        // GPS fix
+  GPS_FIX_DGPS = 2,       // differential GPS fix
+  GPS_FIX_PPS = 3,        // PPS fix
+  GPS_FIX_RTK = 4,        // Real Time Kinematic fix
+  GPS_FIX_FRTK = 5,       // Float Real Time Kinematic fix
   GPS_FIX_ESTIMATED = 6,  // Estimated fix
-  GPS_FIX_MANUAL    = 7,  // Manual fix
+  GPS_FIX_MANUAL = 7,     // Manual fix
   GPS_FIX_SIMULATED = 8,  // Simulated fix
 };
 
 struct GpsData {
   TickType_t last_update;
-  bool       valid;
+  bool valid;
   GpsFixQual fix_quality;
-  uint8_t    num_satellites;
+  uint8_t num_satellites;
 
   float latitude;   // degrees +/-
   float longitude;  // degrees +/-
   float altitude;   // meters
   float speed;      // knots
   float course;     // degrees clockwise of north
+  float pdop;
+  float hdop;
+  float vdop;
 };
 
 class UART_GPS {
@@ -43,14 +46,18 @@ class UART_GPS {
   void GetData(GpsData& data);
 
  private:
-  TaskHandle_t      task_;
+  TaskHandle_t task_;
   SemaphoreHandle_t mutex_;
-  GpsData           data_;
+  QueueHandle_t uart_queue_;
+  GpsData data_;
+  uint8_t* buffer_;
 
   esp_err_t WriteCommand(const char* cmd, size_t len);
-  void      ProcessLine(const char* line, size_t len);
+  void HandleUartPattern();
+  void ProcessLine(const char* line, size_t len);
 
-  static void GPS_Task(void* arg);
+  void DoTask();
+  static void TaskEntry(void* arg);
 };
 
 }  // namespace io

main/ugv_io_mpu.cc

@@ -3,17 +3,39 @@
 #include <driver/uart.h>
 #include <esp_log.h>
 
+#include <cmath>
+
+constexpr float M_PI = 3.1415926535897932384626433832795;
+
 #include "MPU.hpp"
+#include "i2c_mutex.h"
 #include "mpu/math.hpp"
 
 namespace ugv {
 namespace io {
 
-static constexpr gpio_num_t       MPU_SDA         = GPIO_NUM_2;
-static constexpr gpio_num_t       MPU_SCL         = GPIO_NUM_3;
-static constexpr mpud::accel_fs_t MPU_ACCEL_FS    = mpud::ACCEL_FS_2G;
-static constexpr mpud::gyro_fs_t  MPU_GYRO_FS     = mpud::GYRO_FS_500DPS;
-static constexpr float            MPU_MAG_TO_FLUX = (4912.f) / (32760.f);
+static constexpr gpio_num_t MPU_SDA = GPIO_NUM_5;
+static constexpr gpio_num_t MPU_SCL = GPIO_NUM_4;
+static constexpr mpud::accel_fs_t MPU_ACCEL_FS = mpud::ACCEL_FS_2G;
+static constexpr mpud::gyro_fs_t MPU_GYRO_FS = mpud::GYRO_FS_500DPS;
+static constexpr float MPU_MAG_TO_FLUX = (4912.f) / (32760.f);
+
+static const Vec3f ACCEL_OFFSET = {0., 0., 0.};
+static const Mat3f ACCEL_MAT = {1., 0., 0., 0., 1., 0., 0., 0., 1.};
+//static const Vec3f MAG_OFFSET   = {76.6938, 11.5453, 64.3988};
+//static const Vec3f MAG_OFFSET   = {62.3746, 9.89597,39.9587};
+static const Vec3f MAG_OFFSET   = {70.8462, 19.7919, 64.0239};
+//static const Vec3f MAG_OFFSET = {0, 0, 0,};
+//static const Mat3f MAG_MAT = {0., -0.0305935, 0., 0.0294454, 0.,
+//                              0., 0.,         0., 0.0289344};
+//  static const Mat3f MAG_MAT = {0., -0.034202, 0.,0.028624, 0., 0., 0., 0., 0.0276165};
+//static const Mat3f MAG_MAT = {0., -0.028624, 0.,0.034202, 0., 0., 0., 0., 0.0276165};
+static const Mat3f MAG_MAT = {0., -0.0279054, 0., 0.0272776, 0., 0., 0., 0., 0.0259509};
+//static const Mat3f MAG_MAT = {1, 0, 0, 0, 1, 0, 0, 0, 1};
+//static const Vec3f GYRO_OFFSET = {-2.01854, -2.62769, -0.30177};
+static const Vec3f GYRO_OFFSET = {-1.99247, -2.48463, 0.294875};
+//static const Vec3f GYRO_OFFSET = {0, 0, 0};
+static const Mat3f GYRO_MAT = {1., 0., 0., 0., 1., 0., 0., 0., -1.};
 
 static const char *TAG = "ugv_io_mpu";
 
@@ -22,68 +44,140 @@ Vec3f::Vec3f(float x, float y, float z) : x(x), y(y), z(z) {}
 Vec3f::Vec3f(const mpud::float_axes_t &axes)
     : x(axes.x), y(axes.y), z(axes.z) {}
 
-MPU::MPU() : mpu_(nullptr), mpu_data_() {}
+MPU::MPU() : mpu_(nullptr) {}
 
 MPU::~MPU() { delete mpu_; }
 
 void MPU::Init() {
-  mutex_    = xSemaphoreCreateMutex();
-  mpu_data_ = MpuData{};
+  mutex_ = xSemaphoreCreateMutex();
+
+  BaseType_t xRet = xTaskCreate(MPU::TaskEntry, "ugv_io_mpu", 8 * 1024, this, 3,
+                                &this->task_);
+  if (xRet != pdTRUE) {
+    ESP_LOGE(TAG, "error creating MPU task");
+    return;
+  }
+}
+
+void MPU::Calibrate() {
+  mpud::raw_axes_t accel_offset, gyro_offset;
+  mpu_->computeOffsets(&accel_offset, &gyro_offset);
+  mpu_->setAccelOffset(accel_offset);
+  mpu_->setGyroOffset(gyro_offset);
+  {
+    auto ao = mpud::math::accelGravity(accel_offset, mpud::ACCEL_FS_2G);
+    auto go = mpud::math::gyroDegPerSec(gyro_offset, mpud::GYRO_FS_250DPS);
+    ESP_LOGI(TAG, "MPU offsets: accel=(%f, %f, %f) gyro=(%f, %f, %f)", ao.x,
+             ao.y, ao.z, go.x, go.y, go.z);
+  }
+}
 
+void MPU::GetData(MpuData &data) {
+  xSemaphoreTake(mutex_, pdMS_TO_TICKS(10));
+  data = last_data_;
+  last_data_.gyro_rate = {0, 0, 0};
+  xSemaphoreGive(mutex_);
+}
+
+bool MPU::Initialize() {
+  xSemaphoreTake(i2c_mutex, portMAX_DELAY);
   mpu_bus_ = &i2c0;
-  mpu_bus_->begin(MPU_SDA, MPU_SCL);
+  // This is shared with the oled, so just use those pins
+  mpu_bus_->setTimeout(10);
+  mpu_bus_->begin(MPU_SDA, MPU_SCL, GPIO_PULLUP_DISABLE, GPIO_PULLUP_DISABLE, 400000);
   mpu_ = new mpud::MPU(*mpu_bus_);
 
   esp_err_t ret;
-  ret = mpu_->testConnection();
-  if (ret != ESP_OK) {
-    ESP_LOGE(TAG, "MPU not connected");
-    return;
+  int tries = 0;
+  for (; tries < 5; ++tries) {
+    mpu_->getInterruptStatus();
+    ret = mpu_->testConnection();
+    if (ret != ESP_OK) {
+      uint8_t wai = mpu_->whoAmI();
+      ESP_LOGE(TAG, "MPU not connected (whoAmI: %#x)", wai);
+      vTaskDelay(pdMS_TO_TICKS(100));
+      continue;
+    }
+    ret = mpu_->compassTestConnection();
+    if (ret != ESP_OK) {
+      uint8_t wai = mpu_->compassWhoAmI();
+      ESP_LOGW(TAG, "MPU compass not connected (whoAmI: %#x)", wai);
+    }
+    ret = mpu_->initialize();
+    if (ret != ESP_OK) {
+      ESP_LOGE(TAG, "MPU initialization error");
+      continue;
+    }
+    break;
   }
-  ret = mpu_->initialize();
-  if (ret != ESP_OK) {
-    ESP_LOGE(TAG, "MPU initialization error");
-    return;
+  if (tries == 5) {
+    return false;
   }
+  // Calibrate();
   mpu_->setAccelFullScale(MPU_ACCEL_FS);
   mpu_->setGyroFullScale(MPU_GYRO_FS);
+  // force magnetometer into continuous mode
+  mpu_->compassWriteByte(0x0A, 0x12);
+  xSemaphoreGive(i2c_mutex);
 
-  BaseType_t xRet =
-      xTaskCreate(MPU::MPU_Task, "ugv_io_mpu", 2 * 1024, this, 3, &this->task_);
-  if (xRet != pdTRUE) {
-    ESP_LOGE(TAG, "error creating MPU task");
-    return;
-  }
-  ESP_LOGI(TAG, "MPU initialized and task started");
-}
-
-void MPU::GetData(MpuData &data) {
-  xSemaphoreTake(mutex_, pdMS_TO_TICKS(1000));
-  data = MpuData(mpu_data_);
-  xSemaphoreGive(mutex_);
+  ESP_LOGI(TAG, "MPU initialized");
+  return true;
 }
 
 void MPU::DoTask() {
+  ESP_LOGI(TAG, "MPU task start");
+  mpud::raw_axes_t accel_, gyro_, mag_;
   esp_err_t ret;
+  MpuData data;
+
+  if (!Initialize()) {
+    return;
+  }
+
   while (true) {
-    vTaskDelay(pdMS_TO_TICKS(50));
-    ret = mpu_->motion(&accel_, &gyro_, &mag_);
+    xSemaphoreTake(i2c_mutex, portMAX_DELAY);
+    ret = mpu_->motion(&accel_, &gyro_);
+    if (ret != ESP_OK) {
+      ESP_LOGE(TAG, "error reading MPU: %#x", ret);
+      xSemaphoreGive(i2c_mutex);
+      continue;
+    }
+    uint8_t compass_data[7];
+    mpu_->setAuxI2CBypass(true);
+    ret = mpu_bus_->readBytes(mpud::COMPASS_I2CADDRESS, 0x03, 7, compass_data);
     if (ret != ESP_OK) {
-      ESP_LOGE(TAG, "error reading MPU");
+      ESP_LOGE(TAG, "error reading MPU compass: %#x", ret);
+      xSemaphoreGive(i2c_mutex);
       continue;
     }
-    xSemaphoreTake(mutex_, pdMS_TO_TICKS(1000));
-    mpu_data_.accel     = mpud::accelGravity(accel_, MPU_ACCEL_FS);
-    mpu_data_.gyro_rate = mpud::gyroDegPerSec(gyro_, MPU_GYRO_FS);
-    mpu_data_.mag.x     = ((float)mag_.x) * MPU_MAG_TO_FLUX;
-    mpu_data_.mag.y     = ((float)mag_.y) * MPU_MAG_TO_FLUX;
-    mpu_data_.mag.z     = ((float)mag_.z) * MPU_MAG_TO_FLUX;
+    mpu_->setAuxI2CBypass(false);
+    xSemaphoreGive(i2c_mutex);
+    int16_t mx = (static_cast<int16_t>(compass_data[1]) << 8) |
+                 static_cast<int16_t>(compass_data[0]);
+    int16_t my = (static_cast<int16_t>(compass_data[3]) << 8) |
+                 static_cast<int16_t>(compass_data[2]);
+    int16_t mz = (static_cast<int16_t>(compass_data[5]) << 8) |
+                 static_cast<int16_t>(compass_data[4]);
+    //  ESP_LOGV(TAG, "compass: %d, %d, %d", mx, my, mz);
+    data.accel = mpud::accelGravity(accel_, MPU_ACCEL_FS);
+    data.accel = ACCEL_MAT * (data.accel + ACCEL_OFFSET);
+    data.gyro_rate = mpud::gyroDegPerSec(gyro_, MPU_GYRO_FS);
+    data.gyro_rate = GYRO_MAT * (data.gyro_rate + GYRO_OFFSET);
+    data.mag.x = ((float)mx) * MPU_MAG_TO_FLUX;
+    data.mag.y = ((float)my) * MPU_MAG_TO_FLUX;
+    data.mag.z = ((float)mz) * MPU_MAG_TO_FLUX;
+    data.mag = MAG_MAT * (data.mag + MAG_OFFSET);
+    xSemaphoreTake(mutex_, pdMS_TO_TICKS(100));
+    last_data_ = data;
     xSemaphoreGive(mutex_);
   }
-  vTaskDelete(NULL);
 }
 
-void MPU::MPU_Task(void *arg) { ((MPU *)arg)->DoTask(); }
+void MPU::TaskEntry(void *arg) {
+  MPU *mpu = (MPU *)arg;
+  mpu->DoTask();
+  vTaskDelete(NULL);
+}
 
 };  // namespace io
 };  // namespace ugv

main/ugv_io_mpu.hh

@@ -1,6 +1,5 @@
 #pragma once
 
-#include <freertos/FreeRTOS.h>
 #include <stdint.h>
 #include <MPU.hpp>
 
@@ -14,7 +13,70 @@ struct Vec3f {
 
   Vec3f();
   Vec3f(float x, float y, float z);
-  Vec3f(const mpud::float_axes_t &axes);
+  Vec3f(const mpud::float_axes_t& axes);
+
+  Vec3f operator*(float s) const { return {s * x, s * y, s * z}; }
+
+  Vec3f& operator*=(float s) {
+    x *= s;
+    y *= s;
+    z *= s;
+    return *this;
+  }
+
+  Vec3f operator+(const Vec3f& a) const { return {x + a.x, y + a.y, z + a.z}; }
+
+  Vec3f& operator+=(const Vec3f& a) {
+    x += a.x;
+    y += a.y;
+    z += a.z;
+    return *this;
+  }
+
+  Vec3f operator-(const Vec3f& a) const { return {x - a.x, y - a.y, z - a.z}; }
+
+  Vec3f& operator-=(const Vec3f& a) {
+    x -= a.x;
+    y -= a.y;
+    z -= a.z;
+    return *this;
+  }
+
+  float dot(const Vec3f& v) const { return x * v.x + y * v.y + z * v.z; }
+
+  float norm2() const { return x * x + y * y + z * z; }
+};
+
+struct Mat3f {
+  union {
+    Vec3f rx;
+    float xx, xy, xz;
+  };
+  union {
+    Vec3f ry;
+    float yx, yy, yz;
+  };
+  union {
+    Vec3f rz;
+    float zx, zy, zz;
+  };
+
+  Mat3f(Vec3f rx_, Vec3f ry_, Vec3f rz_) : rx(rx_), ry(ry_), rz(rz_) {}
+
+  Mat3f(float xx, float xy, float xz, float yx, float yy, float yz, float zx,
+        float zy, float zz)
+      : rx(xx, xy, xz), ry(yx, yy, yz), rz(zx, zy, zz) {}
+
+  Vec3f operator*(const Vec3f& v) const {
+    return {rx.dot(v), ry.dot(v), rz.dot(v)};
+  }
+
+  Mat3f transpose() const { return {xx, yx, zx, xy, yy, zy, xz, yz, zz}; }
+
+  Mat3f operator*(const Mat3f& m) const {
+    Mat3f mt = m.transpose();
+    return {mt * rx, mt * ry, mt * rz};
+  }
 };
 
 struct MpuData {
@@ -32,21 +94,20 @@ class MPU {
   ~MPU();
 
   void Init();
+  void Calibrate();
 
-  void GetData(MpuData &data);
+  void GetData(MpuData& data);
 
  private:
-  mpud::mpu_bus_t *mpu_bus_;
-  mpud::MPU *      mpu_;
-  mpud::raw_axes_t accel_, gyro_, mag_;
-
-  TaskHandle_t      task_;
+  mpud::mpu_bus_t* mpu_bus_;
+  mpud::MPU* mpu_;
+  TaskHandle_t task_;
   SemaphoreHandle_t mutex_;
-  MpuData           mpu_data_;
+  MpuData last_data_;
 
   void DoTask();
-
-  static void MPU_Task(void *arg);
+  bool Initialize();
+  static void TaskEntry(void* arg);
 };
 
 }  // namespace io

main/ugv_main.cc

@@ -1,100 +1,11 @@
-#include <esp_log.h>
-#include <string.h>
-#include <u8g2.h>
+#include "i2c_mutex.h"
+#include "ugv.hh"
 
-#include "U8g2lib.hh"
-#include "sx127x_driver.h"
-#include "sx127x_registers.h"
-#include "ugv_comms.h"
-#include "ugv_config.h"
-#include "ugv_io.hh"
-
-namespace ugv {
-
-using ugv::io::IO;
-
-static const char *TAG = "ugv_main";
-
-U8G2 *oled;
-
-void setup_oled(void) {
-  oled = new U8G2_SSD1306_128X64_NONAME_F_HW_I2C(U8G2_R0, 16, 15, 4);
-  oled->initDisplay();
-  oled->clearDisplay();
-  oled->setPowerSave(false);
+extern "C" {
+SemaphoreHandle_t i2c_mutex;
 }
 
-void setup(void) {
-  ESP_LOGI(TAG, "setup");
-
-  setup_oled();
-  ugv_comms_init();
-  IO.Init();
-}
-
-#define BUF_SZ 32
-
-void loop(void) {
-  static int32_t    lora_rssi;
-  static uint8_t    lora_lna_gain;
-  static TickType_t last_packet_tick;
-  static int32_t    last_packet_rssi;
-  static int8_t     last_packet_snr;
-
-  static char buf[BUF_SZ];
-  static io::Inputs inputs;
-
-  IO.ReadInputs(inputs);
-  // ESP_LOGI(TAG, "inputs %s", inputs.ToString());
-
-  oled->firstPage();
-  sx127x_read_rssi(ugv_comms_state.lora, &lora_rssi);
-  sx127x_read_lna_gain(ugv_comms_state.lora, &lora_lna_gain);
-
-  xSemaphoreTake(ugv_comms_state.mutex, portMAX_DELAY);
-  last_packet_tick = ugv_comms_state.last_packet_tick;
-  last_packet_rssi = ugv_comms_state.last_packet_rssi;
-  last_packet_snr  = ugv_comms_state.last_packet_snr;
-  xSemaphoreGive(ugv_comms_state.mutex);
-  do {
-    oled->drawRFrame(0, 0, OLED_W, OLED_H, 4);
-
-    multi_heap_info_t heap_info;
-    heap_caps_get_info(&heap_info, MALLOC_CAP_DEFAULT);
-    oled->setFont(u8g2_font_4x6_mr);
-    oled->drawStr(4, 8, "=====UAS UGV=====");
-    snprintf(buf, BUF_SZ, "heap allc/free %d/%d",
-             heap_info.total_allocated_bytes, heap_info.total_free_bytes);
-    oled->drawStr(4, 2 * 8, buf);
-
-    snprintf(buf, BUF_SZ, "rssi: %d lna gain: %d", lora_rssi, lora_lna_gain);
-    oled->drawStr(4, 3 * 8, buf);
-
-    if (last_packet_tick > 0) {
-      double time_since_last_packet =
-          1000.0f /
-          ((xTaskGetTickCount() - last_packet_tick) * portTICK_RATE_MS);
-      snprintf(buf, BUF_SZ, "last pkt rx %f s ago", time_since_last_packet);
-      oled->drawStr(4, 4 * 8, buf);
-      snprintf(buf, BUF_SZ, "pkt rssi: %d, snr: %f", last_packet_rssi,
-               last_packet_snr * 0.25f);
-      oled->drawStr(4, 5 * 8, buf);
-    } else {
-      oled->drawStr(4, 4 * 8, "no pkt rx");
-    }
-  } while (oled->nextPage());
-  vTaskDelay(pdMS_TO_TICKS(1000));
-}
-
-void loopTask(void *pvUser) {
-  setup();
-  while (1) {
-    loop();
-  }
-}
-
-}  // namespace ugv
-
 extern "C" void app_main() {
-  xTaskCreatePinnedToCore(ugv::loopTask, "loopTask", 8192, NULL, 1, NULL, 1);
+  i2c_mutex = xSemaphoreCreateMutex();
+  ugv::Start();
 }

sdkconfig.default

@@ -0,0 +1,632 @@
+#
+# Automatically generated file; DO NOT EDIT.
+# Espressif IoT Development Framework Configuration
+#
+
+#
+# SDK tool configuration
+#
+CONFIG_TOOLPREFIX="xtensa-esp32-elf-"
+CONFIG_MAKE_WARN_UNDEFINED_VARIABLES=y
+
+#
+# Example Configuration
+#
+CONFIG_LCD_TYPE_AUTO=y
+CONFIG_LCD_TYPE_ST7789V=
+CONFIG_LCD_TYPE_ILI9341=
+CONFIG_LCD_OVERCLOCK=
+
+#
+# SX127X Driver Configuration
+#
+CONFIG_SX127X_TASK_STACK_SIZE=2048
+CONFIG_SX127X_TASK_PRIORITY=3
+CONFIG_SX127X_RX_QUEUE_LEN=8
+CONFIG_SX127X_TX_QUEUE_LEN=8
+CONFIG_SX127X_SPI_CLOCK_HZ=8000000
+CONFIG_SX127X_SPI_QUEUE_SIZE=8
+CONFIG_SX127X_SPI_DMA_CHAN=1
+CONFIG_SX127X_USE_NANOPB=y
+
+#
+# MPU driver
+#
+CONFIG_MPU6000=
+CONFIG_MPU6050=
+CONFIG_MPU6500=
+CONFIG_MPU6555=
+CONFIG_MPU9150=
+CONFIG_MPU9250=
+CONFIG_MPU9255=y
+CONFIG_MPU_CHIP_MODEL="MPU9255"
+CONFIG_MPU_AK8963=y
+CONFIG_MPU_AK89xx=y
+CONFIG_MPU_I2C=y
+CONFIG_MPU_SPI=
+CONFIG_MPU_COMM_PROTOCOL="I2C"
+CONFIG_MPU_LOG_LEVEL_DEFAULT=y
+CONFIG_MPU_LOG_LEVEL_NONE=
+CONFIG_MPU_LOG_LEVEL_ERROR=
+CONFIG_MPU_LOG_LEVEL_WARN=
+CONFIG_MPU_LOG_LEVEL_INFO=
+CONFIG_MPU_LOG_LEVEL_DEBUG=
+CONFIG_MPU_LOG_LEVEL=4
+CONFIG_MPU_ENABLE_DMP=
+CONFIG_MPU_LOG_ERROR_TRACES=y
+
+#
+# I2Cbus
+#
+CONFIG_I2CBUS_LOG_ERRORS=y
+CONFIG_I2CBUS_LOG_READWRITES=
+
+#
+# Bootloader config
+#
+CONFIG_LOG_BOOTLOADER_LEVEL_NONE=
+CONFIG_LOG_BOOTLOADER_LEVEL_ERROR=
+CONFIG_LOG_BOOTLOADER_LEVEL_WARN=y
+CONFIG_LOG_BOOTLOADER_LEVEL_INFO=
+CONFIG_LOG_BOOTLOADER_LEVEL_DEBUG=
+CONFIG_LOG_BOOTLOADER_LEVEL_VERBOSE=
+CONFIG_LOG_BOOTLOADER_LEVEL=2
+CONFIG_BOOTLOADER_VDDSDIO_BOOST_1_8V=
+CONFIG_BOOTLOADER_VDDSDIO_BOOST_1_9V=y
+CONFIG_BOOTLOADER_FACTORY_RESET=
+CONFIG_BOOTLOADER_APP_TEST=
+
+#
+# Security features
+#
+CONFIG_SECURE_SIGNED_APPS_NO_SECURE_BOOT=
+CONFIG_SECURE_BOOT_ENABLED=
+CONFIG_FLASH_ENCRYPTION_ENABLED=
+
+#
+# Serial flasher config
+#
+CONFIG_ESPTOOLPY_BAUD_OTHER_VAL=115200
+CONFIG_FLASHMODE_QIO=
+CONFIG_FLASHMODE_QOUT=
+CONFIG_FLASHMODE_DIO=y
+CONFIG_FLASHMODE_DOUT=
+CONFIG_ESPTOOLPY_FLASHMODE="dio"
+CONFIG_ESPTOOLPY_FLASHFREQ_80M=y
+CONFIG_ESPTOOLPY_FLASHFREQ_40M=
+CONFIG_ESPTOOLPY_FLASHFREQ_26M=
+CONFIG_ESPTOOLPY_FLASHFREQ_20M=
+CONFIG_ESPTOOLPY_FLASHFREQ="80m"
+CONFIG_ESPTOOLPY_FLASHSIZE_1MB=
+CONFIG_ESPTOOLPY_FLASHSIZE_2MB=
+CONFIG_ESPTOOLPY_FLASHSIZE_4MB=y
+CONFIG_ESPTOOLPY_FLASHSIZE_8MB=
+CONFIG_ESPTOOLPY_FLASHSIZE_16MB=
+CONFIG_ESPTOOLPY_FLASHSIZE="4MB"
+CONFIG_ESPTOOLPY_FLASHSIZE_DETECT=y
+CONFIG_ESPTOOLPY_BEFORE_RESET=y
+CONFIG_ESPTOOLPY_BEFORE_NORESET=
+CONFIG_ESPTOOLPY_BEFORE="default_reset"
+CONFIG_ESPTOOLPY_AFTER_RESET=y
+CONFIG_ESPTOOLPY_AFTER_NORESET=
+CONFIG_ESPTOOLPY_AFTER="hard_reset"
+CONFIG_MONITOR_BAUD_9600B=
+CONFIG_MONITOR_BAUD_57600B=
+CONFIG_MONITOR_BAUD_115200B=y
+CONFIG_MONITOR_BAUD_230400B=
+CONFIG_MONITOR_BAUD_921600B=
+CONFIG_MONITOR_BAUD_2MB=
+CONFIG_MONITOR_BAUD_OTHER=
+CONFIG_MONITOR_BAUD_OTHER_VAL=115200
+CONFIG_MONITOR_BAUD=115200
+
+#
+# Partition Table
+#
+CONFIG_PARTITION_TABLE_SINGLE_APP=y
+CONFIG_PARTITION_TABLE_TWO_OTA=
+CONFIG_PARTITION_TABLE_CUSTOM=
+CONFIG_PARTITION_TABLE_CUSTOM_FILENAME="partitions.csv"
+CONFIG_PARTITION_TABLE_FILENAME="partitions_singleapp.csv"
+CONFIG_PARTITION_TABLE_OFFSET=0x8000
+CONFIG_PARTITION_TABLE_MD5=y
+
+#
+# Compiler options
+#
+CONFIG_OPTIMIZATION_LEVEL_DEBUG=y
+CONFIG_OPTIMIZATION_LEVEL_RELEASE=
+CONFIG_OPTIMIZATION_ASSERTIONS_ENABLED=y
+CONFIG_OPTIMIZATION_ASSERTIONS_SILENT=
+CONFIG_OPTIMIZATION_ASSERTIONS_DISABLED=
+CONFIG_CXX_EXCEPTIONS=
+CONFIG_STACK_CHECK_NONE=
+CONFIG_STACK_CHECK_NORM=
+CONFIG_STACK_CHECK_STRONG=y
+CONFIG_STACK_CHECK_ALL=
+CONFIG_STACK_CHECK=y
+CONFIG_WARN_WRITE_STRINGS=y
+
+#
+# Component config
+#
+
+#
+# MPU driver
+#
+
+#
+# I2Cbus
+#
+
+#
+# Driver configurations
+#
+
+#
+# ADC configuration
+#
+CONFIG_ADC_FORCE_XPD_FSM=
+CONFIG_ADC2_DISABLE_DAC=y
+
+#
+# SPI master configuration
+#
+CONFIG_SPI_MASTER_IN_IRAM=
+CONFIG_SPI_MASTER_ISR_IN_IRAM=y
+
+#
+# Application Level Tracing
+#
+CONFIG_ESP32_APPTRACE_DEST_TRAX=
+CONFIG_ESP32_APPTRACE_DEST_NONE=y
+CONFIG_ESP32_APPTRACE_ENABLE=
+CONFIG_ESP32_APPTRACE_LOCK_ENABLE=y
+
+#
+# SPI Flash driver
+#
+CONFIG_SPI_FLASH_VERIFY_WRITE=
+CONFIG_SPI_FLASH_ENABLE_COUNTERS=
+CONFIG_SPI_FLASH_ROM_DRIVER_PATCH=y
+CONFIG_SPI_FLASH_WRITING_DANGEROUS_REGIONS_ABORTS=y
+CONFIG_SPI_FLASH_WRITING_DANGEROUS_REGIONS_FAILS=
+CONFIG_SPI_FLASH_WRITING_DANGEROUS_REGIONS_ALLOWED=
+
+#
+# mbedTLS
+#
+CONFIG_MBEDTLS_SSL_MAX_CONTENT_LEN=16384
+CONFIG_MBEDTLS_DEBUG=
+CONFIG_MBEDTLS_HARDWARE_AES=y
+CONFIG_MBEDTLS_HARDWARE_MPI=
+CONFIG_MBEDTLS_HARDWARE_SHA=
+CONFIG_MBEDTLS_HAVE_TIME=y
+CONFIG_MBEDTLS_HAVE_TIME_DATE=
+CONFIG_MBEDTLS_TLS_SERVER_AND_CLIENT=y
+CONFIG_MBEDTLS_TLS_SERVER_ONLY=
+CONFIG_MBEDTLS_TLS_CLIENT_ONLY=
+CONFIG_MBEDTLS_TLS_DISABLED=
+CONFIG_MBEDTLS_TLS_SERVER=y
+CONFIG_MBEDTLS_TLS_CLIENT=y
+CONFIG_MBEDTLS_TLS_ENABLED=y
+
+#
+# TLS Key Exchange Methods
+#
+CONFIG_MBEDTLS_PSK_MODES=
+CONFIG_MBEDTLS_KEY_EXCHANGE_RSA=y
+CONFIG_MBEDTLS_KEY_EXCHANGE_DHE_RSA=y
+CONFIG_MBEDTLS_KEY_EXCHANGE_ELLIPTIC_CURVE=y
+CONFIG_MBEDTLS_KEY_EXCHANGE_ECDHE_RSA=y
+CONFIG_MBEDTLS_KEY_EXCHANGE_ECDHE_ECDSA=y
+CONFIG_MBEDTLS_KEY_EXCHANGE_ECDH_ECDSA=y
+CONFIG_MBEDTLS_KEY_EXCHANGE_ECDH_RSA=y
+CONFIG_MBEDTLS_SSL_RENEGOTIATION=y
+CONFIG_MBEDTLS_SSL_PROTO_SSL3=
+CONFIG_MBEDTLS_SSL_PROTO_TLS1=y
+CONFIG_MBEDTLS_SSL_PROTO_TLS1_1=y
+CONFIG_MBEDTLS_SSL_PROTO_TLS1_2=y
+CONFIG_MBEDTLS_SSL_PROTO_DTLS=
+CONFIG_MBEDTLS_SSL_ALPN=y
+CONFIG_MBEDTLS_SSL_SESSION_TICKETS=y
+
+#
+# Symmetric Ciphers
+#
+CONFIG_MBEDTLS_AES_C=y
+CONFIG_MBEDTLS_CAMELLIA_C=
+CONFIG_MBEDTLS_DES_C=
+CONFIG_MBEDTLS_RC4_DISABLED=y
+CONFIG_MBEDTLS_RC4_ENABLED_NO_DEFAULT=
+CONFIG_MBEDTLS_RC4_ENABLED=
+CONFIG_MBEDTLS_BLOWFISH_C=
+CONFIG_MBEDTLS_XTEA_C=
+CONFIG_MBEDTLS_CCM_C=y
+CONFIG_MBEDTLS_GCM_C=y
+CONFIG_MBEDTLS_RIPEMD160_C=
+
+#
+# Certificates
+#
+CONFIG_MBEDTLS_PEM_PARSE_C=y
+CONFIG_MBEDTLS_PEM_WRITE_C=y
+CONFIG_MBEDTLS_X509_CRL_PARSE_C=y
+CONFIG_MBEDTLS_X509_CSR_PARSE_C=y
+CONFIG_MBEDTLS_ECP_C=y
+CONFIG_MBEDTLS_ECDH_C=y
+CONFIG_MBEDTLS_ECDSA_C=y
+CONFIG_MBEDTLS_ECP_DP_SECP192R1_ENABLED=y
+CONFIG_MBEDTLS_ECP_DP_SECP224R1_ENABLED=y
+CONFIG_MBEDTLS_ECP_DP_SECP256R1_ENABLED=y
+CONFIG_MBEDTLS_ECP_DP_SECP384R1_ENABLED=y
+CONFIG_MBEDTLS_ECP_DP_SECP521R1_ENABLED=y
+CONFIG_MBEDTLS_ECP_DP_SECP192K1_ENABLED=y
+CONFIG_MBEDTLS_ECP_DP_SECP224K1_ENABLED=y
+CONFIG_MBEDTLS_ECP_DP_SECP256K1_ENABLED=y
+CONFIG_MBEDTLS_ECP_DP_BP256R1_ENABLED=y
+CONFIG_MBEDTLS_ECP_DP_BP384R1_ENABLED=y
+CONFIG_MBEDTLS_ECP_DP_BP512R1_ENABLED=y
+CONFIG_MBEDTLS_ECP_DP_CURVE25519_ENABLED=y
+CONFIG_MBEDTLS_ECP_NIST_OPTIM=y
+
+#
+# LWIP
+#
+CONFIG_L2_TO_L3_COPY=
+CONFIG_LWIP_IRAM_OPTIMIZATION=
+CONFIG_LWIP_MAX_SOCKETS=10
+CONFIG_USE_ONLY_LWIP_SELECT=
+CONFIG_LWIP_SO_REUSE=y
+CONFIG_LWIP_SO_REUSE_RXTOALL=y
+CONFIG_LWIP_SO_RCVBUF=
+CONFIG_LWIP_DHCP_MAX_NTP_SERVERS=1
+CONFIG_LWIP_IP_FRAG=
+CONFIG_LWIP_IP_REASSEMBLY=
+CONFIG_LWIP_STATS=
+CONFIG_LWIP_ETHARP_TRUST_IP_MAC=
+CONFIG_ESP_GRATUITOUS_ARP=y
+CONFIG_GARP_TMR_INTERVAL=60
+CONFIG_TCPIP_RECVMBOX_SIZE=32
+CONFIG_LWIP_DHCP_DOES_ARP_CHECK=y
+
+#
+# DHCP server
+#
+CONFIG_LWIP_DHCPS_LEASE_UNIT=60
+CONFIG_LWIP_DHCPS_MAX_STATION_NUM=8
+CONFIG_LWIP_AUTOIP=
+CONFIG_LWIP_NETIF_LOOPBACK=y
+CONFIG_LWIP_LOOPBACK_MAX_PBUFS=8
+
+#
+# TCP
+#
+CONFIG_LWIP_MAX_ACTIVE_TCP=16
+CONFIG_LWIP_MAX_LISTENING_TCP=16
+CONFIG_TCP_MAXRTX=12
+CONFIG_TCP_SYNMAXRTX=6
+CONFIG_TCP_MSS=1436
+CONFIG_TCP_MSL=60000
+CONFIG_TCP_SND_BUF_DEFAULT=5744
+CONFIG_TCP_WND_DEFAULT=5744
+CONFIG_TCP_RECVMBOX_SIZE=6
+CONFIG_TCP_QUEUE_OOSEQ=y
+CONFIG_ESP_TCP_KEEP_CONNECTION_WHEN_IP_CHANGES=
+CONFIG_TCP_OVERSIZE_MSS=y
+CONFIG_TCP_OVERSIZE_QUARTER_MSS=
+CONFIG_TCP_OVERSIZE_DISABLE=
+
+#
+# UDP
+#
+CONFIG_LWIP_MAX_UDP_PCBS=16
+CONFIG_UDP_RECVMBOX_SIZE=6
+CONFIG_TCPIP_TASK_STACK_SIZE=2560
+CONFIG_PPP_SUPPORT=
+
+#
+# ICMP
+#
+CONFIG_LWIP_MULTICAST_PING=
+CONFIG_LWIP_BROADCAST_PING=
+
+#
+# LWIP RAW API
+#
+CONFIG_LWIP_MAX_RAW_PCBS=16
+
+#
+# Virtual file system
+#
+CONFIG_SUPPRESS_SELECT_DEBUG_OUTPUT=y
+
+#
+# Ethernet
+#
+CONFIG_DMA_RX_BUF_NUM=10
+CONFIG_DMA_TX_BUF_NUM=10
+CONFIG_EMAC_L2_TO_L3_RX_BUF_MODE=
+CONFIG_EMAC_TASK_PRIORITY=20
+
+#
+# tcpip adapter
+#
+CONFIG_IP_LOST_TIMER_INTERVAL=120
+CONFIG_AWS_IOT_SDK=
+
+#
+# Bluetooth
+#
+CONFIG_BT_ENABLED=
+CONFIG_BTDM_CONTROLLER_PINNED_TO_CORE=0
+CONFIG_BT_RESERVE_DRAM=0
+
+#
+# NVS
+#
+CONFIG_MP_BLOB_SUPPORT=
+
+#
+# ESP32-specific
+#
+CONFIG_ESP32_DEFAULT_CPU_FREQ_80=
+CONFIG_ESP32_DEFAULT_CPU_FREQ_160=
+CONFIG_ESP32_DEFAULT_CPU_FREQ_240=y
+CONFIG_ESP32_DEFAULT_CPU_FREQ_MHZ=240
+CONFIG_SPIRAM_SUPPORT=
+CONFIG_MEMMAP_TRACEMEM=
+CONFIG_MEMMAP_TRACEMEM_TWOBANKS=
+CONFIG_ESP32_TRAX=
+CONFIG_TRACEMEM_RESERVE_DRAM=0x0
+CONFIG_ESP32_ENABLE_COREDUMP_TO_FLASH=
+CONFIG_ESP32_ENABLE_COREDUMP_TO_UART=
+CONFIG_ESP32_ENABLE_COREDUMP_TO_NONE=y
+CONFIG_ESP32_ENABLE_COREDUMP=
+CONFIG_TWO_UNIVERSAL_MAC_ADDRESS=
+CONFIG_FOUR_UNIVERSAL_MAC_ADDRESS=y
+CONFIG_NUMBER_OF_UNIVERSAL_MAC_ADDRESS=4
+CONFIG_SYSTEM_EVENT_QUEUE_SIZE=32
+CONFIG_SYSTEM_EVENT_TASK_STACK_SIZE=2304
+CONFIG_MAIN_TASK_STACK_SIZE=3584
+CONFIG_IPC_TASK_STACK_SIZE=1024
+CONFIG_TIMER_TASK_STACK_SIZE=3584
+CONFIG_NEWLIB_STDOUT_LINE_ENDING_CRLF=y
+CONFIG_NEWLIB_STDOUT_LINE_ENDING_LF=
+CONFIG_NEWLIB_STDOUT_LINE_ENDING_CR=
+CONFIG_NEWLIB_STDIN_LINE_ENDING_CRLF=
+CONFIG_NEWLIB_STDIN_LINE_ENDING_LF=
+CONFIG_NEWLIB_STDIN_LINE_ENDING_CR=y
+CONFIG_NEWLIB_NANO_FORMAT=
+CONFIG_CONSOLE_UART_DEFAULT=y
+CONFIG_CONSOLE_UART_CUSTOM=
+CONFIG_CONSOLE_UART_NONE=
+CONFIG_CONSOLE_UART_NUM=0
+CONFIG_CONSOLE_UART_BAUDRATE=115200
+CONFIG_ULP_COPROC_ENABLED=
+CONFIG_ULP_COPROC_RESERVE_MEM=0
+CONFIG_ESP32_PANIC_PRINT_HALT=
+CONFIG_ESP32_PANIC_PRINT_REBOOT=y
+CONFIG_ESP32_PANIC_SILENT_REBOOT=
+CONFIG_ESP32_PANIC_GDBSTUB=
+CONFIG_ESP32_DEBUG_OCDAWARE=y
+CONFIG_ESP32_DEBUG_STUBS_ENABLE=y
+CONFIG_INT_WDT=y
+CONFIG_INT_WDT_TIMEOUT_MS=300
+CONFIG_INT_WDT_CHECK_CPU1=y
+CONFIG_TASK_WDT=y
+CONFIG_TASK_WDT_PANIC=
+CONFIG_TASK_WDT_TIMEOUT_S=5
+CONFIG_TASK_WDT_CHECK_IDLE_TASK_CPU0=y
+CONFIG_TASK_WDT_CHECK_IDLE_TASK_CPU1=y
+CONFIG_BROWNOUT_DET=y
+CONFIG_BROWNOUT_DET_LVL_SEL_0=y
+CONFIG_BROWNOUT_DET_LVL_SEL_1=
+CONFIG_BROWNOUT_DET_LVL_SEL_2=
+CONFIG_BROWNOUT_DET_LVL_SEL_3=
+CONFIG_BROWNOUT_DET_LVL_SEL_4=
+CONFIG_BROWNOUT_DET_LVL_SEL_5=
+CONFIG_BROWNOUT_DET_LVL_SEL_6=
+CONFIG_BROWNOUT_DET_LVL_SEL_7=
+CONFIG_BROWNOUT_DET_LVL=0
+CONFIG_ESP32_TIME_SYSCALL_USE_RTC_FRC1=y
+CONFIG_ESP32_TIME_SYSCALL_USE_RTC=
+CONFIG_ESP32_TIME_SYSCALL_USE_FRC1=
+CONFIG_ESP32_TIME_SYSCALL_USE_NONE=
+CONFIG_ESP32_RTC_CLOCK_SOURCE_INTERNAL_RC=y
+CONFIG_ESP32_RTC_CLOCK_SOURCE_EXTERNAL_CRYSTAL=
+CONFIG_ESP32_RTC_CLK_CAL_CYCLES=1024
+CONFIG_ESP32_DEEP_SLEEP_WAKEUP_DELAY=2000
+CONFIG_ESP32_XTAL_FREQ_40=y
+CONFIG_ESP32_XTAL_FREQ_26=
+CONFIG_ESP32_XTAL_FREQ_AUTO=
+CONFIG_ESP32_XTAL_FREQ=40
+CONFIG_DISABLE_BASIC_ROM_CONSOLE=
+CONFIG_NO_BLOBS=
+CONFIG_ESP_TIMER_PROFILING=
+CONFIG_COMPATIBLE_PRE_V2_1_BOOTLOADERS=
+CONFIG_ESP_ERR_TO_NAME_LOOKUP=y
+
+#
+# Wi-Fi
+#
+CONFIG_ESP32_WIFI_STATIC_RX_BUFFER_NUM=10
+CONFIG_ESP32_WIFI_DYNAMIC_RX_BUFFER_NUM=32
+CONFIG_ESP32_WIFI_STATIC_TX_BUFFER=
+CONFIG_ESP32_WIFI_DYNAMIC_TX_BUFFER=y
+CONFIG_ESP32_WIFI_TX_BUFFER_TYPE=1
+CONFIG_ESP32_WIFI_DYNAMIC_TX_BUFFER_NUM=32
+CONFIG_ESP32_WIFI_CSI_ENABLED=
+CONFIG_ESP32_WIFI_AMPDU_TX_ENABLED=y
+CONFIG_ESP32_WIFI_TX_BA_WIN=6
+CONFIG_ESP32_WIFI_AMPDU_RX_ENABLED=y
+CONFIG_ESP32_WIFI_RX_BA_WIN=6
+CONFIG_ESP32_WIFI_NVS_ENABLED=y
+CONFIG_ESP32_WIFI_TASK_PINNED_TO_CORE_0=y
+CONFIG_ESP32_WIFI_TASK_PINNED_TO_CORE_1=
+CONFIG_ESP32_WIFI_SOFTAP_BEACON_MAX_LEN=752
+
+#
+# PHY
+#
+CONFIG_ESP32_PHY_CALIBRATION_AND_DATA_STORAGE=y
+CONFIG_ESP32_PHY_INIT_DATA_IN_PARTITION=
+CONFIG_ESP32_PHY_MAX_WIFI_TX_POWER=20
+CONFIG_ESP32_PHY_MAX_TX_POWER=20
+
+#
+# Power Management
+#
+CONFIG_PM_ENABLE=
+
+#
+# Log output
+#
+CONFIG_LOG_DEFAULT_LEVEL_NONE=
+CONFIG_LOG_DEFAULT_LEVEL_ERROR=
+CONFIG_LOG_DEFAULT_LEVEL_WARN=
+CONFIG_LOG_DEFAULT_LEVEL_INFO=
+CONFIG_LOG_DEFAULT_LEVEL_DEBUG=y
+CONFIG_LOG_DEFAULT_LEVEL_VERBOSE=
+CONFIG_LOG_DEFAULT_LEVEL=4
+CONFIG_LOG_COLORS=y
+
+#
+# PThreads
+#
+CONFIG_ESP32_PTHREAD_TASK_PRIO_DEFAULT=5
+CONFIG_ESP32_PTHREAD_TASK_STACK_SIZE_DEFAULT=3072
+
+#
+# ADC-Calibration
+#
+CONFIG_ADC_CAL_EFUSE_TP_ENABLE=y
+CONFIG_ADC_CAL_EFUSE_VREF_ENABLE=y
+CONFIG_ADC_CAL_LUT_ENABLE=y
+
+#
+# ESP HTTP client
+#
+CONFIG_ESP_HTTP_CLIENT_ENABLE_HTTPS=y
+
+#
+# FAT Filesystem support
+#
+CONFIG_FATFS_CODEPAGE_DYNAMIC=
+CONFIG_FATFS_CODEPAGE_437=y
+CONFIG_FATFS_CODEPAGE_720=
+CONFIG_FATFS_CODEPAGE_737=
+CONFIG_FATFS_CODEPAGE_771=
+CONFIG_FATFS_CODEPAGE_775=
+CONFIG_FATFS_CODEPAGE_850=
+CONFIG_FATFS_CODEPAGE_852=
+CONFIG_FATFS_CODEPAGE_855=
+CONFIG_FATFS_CODEPAGE_857=
+CONFIG_FATFS_CODEPAGE_860=
+CONFIG_FATFS_CODEPAGE_861=
+CONFIG_FATFS_CODEPAGE_862=
+CONFIG_FATFS_CODEPAGE_863=
+CONFIG_FATFS_CODEPAGE_864=
+CONFIG_FATFS_CODEPAGE_865=
+CONFIG_FATFS_CODEPAGE_866=
+CONFIG_FATFS_CODEPAGE_869=
+CONFIG_FATFS_CODEPAGE_932=
+CONFIG_FATFS_CODEPAGE_936=
+CONFIG_FATFS_CODEPAGE_949=
+CONFIG_FATFS_CODEPAGE_950=
+CONFIG_FATFS_CODEPAGE=437
+CONFIG_FATFS_LFN_NONE=y
+CONFIG_FATFS_LFN_HEAP=
+CONFIG_FATFS_LFN_STACK=
+CONFIG_FATFS_FS_LOCK=0
+CONFIG_FATFS_TIMEOUT_MS=10000
+CONFIG_FATFS_PER_FILE_CACHE=y
+
+#
+# Wear Levelling
+#
+CONFIG_WL_SECTOR_SIZE_512=
+CONFIG_WL_SECTOR_SIZE_4096=y
+CONFIG_WL_SECTOR_SIZE=4096
+
+#
+# FreeRTOS
+#
+CONFIG_FREERTOS_UNICORE=
+CONFIG_FREERTOS_CORETIMER_0=y
+CONFIG_FREERTOS_CORETIMER_1=
+CONFIG_FREERTOS_HZ=100
+CONFIG_FREERTOS_ASSERT_ON_UNTESTED_FUNCTION=y
+CONFIG_FREERTOS_CHECK_STACKOVERFLOW_NONE=
+CONFIG_FREERTOS_CHECK_STACKOVERFLOW_PTRVAL=
+CONFIG_FREERTOS_CHECK_STACKOVERFLOW_CANARY=y
+CONFIG_FREERTOS_WATCHPOINT_END_OF_STACK=
+CONFIG_FREERTOS_INTERRUPT_BACKTRACE=y
+CONFIG_FREERTOS_THREAD_LOCAL_STORAGE_POINTERS=1
+CONFIG_FREERTOS_ASSERT_FAIL_ABORT=y
+CONFIG_FREERTOS_ASSERT_FAIL_PRINT_CONTINUE=
+CONFIG_FREERTOS_ASSERT_DISABLE=
+CONFIG_FREERTOS_IDLE_TASK_STACKSIZE=1536
+CONFIG_FREERTOS_ISR_STACKSIZE=1536
+CONFIG_FREERTOS_LEGACY_HOOKS=
+CONFIG_FREERTOS_MAX_TASK_NAME_LEN=16
+CONFIG_SUPPORT_STATIC_ALLOCATION=
+CONFIG_TIMER_TASK_PRIORITY=1
+CONFIG_TIMER_TASK_STACK_DEPTH=2048
+CONFIG_TIMER_QUEUE_LENGTH=10
+CONFIG_FREERTOS_QUEUE_REGISTRY_SIZE=0
+CONFIG_FREERTOS_USE_TRACE_FACILITY=
+CONFIG_FREERTOS_GENERATE_RUN_TIME_STATS=
+CONFIG_FREERTOS_DEBUG_INTERNALS=
+
+#
+# Heap memory debugging
+#
+CONFIG_HEAP_POISONING_DISABLED=
+CONFIG_HEAP_POISONING_LIGHT=
+CONFIG_HEAP_POISONING_COMPREHENSIVE=y
+CONFIG_HEAP_TRACING=
+CONFIG_HEAP_TASK_TRACKING=
+
+#
+# libsodium
+#
+CONFIG_LIBSODIUM_USE_MBEDTLS_SHA=y
+
+#
+# OpenSSL
+#
+CONFIG_OPENSSL_DEBUG=
+CONFIG_OPENSSL_ASSERT_DO_NOTHING=y
+CONFIG_OPENSSL_ASSERT_EXIT=
+
+#
+# SPIFFS Configuration
+#
+CONFIG_SPIFFS_MAX_PARTITIONS=3
+
+#
+# SPIFFS Cache Configuration
+#
+CONFIG_SPIFFS_CACHE=y
+CONFIG_SPIFFS_CACHE_WR=y
+CONFIG_SPIFFS_CACHE_STATS=
+CONFIG_SPIFFS_PAGE_CHECK=y
+CONFIG_SPIFFS_GC_MAX_RUNS=10
+CONFIG_SPIFFS_GC_STATS=
+CONFIG_SPIFFS_PAGE_SIZE=256
+CONFIG_SPIFFS_OBJ_NAME_LEN=32
+CONFIG_SPIFFS_USE_MAGIC=y
+CONFIG_SPIFFS_USE_MAGIC_LENGTH=y
+CONFIG_SPIFFS_META_LENGTH=4
+CONFIG_SPIFFS_USE_MTIME=y
+
+#
+# Debug Configuration
+#
+CONFIG_SPIFFS_DBG=
+CONFIG_SPIFFS_API_DBG=
+CONFIG_SPIFFS_GC_DBG=
+CONFIG_SPIFFS_CACHE_DBG=
+CONFIG_SPIFFS_CHECK_DBG=
+CONFIG_SPIFFS_TEST_VISUALISATION=

tools/config.yml

@@ -0,0 +1,12 @@
+REVISION: 1
+angle_pid:
+  kp: 0.04
+  ki: 0.00 #0001
+  kd: 0.4
+  max_output: 0.4
+  max_i_error: 15.0
+
+min_target_dist: 1.0
+min_flip_pitch: 90.0
+drive_power: 0.3
+mag_declination: 11.5

tools/config2c.py

@@ -0,0 +1,43 @@
+#!/usr/bin/env python3
+
+import config_pb2
+import yaml
+
+
+def dict2pb(d, pb):
+    for key in d:
+        val = d[key]
+        if isinstance(val, dict):
+            dict2pb(val, getattr(pb, key))
+        else:
+            setattr(pb, key, val)
+
+
+def main():
+    with open('./tools/config.yml', 'r') as configfile:
+        config = yaml.load(configfile)
+
+    if 'REVISION' in config:
+        config_rev = config['REVISION']
+        del config['REVISION']
+    else:
+        config_rev = 1
+
+    confpb = config_pb2.Config()
+    dict2pb(config, confpb)
+
+    pbdata = confpb.SerializeToString()
+
+    pbdataarry = ','.join([str(int(b)) for b in pbdata])
+
+    cfile = """#include <stdint.h>
+
+uint8_t CONFIG_DATA[] = {%s};
+size_t CONFIG_DATA_LEN = %s;
+int CONFIG_REV = %s;""" % (pbdataarry, len(pbdata), int(config_rev))
+
+    print(cfile)
+
+
+if __name__ == "__main__":
+    main()

tools/config_pb2.py

@@ -0,0 +1,167 @@
+# -*- coding: utf-8 -*-
+# Generated by the protocol buffer compiler.  DO NOT EDIT!
+# source: config.proto
+
+import sys
+_b=sys.version_info[0]<3 and (lambda x:x) or (lambda x:x.encode('latin1'))
+from google.protobuf import descriptor as _descriptor
+from google.protobuf import message as _message
+from google.protobuf import reflection as _reflection
+from google.protobuf import symbol_database as _symbol_database
+# @@protoc_insertion_point(imports)
+
+_sym_db = _symbol_database.Default()
+
+
+
+
+DESCRIPTOR = _descriptor.FileDescriptor(
+  name='config.proto',
+  package='ugv.config',
+  syntax='proto3',
+  serialized_options=_b('H\003'),
+  serialized_pb=_b('\n\x0c\x63onfig.proto\x12\nugv.config\"X\n\tPidParams\x12\n\n\x02kp\x18\x01 \x01(\x02\x12\n\n\x02ki\x18\x02 \x01(\x02\x12\n\n\x02kd\x18\x03 \x01(\x02\x12\x12\n\nmax_output\x18\x04 \x01(\x02\x12\x13\n\x0bmax_i_error\x18\x05 \x01(\x02\"\x91\x01\n\x06\x43onfig\x12\x17\n\x0fmin_target_dist\x18\x01 \x01(\x02\x12\x13\n\x0b\x64rive_power\x18\x02 \x01(\x02\x12(\n\tangle_pid\x18\x03 \x01(\x0b\x32\x15.ugv.config.PidParams\x12\x16\n\x0emin_flip_pitch\x18\x04 \x01(\x02\x12\x17\n\x0fmag_declination\x18\x05 \x01(\x02\x42\x02H\x03\x62\x06proto3')
+)
+
+
+
+
+_PIDPARAMS = _descriptor.Descriptor(
+  name='PidParams',
+  full_name='ugv.config.PidParams',
+  filename=None,
+  file=DESCRIPTOR,
+  containing_type=None,
+  fields=[
+    _descriptor.FieldDescriptor(
+      name='kp', full_name='ugv.config.PidParams.kp', index=0,
+      number=1, type=2, cpp_type=6, label=1,
+      has_default_value=False, default_value=float(0),
+      message_type=None, enum_type=None, containing_type=None,
+      is_extension=False, extension_scope=None,
+      serialized_options=None, file=DESCRIPTOR),
+    _descriptor.FieldDescriptor(
+      name='ki', full_name='ugv.config.PidParams.ki', index=1,
+      number=2, type=2, cpp_type=6, label=1,
+      has_default_value=False, default_value=float(0),
+      message_type=None, enum_type=None, containing_type=None,
+      is_extension=False, extension_scope=None,
+      serialized_options=None, file=DESCRIPTOR),
+    _descriptor.FieldDescriptor(
+      name='kd', full_name='ugv.config.PidParams.kd', index=2,
+      number=3, type=2, cpp_type=6, label=1,
+      has_default_value=False, default_value=float(0),
+      message_type=None, enum_type=None, containing_type=None,
+      is_extension=False, extension_scope=None,
+      serialized_options=None, file=DESCRIPTOR),
+    _descriptor.FieldDescriptor(
+      name='max_output', full_name='ugv.config.PidParams.max_output', index=3,
+      number=4, type=2, cpp_type=6, label=1,
+      has_default_value=False, default_value=float(0),
+      message_type=None, enum_type=None, containing_type=None,
+      is_extension=False, extension_scope=None,
+      serialized_options=None, file=DESCRIPTOR),
+    _descriptor.FieldDescriptor(
+      name='max_i_error', full_name='ugv.config.PidParams.max_i_error', index=4,
+      number=5, type=2, cpp_type=6, label=1,
+      has_default_value=False, default_value=float(0),
+      message_type=None, enum_type=None, containing_type=None,
+      is_extension=False, extension_scope=None,
+      serialized_options=None, file=DESCRIPTOR),
+  ],
+  extensions=[
+  ],
+  nested_types=[],
+  enum_types=[
+  ],
+  serialized_options=None,
+  is_extendable=False,
+  syntax='proto3',
+  extension_ranges=[],
+  oneofs=[
+  ],
+  serialized_start=28,
+  serialized_end=116,
+)
+
+
+_CONFIG = _descriptor.Descriptor(
+  name='Config',
+  full_name='ugv.config.Config',
+  filename=None,
+  file=DESCRIPTOR,
+  containing_type=None,
+  fields=[
+    _descriptor.FieldDescriptor(
+      name='min_target_dist', full_name='ugv.config.Config.min_target_dist', index=0,
+      number=1, type=2, cpp_type=6, label=1,
+      has_default_value=False, default_value=float(0),
+      message_type=None, enum_type=None, containing_type=None,
+      is_extension=False, extension_scope=None,
+      serialized_options=None, file=DESCRIPTOR),
+    _descriptor.FieldDescriptor(
+      name='drive_power', full_name='ugv.config.Config.drive_power', index=1,
+      number=2, type=2, cpp_type=6, label=1,
+      has_default_value=False, default_value=float(0),
+      message_type=None, enum_type=None, containing_type=None,
+      is_extension=False, extension_scope=None,
+      serialized_options=None, file=DESCRIPTOR),
+    _descriptor.FieldDescriptor(
+      name='angle_pid', full_name='ugv.config.Config.angle_pid', index=2,
+      number=3, type=11, cpp_type=10, label=1,
+      has_default_value=False, default_value=None,
+      message_type=None, enum_type=None, containing_type=None,
+      is_extension=False, extension_scope=None,
+      serialized_options=None, file=DESCRIPTOR),
+    _descriptor.FieldDescriptor(
+      name='min_flip_pitch', full_name='ugv.config.Config.min_flip_pitch', index=3,
+      number=4, type=2, cpp_type=6, label=1,
+      has_default_value=False, default_value=float(0),
+      message_type=None, enum_type=None, containing_type=None,
+      is_extension=False, extension_scope=None,
+      serialized_options=None, file=DESCRIPTOR),
+    _descriptor.FieldDescriptor(
+      name='mag_declination', full_name='ugv.config.Config.mag_declination', index=4,
+      number=5, type=2, cpp_type=6, label=1,
+      has_default_value=False, default_value=float(0),
+      message_type=None, enum_type=None, containing_type=None,
+      is_extension=False, extension_scope=None,
+      serialized_options=None, file=DESCRIPTOR),
+  ],
+  extensions=[
+  ],
+  nested_types=[],
+  enum_types=[
+  ],
+  serialized_options=None,
+  is_extendable=False,
+  syntax='proto3',
+  extension_ranges=[],
+  oneofs=[
+  ],
+  serialized_start=119,
+  serialized_end=264,
+)
+
+_CONFIG.fields_by_name['angle_pid'].message_type = _PIDPARAMS
+DESCRIPTOR.message_types_by_name['PidParams'] = _PIDPARAMS
+DESCRIPTOR.message_types_by_name['Config'] = _CONFIG
+_sym_db.RegisterFileDescriptor(DESCRIPTOR)
+
+PidParams = _reflection.GeneratedProtocolMessageType('PidParams', (_message.Message,), dict(
+  DESCRIPTOR = _PIDPARAMS,
+  __module__ = 'config_pb2'
+  # @@protoc_insertion_point(class_scope:ugv.config.PidParams)
+  ))
+_sym_db.RegisterMessage(PidParams)
+
+Config = _reflection.GeneratedProtocolMessageType('Config', (_message.Message,), dict(
+  DESCRIPTOR = _CONFIG,
+  __module__ = 'config_pb2'
+  # @@protoc_insertion_point(class_scope:ugv.config.Config)
+  ))
+_sym_db.RegisterMessage(Config)
+
+
+DESCRIPTOR._options = None
+# @@protoc_insertion_point(module_scope)

tools/e32.py

@@ -0,0 +1,167 @@
+#!/usr/bin/env python3
+
+import serial
+import struct
+
+PARITY_NONE = 0
+PARITY_ODD = 1
+PARITY_EVEN = 2
+
+BAUD_TABLE = {
+    0: 1200,
+    1: 2400,
+    2: 4800,
+    3: 9600,
+    4: 19200,
+    5: 38400,
+    6: 57600,
+    7: 115200,
+}
+
+AIR_DATA_RATE_TABLE = {
+    0: 300,
+    1: 1200,
+    2: 2400,
+    3: 4800,
+    4: 9600,
+    5: 19200,
+    6: 19200,
+    7: 19200,
+}
+
+TX_MODE_TRANSPARENT = 0
+TX_MODE_FIXED = 1
+
+IO_MODE_OPEN_COLLECTOR = 0
+IO_MODE_PUSH_PULL = 1
+
+TX_POWER_TABLE = {
+    0: 30,
+    1: 27,
+    2: 24,
+    3: 21,
+}
+
+
+def least_gte(value, dic):
+    items = sorted(dic.items(), key=lambda item: item[1])
+    last_k = None
+    for k, v in items:
+        last_k = k
+        if v >= value:
+            break
+    return last_k
+
+
+class E32_Params:
+    save: bool
+    address: int
+    parity: int
+    baud: int
+    air_data_rate: int
+    channel: int
+    tx_mode: int
+    io_mode: int
+    wake_up_time: int
+    fec_enabled: bool
+    tx_power: int
+
+    @staticmethod
+    def default():
+        p = E32_Params()
+        p.save = True
+        p.address = 0
+        p.parity = PARITY_NONE
+        p.baud = 9600
+        p.air_data_rate = 2400
+        p.channel = 0x17
+        p.tx_mode = TX_MODE_TRANSPARENT
+        p.io_mode = IO_MODE_PUSH_PULL
+        p.wake_up_time = 250
+        p.fec_enabled = True
+        p.tx_power = 30
+        return p
+
+    @staticmethod
+    def unpack(data):
+        p = E32_Params()
+        datab = bytes(data)
+        if len(datab) != 6:
+            raise Exception('invalid E32_Params data length')
+        if datab[0] == 0xC0:
+            p.save = True
+        elif datab[0] == 0xC2:
+            p.save = False
+        else:
+            raise Exception('invalid E32_Params data header')
+        p.address = (datab[1] << 8) | datab[2]
+        p.parity = (datab[3] >> 6) & 0b11
+        p.baud = BAUD_TABLE[(datab[3] >> 3) & 0b111]
+        p.air_data_rate = AIR_DATA_RATE_TABLE[datab[3] & 0b111]
+        p.channel = datab[4]
+        p.tx_mode = (datab[5] >> 7) & 1
+        p.io_mode = (datab[5] >> 6) & 1
+        p.wake_up_time = 250 * (((datab[5] >> 3) & 0b111) + 1)
+        p.fec_enabled = (datab[5] & (1 << 2)) != 0
+        p.tx_power = TX_POWER_TABLE[datab[5] & 0b11]
+        return p
+
+    def pack(self):
+        p = self
+        datab = bytearray(range(6))
+        if p.save:
+            datab[0] = 0xC0
+        else:
+            datab[0] = 0xC2
+        datab[1] = (p.address >> 8) & 0xFF
+        datab[2] = (p.address) & 0xFF
+        datab[3] = 0
+        datab[3] |= p.parity << 6
+        datab[3] |= (least_gte(p.baud, BAUD_TABLE) << 3)
+        datab[3] |= (least_gte(p.air_data_rate, AIR_DATA_RATE_TABLE))
+        datab[4] = p.channel
+        datab[5] = 0
+        datab[5] |= p.tx_mode << 7
+        datab[5] |= p.io_mode << 6
+        datab[5] |= (int((p.wake_up_time / 250) - 1) & 0b111) << 3
+        datab[5] |= p.fec_enabled << 2
+        datab[5] |= least_gte(p.tx_power, TX_POWER_TABLE)
+        return datab
+
+
+class E32:
+    ser: serial.Serial
+
+    def __init__(self, serial_port: serial.Serial):
+        self.ser = serial_port
+
+    def close(self):
+        self.ser.close()
+
+    def read_version(self):
+        # self.ser.flush()
+        self.ser.write(b'\xC3\xC3\xC3')
+        version = self.ser.read(size=4)
+        print("version: ", version)
+        return version
+
+    def reset(self):
+        # self.ser.flush()
+        print("writing: ", b'\xC4\xC4\xC4')
+        self.ser.write(b'0xC40xC40xC4')
+
+    def read_params(self):
+        # self.ser.flush()
+        self.ser.write(b'\xC1\xC1\xC1')
+        param_bytes = self.ser.read(size=6)
+        print("param_bytes: ", param_bytes)
+        return E32_Params.unpack(param_bytes)
+
+
+if __name__ == "__main__":
+    p = E32_Params.default()
+    print("params: ", p.__dict__)
+    data = p.pack()
+    print("packed data: ", ', '.join(format(x, '02x') for x in data))
+    p2 = E32_Params.unpack(data)
+    print("unpacked params: ", p2.__dict__)

tools/messages_pb2.py

@@ -0,0 +1,562 @@
+# -*- coding: utf-8 -*-
+# Generated by the protocol buffer compiler.  DO NOT EDIT!
+# source: messages.proto
+
+import sys
+_b=sys.version_info[0]<3 and (lambda x:x) or (lambda x:x.encode('latin1'))
+from google.protobuf.internal import enum_type_wrapper
+from google.protobuf import descriptor as _descriptor
+from google.protobuf import message as _message
+from google.protobuf import reflection as _reflection
+from google.protobuf import symbol_database as _symbol_database
+# @@protoc_insertion_point(imports)
+
+_sym_db = _symbol_database.Default()
+
+
+import config_pb2 as config__pb2
+
+from config_pb2 import *
+
+DESCRIPTOR = _descriptor.FileDescriptor(
+  name='messages.proto',
+  package='ugv.messages',
+  syntax='proto3',
+  serialized_options=_b('H\003'),
+  serialized_pb=_b('\n\x0emessages.proto\x12\x0cugv.messages\x1a\x0c\x63onfig.proto\"5\n\x0eTargetLocation\x12\x10\n\x08latitude\x18\x01 \x01(\x02\x12\x11\n\tlongitude\x18\x02 \x01(\x02\"V\n\x08Location\x12\x13\n\x0b\x66ix_quality\x18\x01 \x01(\r\x12\x10\n\x08latitude\x18\x02 \x01(\x02\x12\x11\n\tlongitude\x18\x03 \x01(\x02\x12\x10\n\x08\x61ltitude\x18\x04 \x01(\x02\"\xac\x01\n\nUGV_Status\x12&\n\x05state\x18\x01 \x01(\x0e\x32\x17.ugv.messages.UGV_State\x12(\n\x08location\x18\x02 \x01(\x0b\x32\x16.ugv.messages.Location\x12\x11\n\tyaw_angle\x18\x03 \x01(\x02\x12\x13\n\x0bpitch_angle\x18\x04 \x01(\x02\x12\x12\n\nroll_angle\x18\x05 \x01(\x02\x12\x10\n\x08is_still\x18\x06 \x01(\x08\"_\n\x0bUGV_Message\x12*\n\x06status\x18\x01 \x01(\x0b\x32\x18.ugv.messages.UGV_StatusH\x00\x12\x15\n\x0b\x63ommand_ack\x18\x02 \x01(\rH\x00\x42\r\n\x0bugv_message\"2\n\x10\x44riveHeadingData\x12\x0f\n\x07heading\x18\x01 \x01(\x02\x12\r\n\x05power\x18\x02 \x01(\x02\"\xea\x01\n\rGroundCommand\x12\n\n\x02id\x18\x01 \x01(\r\x12-\n\x04type\x18\x02 \x01(\x0e\x32\x1f.ugv.messages.GroundCommandType\x12\x37\n\rdrive_heading\x18\x03 \x01(\x0b\x32\x1e.ugv.messages.DriveHeadingDataH\x00\x12\x37\n\x0ftarget_location\x18\x04 \x01(\x0b\x32\x1c.ugv.messages.TargetLocationH\x00\x12$\n\x06\x63onfig\x18\x05 \x01(\x0b\x32\x12.ugv.config.ConfigH\x00\x42\x06\n\x04\x64\x61ta\"Q\n\rGroundMessage\x12.\n\x07\x63ommand\x18\x01 \x01(\x0b\x32\x1b.ugv.messages.GroundCommandH\x00\x42\x10\n\x0eground_message*\xa6\x01\n\tUGV_State\x12\x0e\n\nSTATE_IDLE\x10\x00\x12\x12\n\x0eSTATE_AQUIRING\x10\x01\x12\x11\n\rSTATE_DRIVING\x10\x02\x12\x12\n\x0eSTATE_FINISHED\x10\x03\x12\x0e\n\nSTATE_TEST\x10\x04\x12\x12\n\x0eSTATE_FLIPPING\x10\x05\x12\x11\n\rSTATE_TURNING\x10\x06\x12\x17\n\x13STATE_DRIVE_HEADING\x10\x07*\xac\x01\n\x11GroundCommandType\x12\x0f\n\x0b\x43MD_DISABLE\x10\x00\x12\x17\n\x13\x43MD_DRIVE_TO_TARGET\x10\x01\x12\x0c\n\x08\x43MD_TEST\x10\x02\x12\x15\n\x11\x43MD_DRIVE_HEADING\x10\x03\x12\x12\n\x0e\x43MD_SET_TARGET\x10\x04\x12\x12\n\x0e\x43MD_SET_CONFIG\x10\x05\x12\x12\n\x0e\x43MD_GET_STATUS\x10\x06\x12\x0c\n\x08\x43MD_PING\x10\x07\x42\x02H\x03P\x00\x62\x06proto3')
+  ,
+  dependencies=[config__pb2.DESCRIPTOR,],
+  public_dependencies=[config__pb2.DESCRIPTOR,])
+
+_UGV_STATE = _descriptor.EnumDescriptor(
+  name='UGV_State',
+  full_name='ugv.messages.UGV_State',
+  filename=None,
+  file=DESCRIPTOR,
+  values=[
+    _descriptor.EnumValueDescriptor(
+      name='STATE_IDLE', index=0, number=0,
+      serialized_options=None,
+      type=None),
+    _descriptor.EnumValueDescriptor(
+      name='STATE_AQUIRING', index=1, number=1,
+      serialized_options=None,
+      type=None),
+    _descriptor.EnumValueDescriptor(
+      name='STATE_DRIVING', index=2, number=2,
+      serialized_options=None,
+      type=None),
+    _descriptor.EnumValueDescriptor(
+      name='STATE_FINISHED', index=3, number=3,
+      serialized_options=None,
+      type=None),
+    _descriptor.EnumValueDescriptor(
+      name='STATE_TEST', index=4, number=4,
+      serialized_options=None,
+      type=None),
+    _descriptor.EnumValueDescriptor(
+      name='STATE_FLIPPING', index=5, number=5,
+      serialized_options=None,
+      type=None),
+    _descriptor.EnumValueDescriptor(
+      name='STATE_TURNING', index=6, number=6,
+      serialized_options=None,
+      type=None),
+    _descriptor.EnumValueDescriptor(
+      name='STATE_DRIVE_HEADING', index=7, number=7,
+      serialized_options=None,
+      type=None),
+  ],
+  containing_type=None,
+  serialized_options=None,
+  serialized_start=834,
+  serialized_end=1000,
+)
+_sym_db.RegisterEnumDescriptor(_UGV_STATE)
+
+UGV_State = enum_type_wrapper.EnumTypeWrapper(_UGV_STATE)
+_GROUNDCOMMANDTYPE = _descriptor.EnumDescriptor(
+  name='GroundCommandType',
+  full_name='ugv.messages.GroundCommandType',
+  filename=None,
+  file=DESCRIPTOR,
+  values=[
+    _descriptor.EnumValueDescriptor(
+      name='CMD_DISABLE', index=0, number=0,
+      serialized_options=None,
+      type=None),
+    _descriptor.EnumValueDescriptor(
+      name='CMD_DRIVE_TO_TARGET', index=1, number=1,
+      serialized_options=None,
+      type=None),
+    _descriptor.EnumValueDescriptor(
+      name='CMD_TEST', index=2, number=2,
+      serialized_options=None,
+      type=None),
+    _descriptor.EnumValueDescriptor(
+      name='CMD_DRIVE_HEADING', index=3, number=3,
+      serialized_options=None,
+      type=None),
+    _descriptor.EnumValueDescriptor(
+      name='CMD_SET_TARGET', index=4, number=4,
+      serialized_options=None,
+      type=None),
+    _descriptor.EnumValueDescriptor(
+      name='CMD_SET_CONFIG', index=5, number=5,
+      serialized_options=None,
+      type=None),
+    _descriptor.EnumValueDescriptor(
+      name='CMD_GET_STATUS', index=6, number=6,
+      serialized_options=None,
+      type=None),
+    _descriptor.EnumValueDescriptor(
+      name='CMD_PING', index=7, number=7,
+      serialized_options=None,
+      type=None),
+  ],
+  containing_type=None,
+  serialized_options=None,
+  serialized_start=1003,
+  serialized_end=1175,
+)
+_sym_db.RegisterEnumDescriptor(_GROUNDCOMMANDTYPE)
+
+GroundCommandType = enum_type_wrapper.EnumTypeWrapper(_GROUNDCOMMANDTYPE)
+STATE_IDLE = 0
+STATE_AQUIRING = 1
+STATE_DRIVING = 2
+STATE_FINISHED = 3
+STATE_TEST = 4
+STATE_FLIPPING = 5
+STATE_TURNING = 6
+STATE_DRIVE_HEADING = 7
+CMD_DISABLE = 0
+CMD_DRIVE_TO_TARGET = 1
+CMD_TEST = 2
+CMD_DRIVE_HEADING = 3
+CMD_SET_TARGET = 4
+CMD_SET_CONFIG = 5
+CMD_GET_STATUS = 6
+CMD_PING = 7
+
+
+
+_TARGETLOCATION = _descriptor.Descriptor(
+  name='TargetLocation',
+  full_name='ugv.messages.TargetLocation',
+  filename=None,
+  file=DESCRIPTOR,
+  containing_type=None,
+  fields=[
+    _descriptor.FieldDescriptor(
+      name='latitude', full_name='ugv.messages.TargetLocation.latitude', index=0,
+      number=1, type=2, cpp_type=6, label=1,
+      has_default_value=False, default_value=float(0),
+      message_type=None, enum_type=None, containing_type=None,
+      is_extension=False, extension_scope=None,
+      serialized_options=None, file=DESCRIPTOR),
+    _descriptor.FieldDescriptor(
+      name='longitude', full_name='ugv.messages.TargetLocation.longitude', index=1,
+      number=2, type=2, cpp_type=6, label=1,
+      has_default_value=False, default_value=float(0),
+      message_type=None, enum_type=None, containing_type=None,
+      is_extension=False, extension_scope=None,
+      serialized_options=None, file=DESCRIPTOR),
+  ],
+  extensions=[
+  ],
+  nested_types=[],
+  enum_types=[
+  ],
+  serialized_options=None,
+  is_extendable=False,
+  syntax='proto3',
+  extension_ranges=[],
+  oneofs=[
+  ],
+  serialized_start=46,
+  serialized_end=99,
+)
+
+
+_LOCATION = _descriptor.Descriptor(
+  name='Location',
+  full_name='ugv.messages.Location',
+  filename=None,
+  file=DESCRIPTOR,
+  containing_type=None,
+  fields=[
+    _descriptor.FieldDescriptor(
+      name='fix_quality', full_name='ugv.messages.Location.fix_quality', index=0,
+      number=1, type=13, cpp_type=3, label=1,
+      has_default_value=False, default_value=0,
+      message_type=None, enum_type=None, containing_type=None,
+      is_extension=False, extension_scope=None,
+      serialized_options=None, file=DESCRIPTOR),
+    _descriptor.FieldDescriptor(
+      name='latitude', full_name='ugv.messages.Location.latitude', index=1,
+      number=2, type=2, cpp_type=6, label=1,
+      has_default_value=False, default_value=float(0),
+      message_type=None, enum_type=None, containing_type=None,
+      is_extension=False, extension_scope=None,
+      serialized_options=None, file=DESCRIPTOR),
+    _descriptor.FieldDescriptor(
+      name='longitude', full_name='ugv.messages.Location.longitude', index=2,
+      number=3, type=2, cpp_type=6, label=1,
+      has_default_value=False, default_value=float(0),
+      message_type=None, enum_type=None, containing_type=None,
+      is_extension=False, extension_scope=None,
+      serialized_options=None, file=DESCRIPTOR),
+    _descriptor.FieldDescriptor(
+      name='altitude', full_name='ugv.messages.Location.altitude', index=3,
+      number=4, type=2, cpp_type=6, label=1,
+      has_default_value=False, default_value=float(0),
+      message_type=None, enum_type=None, containing_type=None,
+      is_extension=False, extension_scope=None,
+      serialized_options=None, file=DESCRIPTOR),
+  ],
+  extensions=[
+  ],
+  nested_types=[],
+  enum_types=[
+  ],
+  serialized_options=None,
+  is_extendable=False,
+  syntax='proto3',
+  extension_ranges=[],
+  oneofs=[
+  ],
+  serialized_start=101,
+  serialized_end=187,
+)
+
+
+_UGV_STATUS = _descriptor.Descriptor(
+  name='UGV_Status',
+  full_name='ugv.messages.UGV_Status',
+  filename=None,
+  file=DESCRIPTOR,
+  containing_type=None,
+  fields=[
+    _descriptor.FieldDescriptor(
+      name='state', full_name='ugv.messages.UGV_Status.state', index=0,
+      number=1, type=14, cpp_type=8, label=1,
+      has_default_value=False, default_value=0,
+      message_type=None, enum_type=None, containing_type=None,
+      is_extension=False, extension_scope=None,
+      serialized_options=None, file=DESCRIPTOR),
+    _descriptor.FieldDescriptor(
+      name='location', full_name='ugv.messages.UGV_Status.location', index=1,
+      number=2, type=11, cpp_type=10, label=1,
+      has_default_value=False, default_value=None,
+      message_type=None, enum_type=None, containing_type=None,
+      is_extension=False, extension_scope=None,
+      serialized_options=None, file=DESCRIPTOR),
+    _descriptor.FieldDescriptor(
+      name='yaw_angle', full_name='ugv.messages.UGV_Status.yaw_angle', index=2,
+      number=3, type=2, cpp_type=6, label=1,
+      has_default_value=False, default_value=float(0),
+      message_type=None, enum_type=None, containing_type=None,
+      is_extension=False, extension_scope=None,
+      serialized_options=None, file=DESCRIPTOR),
+    _descriptor.FieldDescriptor(
+      name='pitch_angle', full_name='ugv.messages.UGV_Status.pitch_angle', index=3,
+      number=4, type=2, cpp_type=6, label=1,
+      has_default_value=False, default_value=float(0),
+      message_type=None, enum_type=None, containing_type=None,
+      is_extension=False, extension_scope=None,
+      serialized_options=None, file=DESCRIPTOR),
+    _descriptor.FieldDescriptor(
+      name='roll_angle', full_name='ugv.messages.UGV_Status.roll_angle', index=4,
+      number=5, type=2, cpp_type=6, label=1,
+      has_default_value=False, default_value=float(0),
+      message_type=None, enum_type=None, containing_type=None,
+      is_extension=False, extension_scope=None,
+      serialized_options=None, file=DESCRIPTOR),
+    _descriptor.FieldDescriptor(
+      name='is_still', full_name='ugv.messages.UGV_Status.is_still', index=5,
+      number=6, type=8, cpp_type=7, label=1,
+      has_default_value=False, default_value=False,
+      message_type=None, enum_type=None, containing_type=None,
+      is_extension=False, extension_scope=None,
+      serialized_options=None, file=DESCRIPTOR),
+  ],
+  extensions=[
+  ],
+  nested_types=[],
+  enum_types=[
+  ],
+  serialized_options=None,
+  is_extendable=False,
+  syntax='proto3',
+  extension_ranges=[],
+  oneofs=[
+  ],
+  serialized_start=190,
+  serialized_end=362,
+)
+
+
+_UGV_MESSAGE = _descriptor.Descriptor(
+  name='UGV_Message',
+  full_name='ugv.messages.UGV_Message',
+  filename=None,
+  file=DESCRIPTOR,
+  containing_type=None,
+  fields=[
+    _descriptor.FieldDescriptor(
+      name='status', full_name='ugv.messages.UGV_Message.status', index=0,
+      number=1, type=11, cpp_type=10, label=1,
+      has_default_value=False, default_value=None,
+      message_type=None, enum_type=None, containing_type=None,
+      is_extension=False, extension_scope=None,
+      serialized_options=None, file=DESCRIPTOR),
+    _descriptor.FieldDescriptor(
+      name='command_ack', full_name='ugv.messages.UGV_Message.command_ack', index=1,
+      number=2, type=13, cpp_type=3, label=1,
+      has_default_value=False, default_value=0,
+      message_type=None, enum_type=None, containing_type=None,
+      is_extension=False, extension_scope=None,
+      serialized_options=None, file=DESCRIPTOR),
+  ],
+  extensions=[
+  ],
+  nested_types=[],
+  enum_types=[
+  ],
+  serialized_options=None,
+  is_extendable=False,
+  syntax='proto3',
+  extension_ranges=[],
+  oneofs=[
+    _descriptor.OneofDescriptor(
+      name='ugv_message', full_name='ugv.messages.UGV_Message.ugv_message',
+      index=0, containing_type=None, fields=[]),
+  ],
+  serialized_start=364,
+  serialized_end=459,
+)
+
+
+_DRIVEHEADINGDATA = _descriptor.Descriptor(
+  name='DriveHeadingData',
+  full_name='ugv.messages.DriveHeadingData',
+  filename=None,
+  file=DESCRIPTOR,
+  containing_type=None,
+  fields=[
+    _descriptor.FieldDescriptor(
+      name='heading', full_name='ugv.messages.DriveHeadingData.heading', index=0,
+      number=1, type=2, cpp_type=6, label=1,
+      has_default_value=False, default_value=float(0),
+      message_type=None, enum_type=None, containing_type=None,
+      is_extension=False, extension_scope=None,
+      serialized_options=None, file=DESCRIPTOR),
+    _descriptor.FieldDescriptor(
+      name='power', full_name='ugv.messages.DriveHeadingData.power', index=1,
+      number=2, type=2, cpp_type=6, label=1,
+      has_default_value=False, default_value=float(0),
+      message_type=None, enum_type=None, containing_type=None,
+      is_extension=False, extension_scope=None,
+      serialized_options=None, file=DESCRIPTOR),
+  ],
+  extensions=[
+  ],
+  nested_types=[],
+  enum_types=[
+  ],
+  serialized_options=None,
+  is_extendable=False,
+  syntax='proto3',
+  extension_ranges=[],
+  oneofs=[
+  ],
+  serialized_start=461,
+  serialized_end=511,
+)
+
+
+_GROUNDCOMMAND = _descriptor.Descriptor(
+  name='GroundCommand',
+  full_name='ugv.messages.GroundCommand',
+  filename=None,
+  file=DESCRIPTOR,
+  containing_type=None,
+  fields=[
+    _descriptor.FieldDescriptor(
+      name='id', full_name='ugv.messages.GroundCommand.id', index=0,
+      number=1, type=13, cpp_type=3, label=1,
+      has_default_value=False, default_value=0,
+      message_type=None, enum_type=None, containing_type=None,
+      is_extension=False, extension_scope=None,
+      serialized_options=None, file=DESCRIPTOR),
+    _descriptor.FieldDescriptor(
+      name='type', full_name='ugv.messages.GroundCommand.type', index=1,
+      number=2, type=14, cpp_type=8, label=1,
+      has_default_value=False, default_value=0,
+      message_type=None, enum_type=None, containing_type=None,
+      is_extension=False, extension_scope=None,
+      serialized_options=None, file=DESCRIPTOR),
+    _descriptor.FieldDescriptor(
+      name='drive_heading', full_name='ugv.messages.GroundCommand.drive_heading', index=2,
+      number=3, type=11, cpp_type=10, label=1,
+      has_default_value=False, default_value=None,
+      message_type=None, enum_type=None, containing_type=None,
+      is_extension=False, extension_scope=None,
+      serialized_options=None, file=DESCRIPTOR),
+    _descriptor.FieldDescriptor(
+      name='target_location', full_name='ugv.messages.GroundCommand.target_location', index=3,
+      number=4, type=11, cpp_type=10, label=1,
+      has_default_value=False, default_value=None,
+      message_type=None, enum_type=None, containing_type=None,
+      is_extension=False, extension_scope=None,
+      serialized_options=None, file=DESCRIPTOR),
+    _descriptor.FieldDescriptor(
+      name='config', full_name='ugv.messages.GroundCommand.config', index=4,
+      number=5, type=11, cpp_type=10, label=1,
+      has_default_value=False, default_value=None,
+      message_type=None, enum_type=None, containing_type=None,
+      is_extension=False, extension_scope=None,
+      serialized_options=None, file=DESCRIPTOR),
+  ],
+  extensions=[
+  ],
+  nested_types=[],
+  enum_types=[
+  ],
+  serialized_options=None,
+  is_extendable=False,
+  syntax='proto3',
+  extension_ranges=[],
+  oneofs=[
+    _descriptor.OneofDescriptor(
+      name='data', full_name='ugv.messages.GroundCommand.data',
+      index=0, containing_type=None, fields=[]),
+  ],
+  serialized_start=514,
+  serialized_end=748,
+)
+
+
+_GROUNDMESSAGE = _descriptor.Descriptor(
+  name='GroundMessage',
+  full_name='ugv.messages.GroundMessage',
+  filename=None,
+  file=DESCRIPTOR,
+  containing_type=None,
+  fields=[
+    _descriptor.FieldDescriptor(
+      name='command', full_name='ugv.messages.GroundMessage.command', index=0,
+      number=1, type=11, cpp_type=10, label=1,
+      has_default_value=False, default_value=None,
+      message_type=None, enum_type=None, containing_type=None,
+      is_extension=False, extension_scope=None,
+      serialized_options=None, file=DESCRIPTOR),
+  ],
+  extensions=[
+  ],
+  nested_types=[],
+  enum_types=[
+  ],
+  serialized_options=None,
+  is_extendable=False,
+  syntax='proto3',
+  extension_ranges=[],
+  oneofs=[
+    _descriptor.OneofDescriptor(
+      name='ground_message', full_name='ugv.messages.GroundMessage.ground_message',
+      index=0, containing_type=None, fields=[]),
+  ],
+  serialized_start=750,
+  serialized_end=831,
+)
+
+_UGV_STATUS.fields_by_name['state'].enum_type = _UGV_STATE
+_UGV_STATUS.fields_by_name['location'].message_type = _LOCATION
+_UGV_MESSAGE.fields_by_name['status'].message_type = _UGV_STATUS
+_UGV_MESSAGE.oneofs_by_name['ugv_message'].fields.append(
+  _UGV_MESSAGE.fields_by_name['status'])
+_UGV_MESSAGE.fields_by_name['status'].containing_oneof = _UGV_MESSAGE.oneofs_by_name['ugv_message']
+_UGV_MESSAGE.oneofs_by_name['ugv_message'].fields.append(
+  _UGV_MESSAGE.fields_by_name['command_ack'])
+_UGV_MESSAGE.fields_by_name['command_ack'].containing_oneof = _UGV_MESSAGE.oneofs_by_name['ugv_message']
+_GROUNDCOMMAND.fields_by_name['type'].enum_type = _GROUNDCOMMANDTYPE
+_GROUNDCOMMAND.fields_by_name['drive_heading'].message_type = _DRIVEHEADINGDATA
+_GROUNDCOMMAND.fields_by_name['target_location'].message_type = _TARGETLOCATION
+_GROUNDCOMMAND.fields_by_name['config'].message_type = config__pb2._CONFIG
+_GROUNDCOMMAND.oneofs_by_name['data'].fields.append(
+  _GROUNDCOMMAND.fields_by_name['drive_heading'])
+_GROUNDCOMMAND.fields_by_name['drive_heading'].containing_oneof = _GROUNDCOMMAND.oneofs_by_name['data']
+_GROUNDCOMMAND.oneofs_by_name['data'].fields.append(
+  _GROUNDCOMMAND.fields_by_name['target_location'])
+_GROUNDCOMMAND.fields_by_name['target_location'].containing_oneof = _GROUNDCOMMAND.oneofs_by_name['data']
+_GROUNDCOMMAND.oneofs_by_name['data'].fields.append(
+  _GROUNDCOMMAND.fields_by_name['config'])
+_GROUNDCOMMAND.fields_by_name['config'].containing_oneof = _GROUNDCOMMAND.oneofs_by_name['data']
+_GROUNDMESSAGE.fields_by_name['command'].message_type = _GROUNDCOMMAND
+_GROUNDMESSAGE.oneofs_by_name['ground_message'].fields.append(
+  _GROUNDMESSAGE.fields_by_name['command'])
+_GROUNDMESSAGE.fields_by_name['command'].containing_oneof = _GROUNDMESSAGE.oneofs_by_name['ground_message']
+DESCRIPTOR.message_types_by_name['TargetLocation'] = _TARGETLOCATION
+DESCRIPTOR.message_types_by_name['Location'] = _LOCATION
+DESCRIPTOR.message_types_by_name['UGV_Status'] = _UGV_STATUS
+DESCRIPTOR.message_types_by_name['UGV_Message'] = _UGV_MESSAGE
+DESCRIPTOR.message_types_by_name['DriveHeadingData'] = _DRIVEHEADINGDATA
+DESCRIPTOR.message_types_by_name['GroundCommand'] = _GROUNDCOMMAND
+DESCRIPTOR.message_types_by_name['GroundMessage'] = _GROUNDMESSAGE
+DESCRIPTOR.enum_types_by_name['UGV_State'] = _UGV_STATE
+DESCRIPTOR.enum_types_by_name['GroundCommandType'] = _GROUNDCOMMANDTYPE
+_sym_db.RegisterFileDescriptor(DESCRIPTOR)
+
+TargetLocation = _reflection.GeneratedProtocolMessageType('TargetLocation', (_message.Message,), dict(
+  DESCRIPTOR = _TARGETLOCATION,
+  __module__ = 'messages_pb2'
+  # @@protoc_insertion_point(class_scope:ugv.messages.TargetLocation)
+  ))
+_sym_db.RegisterMessage(TargetLocation)
+
+Location = _reflection.GeneratedProtocolMessageType('Location', (_message.Message,), dict(
+  DESCRIPTOR = _LOCATION,
+  __module__ = 'messages_pb2'
+  # @@protoc_insertion_point(class_scope:ugv.messages.Location)
+  ))
+_sym_db.RegisterMessage(Location)
+
+UGV_Status = _reflection.GeneratedProtocolMessageType('UGV_Status', (_message.Message,), dict(
+  DESCRIPTOR = _UGV_STATUS,
+  __module__ = 'messages_pb2'
+  # @@protoc_insertion_point(class_scope:ugv.messages.UGV_Status)
+  ))
+_sym_db.RegisterMessage(UGV_Status)
+
+UGV_Message = _reflection.GeneratedProtocolMessageType('UGV_Message', (_message.Message,), dict(
+  DESCRIPTOR = _UGV_MESSAGE,
+  __module__ = 'messages_pb2'
+  # @@protoc_insertion_point(class_scope:ugv.messages.UGV_Message)
+  ))
+_sym_db.RegisterMessage(UGV_Message)
+
+DriveHeadingData = _reflection.GeneratedProtocolMessageType('DriveHeadingData', (_message.Message,), dict(
+  DESCRIPTOR = _DRIVEHEADINGDATA,
+  __module__ = 'messages_pb2'
+  # @@protoc_insertion_point(class_scope:ugv.messages.DriveHeadingData)
+  ))
+_sym_db.RegisterMessage(DriveHeadingData)
+
+GroundCommand = _reflection.GeneratedProtocolMessageType('GroundCommand', (_message.Message,), dict(
+  DESCRIPTOR = _GROUNDCOMMAND,
+  __module__ = 'messages_pb2'
+  # @@protoc_insertion_point(class_scope:ugv.messages.GroundCommand)
+  ))
+_sym_db.RegisterMessage(GroundCommand)
+
+GroundMessage = _reflection.GeneratedProtocolMessageType('GroundMessage', (_message.Message,), dict(
+  DESCRIPTOR = _GROUNDMESSAGE,
+  __module__ = 'messages_pb2'
+  # @@protoc_insertion_point(class_scope:ugv.messages.GroundMessage)
+  ))
+_sym_db.RegisterMessage(GroundMessage)
+
+
+DESCRIPTOR._options = None
+# @@protoc_insertion_point(module_scope)

tools/requirements.txt

@@ -0,0 +1,14 @@
+certifi==2019.3.9
+chardet==3.0.4
+idna==2.8
+netifaces==0.10.6
+protobuf==3.7.1
+PyCRC==1.21
+pyserial==3.4
+python-engineio==3.5.2
+python-socketio==4.0.2
+PyYAML==5.1
+requests==2.22.0
+six==1.12.0
+urllib3==1.25.3
+websocket-client==0.56.0

tools/ugv.py

@@ -0,0 +1,205 @@
+#!/usr/bin/env python3
+
+import logging
+import sys
+import serial
+import threading
+from threading import Thread
+import time
+import binascii
+from base64 import b64decode, b64encode
+from PyCRC.CRC32 import CRC32
+import struct
+
+import messages_pb2 as messages
+from google.protobuf.message import Message
+
+log = logging.getLogger("ugv")
+
+
+class UGVComms:
+    MAX_WRITE_RETRY = 5
+    RETRY_TIME = 1.5
+
+    def __init__(self, serial_port: serial.Serial, on_msg_received=None):
+        self.ser = serial_port
+        self.on_msg_received = on_msg_received
+        self.msg_acks = []
+        self.ack_cv = threading.Condition()
+        self.next_command_id = 1
+        self.last_status = None
+        self.last_status_time = None
+        self.rx_thread = None
+        self.is_running = False
+        self.log_file = None
+
+    def write_base64(self, data: bytes):
+        crc = CRC32().calculate(data)
+        data_with_checksum = bytearray(data)
+        data_with_checksum.extend(struct.pack('<L', crc))
+        encoded = b64encode(data_with_checksum)
+        self.ser.write(encoded)
+        self.ser.write(b'\n')
+
+    def write_message(self, msg: Message):
+        log.debug("writing message: %s", msg)
+        data = msg.SerializeToString()
+        self.write_base64(data)
+
+    def write_command(self, command, retry=True):
+        cmdid = self.next_command_id
+        self.next_command_id += 1
+
+        gmsg = messages.GroundMessage()
+        if type(command) is int:
+            gmsg.command.type = command
+        else:
+            gmsg.command.CopyFrom(command)
+        gmsg.command.id = cmdid
+        self.write_message(gmsg)
+        last_write_time = time.time()
+        if not retry:
+            return
+        tries = UGVComms.MAX_WRITE_RETRY
+        with self.ack_cv:
+            while tries > 0:
+                if cmdid in self.msg_acks:
+                    self.msg_acks.remove(cmdid)
+                    log.debug("received ack for command")
+                    return
+                time_left = time.time() - last_write_time
+                if time_left >= self.RETRY_TIME:
+                    log.warning("retry writing command")
+                    self.write_message(gmsg)
+                    last_write_time = time.time()
+                    tries -= 1
+                self.ack_cv.wait(timeout=time_left)
+            raise TimeoutError("Timeout waiting for command ack")
+
+    def read_message(self):
+        data = self.ser.read_until(terminator=b'\n')
+        if len(data) is 0:
+            return None
+        try:
+            decoded = b64decode(data, validate=True)
+        except binascii.Error:
+            log.warning("read bad data: %s", data)
+            self.ser.flush()
+            return None
+        if len(decoded) < 4:
+            log.warning('Message too short ({} bytes)'.format(len(decoded)))
+            return None
+        msgcrc, = struct.unpack('<L', decoded[-4:])
+        calccrc = CRC32().calculate(decoded[:-4])
+        if msgcrc != calccrc:
+            log.warning('Checksum did not match ({} != {})'.format(msgcrc, calccrc))
+            return None
+        msg = messages.UGV_Message()
+        msg.ParseFromString(decoded[:-4])
+        return msg
+
+    def process_message(self, msg: messages.UGV_Message):
+        if msg is None:
+            return
+        log.debug("received UGV message: %s", msg)
+        if self.on_msg_received:
+            self.on_msg_received(msg)
+        if self.log_file:
+            print('[{}] UGV_Message: {}'.format(time.strftime('%Y-%b-%d %H:%M:%S'), msg), file=self.log_file)
+        if msg.HasField("command_ack"):
+            with self.ack_cv:
+                self.msg_acks.append(msg.command_ack)
+                self.ack_cv.notify()
+        elif msg.HasField("status"):
+            self.last_status = msg.status
+            self.last_status_time = time.time()
+        else:
+            log.warning("unknown UGV message: %s", msg)
+
+    def start(self):
+        if self.is_running:
+            log.warning("RX thread already running")
+            return False
+        self.is_running = True
+        self.rx_thread = Thread(target=self.__rx_thread_entry, daemon=True)
+        self.rx_thread.start()
+        log.debug("started RX thread")
+        return True
+
+    def stop(self):
+        if not self.is_running:
+            return False
+        self.is_running = False
+        self.ser.close()
+        self.rx_thread.join()
+        return True
+
+    def save_logs(self, file):
+        self.log_file = open(file, mode='a')
+
+    def __rx_thread_entry(self):
+        try:
+            while self.is_running and self.ser.is_open:
+                try:
+                    msg = self.read_message()
+                    self.process_message(msg)
+                except serial.SerialException:
+                    if not self.ser.is_open or not self.is_running:  # port was probably just closed
+                        return
+                    log.error("serial error", exc_info=True)
+                    return
+                except Exception:
+                    log.error("error reading message", exc_info=True)
+                    continue
+        finally:
+            if self.log_file:
+                self.log_file.close()
+
+
+def main():
+    if len(sys.argv) >= 2:
+        ser_url = sys.argv[1]
+    else:
+        ser_url = "hwgrep://"
+    ser = serial.serial_for_url(ser_url, baudrate=9600, parity=serial.PARITY_NONE,
+                                stopbits=serial.STOPBITS_ONE, bytesize=serial.EIGHTBITS,
+                                timeout=0.5)
+    ugv = UGVComms(ser)
+    ugv.start()
+    time.sleep(0.2)
+    try:
+        cmd = messages.GroundCommand()
+        cmd.type = messages.CMD_SET_TARGET
+        cmd.target_location.latitude = 34.068415
+        cmd.target_location.longitude = -118.443217
+        # ugv.write_command(cmd)
+        cmd.type = messages.CMD_SET_CONFIG
+        cmd.config.angle_pid.kp = 0.10
+        cmd.config.angle_pid.ki = 0  # .00005
+        cmd.config.angle_pid.kd = 0.4
+        cmd.config.angle_pid.max_output = 0.5
+        cmd.config.angle_pid.max_i_error = 15.0
+        cmd.config.min_target_dist = 10.0
+        cmd.config.min_flip_pitch = 90.0
+        ugv.write_command(cmd)
+        while True:
+            if ugv.last_status is None or ugv.last_status.state is not messages.STATE_DRIVE_HEADING:
+                cmd = messages.GroundCommand()
+                cmd.type = messages.CMD_DRIVE_HEADING
+                cmd.drive_heading.heading = -115.0 - 180
+                cmd.drive_heading.power = 0.3
+                ugv.write_command(cmd)
+                time.sleep(2.0)
+    except KeyboardInterrupt:
+        ugv.write_command(messages.CMD_DISABLE)
+        log.info("exiting...")
+    finally:
+        ugv.ser.flush()
+        ugv.ser.close()
+    ugv.stop()
+
+
+if __name__ == "__main__":
+    logging.basicConfig(format='%(asctime)s [%(name)s] %(levelname)s: %(message)s', datefmt='%Y-%b-%d %H:%M:%S')
+    log.setLevel(logging.DEBUG)
+    main()

tools/ugv_cmd.py

@@ -0,0 +1,241 @@
+#!/usr/bin/env python3
+
+import sys
+import serial
+import time
+import logging
+import readline
+import yaml
+try:
+    from yaml import CLoader as YamlLoader, CDumper as YamlDumper
+except ImportError:
+    from yaml import Loader as YamlLoader, Dumper as YamlDumper
+import types
+
+from ugv import UGVComms
+import messages_pb2 as messages
+import config_pb2
+
+log = logging.getLogger("ugv_cmd")
+
+
+def dict2pb(d, pb):
+    for key in d:
+        val = d[key]
+        if isinstance(val, dict):
+            dict2pb(val, getattr(pb, key))
+        else:
+            setattr(pb, key, val)
+
+
+class CLI_CMD:
+    def __init__(self, func, names=[], description=""):
+        self.func = func
+        self.names = names
+        self.description = description
+
+
+cli_commands = []
+
+
+def cli_cmd(names=None, description=""):
+    def dec(fn: types.FunctionType):
+        if dec.names is None:
+            dec.names = [fn.__name__]
+        cli_commands.append(CLI_CMD(fn, dec.names, dec.description))
+        return fn
+
+    dec.names = names
+    dec.description = description
+
+    return dec
+
+
+class UGV_CLI:
+    def __init__(self, on_msg_received=None):
+        self.on_msg_received = on_msg_received
+        self.is_running = False
+        self.last_state = messages.STATE_IDLE
+        self.commands = {
+            'get_status': self.get_status,
+            's': self.get_status,
+        }
+        self.ugv = None
+        pass
+
+    @cli_cmd(names=["help", "h", "?"], description="Print this help message")
+    def help_msg(self):
+        print("Commands:")
+        for cmd in cli_commands:
+            names = ", ".join(cmd.names).ljust(30, ' ')
+            print("{}: {}".format(names, cmd.description))
+        print()
+
+    @cli_cmd(names=["exit", "q", "C-d", "C-c"], description="Quit the program")
+    def exit(self):
+        self.is_running = False
+
+    @cli_cmd(names=["disable", "d"], description="Disable the UGV")
+    def disable(self):
+        self.ugv.write_command(messages.CMD_DISABLE)
+
+    @cli_cmd(names=["set_target", "st"], description="Set the target to <lat> <long>")
+    def set_target(self, lat=34.068415, long=-118.443217):
+        lat = float(lat)
+        long = float(long)
+        cmd = messages.GroundCommand()
+        cmd.type = messages.CMD_SET_TARGET
+        cmd.target_location.latitude = lat
+        cmd.target_location.longitude = long
+        self.ugv.write_command(cmd)
+        log.info("set target to (%f, %f)", lat, long)
+
+    @cli_cmd(names=["set_config", "sc"], description="Load configuration from config.yml and send")
+    def set_config(self, config_file_name="./tools/config.yml"):
+        with open(config_file_name, 'r') as configfile:
+            config = yaml.load(configfile, Loader=YamlLoader)
+
+        if 'REVISION' in config:
+            config_rev = config['REVISION']
+            del config['REVISION']
+        else:
+            config_rev = 1
+
+        cmd = messages.GroundCommand()
+        cmd.type = messages.CMD_SET_CONFIG
+        dict2pb(config, cmd.config)
+        self.ugv.write_command(cmd)
+        log.info("updated config")
+
+    @cli_cmd(names=["drive_heading", "dh"], description="Drive a <heading> with a forward <power>")
+    def drive_heading(self, heading=65, power=0.0):
+        heading = float(heading)
+        power = float(power)
+        cmd = messages.GroundCommand()
+        cmd.type = messages.CMD_DRIVE_HEADING
+        cmd.drive_heading.heading = heading
+        cmd.drive_heading.power = power
+        self.ugv.write_command(cmd)
+        log.info("driving heading %f at power %f", heading, power)
+
+    @cli_cmd(names=["drive_to_target", "dt"], description="Drive to the drop target")
+    def drive_to_target(self):
+        cmd = messages.GroundCommand()
+        cmd.type = messages.CMD_DRIVE_TO_TARGET
+        self.ugv.write_command(cmd)
+        log.info("driving to target")
+
+    @cli_cmd(names=["run_test", "rt"], description="Run test mode")
+    def run_test(self):
+        cmd = messages.GroundCommand()
+        cmd.type = messages.CMD_TEST
+        self.ugv.write_command(cmd)
+        log.info("running test mode")
+
+    @cli_cmd(names=["last_status", "ls", "s"], description="Print the last status of the UGV")
+    def last_status(self):
+        if self.ugv.last_status_time is None:
+            log.info("no status received")
+        else:
+            last_status_delay = time.time() - self.ugv.last_status_time
+            log.info("last status (%.4f seconds ago): %s",
+                     last_status_delay, self.ugv.last_status)
+
+    @cli_cmd(names=["get_status", "gs"], description="Get the current status of the UGV")
+    def get_status(self):
+        cmd = messages.GroundCommand()
+        cmd.type = messages.CMD_GET_STATUS
+        self.ugv.write_command(cmd)
+        self.last_status()
+
+    @cli_cmd(names=["ping", "p"], description="Ping the UGV")
+    def ping(self):
+        cmd = messages.GroundCommand()
+        cmd.type = messages.CMD_PING
+        self.ugv.write_command(cmd)
+        print("Received ping response")
+
+    @cli_cmd(names=["save_logs", "sl"], description="Save logs to a file")
+    def save_logs(self, file=None):
+        if file is None:
+            file = 'ugv_log.txt'
+        self.ugv.save_logs(file)
+        print("Saving logs to {}".format(file))
+
+    @staticmethod
+    def find_command(name):
+        for cmd in cli_commands:
+            if name in cmd.names:
+                return cmd
+        return None
+
+    @staticmethod
+    def complete_command(text, state):
+        options = [name for cmd in cli_commands for name in cmd.names if name.startswith(text)]
+        if state < len(options):
+            return options[state]
+        else:
+            return None
+
+    def start(self):
+        self.is_running = True
+
+        if len(sys.argv) >= 2:
+            ser_url = sys.argv[1]
+        else:
+            ser_url = "hwgrep://USB"
+        ser = serial.serial_for_url(ser_url, baudrate=9600, parity=serial.PARITY_NONE,
+                                    stopbits=serial.STOPBITS_ONE, bytesize=serial.EIGHTBITS,
+                                    timeout=0.5)
+
+        self.ugv = UGVComms(ser, self.on_msg_received)
+        self.ugv.start()
+
+    def run_cli(self):
+        if self.ugv is None:
+            self.start()
+
+        readline.parse_and_bind("tab: complete")
+        readline.set_completer(self.complete_command)
+        last_line = None
+        try:
+            print("Run 'help' to find out what commands are available")
+            while self.is_running:
+                line = input("UGV> ")
+                if len(line) is 0 and last_line is not None:
+                    print(last_line)
+                    line = last_line
+                line_parts = line.split(' ')
+                if len(line_parts) is 0:
+                    continue
+                cmd = self.find_command(line_parts[0])
+                if cmd is None:
+                    print("Unknown command: '%s'" % line_parts[0])
+                    continue
+                last_line = line
+                try:
+                    cmd.func(self, *line_parts[1:])
+                except KeyboardInterrupt:
+                    print("Command interrupted")
+                except Exception as e:
+                    print("Error executing command: ", e)
+            # TODO: continuously write state
+            # while True:
+            #    if self.ugv.last_status is None or self.ugv.last_status.state is not messages.STATE_DRIVE_HEADING:
+        except (KeyboardInterrupt, EOFError):
+            self.exit()
+        finally:
+            log.info("disabling UGV...")
+            try:
+                self.ugv.write_command(messages.CMD_DISABLE)
+                log.info("done. exiting")
+            except KeyboardInterrupt:
+                log.info("force exiting...")
+            self.ugv.stop()
+
+
+if __name__ == "__main__":
+    logging.basicConfig(
+        format='%(asctime)s [%(name)s] %(levelname)s: %(message)s', datefmt='%Y-%b-%d %H:%M:%S')
+    logging.getLogger().setLevel(logging.INFO)
+    UGV_CLI().run_cli()

tools/ugv_to_ground.py

@@ -0,0 +1,82 @@
+#!/usr/bin/env python3
+import socketio
+import logging
+from base64 import b64encode
+from threading import Thread
+import time
+
+from ugv_cmd import UGV_CLI
+import messages_pb2 as messages
+from google.protobuf.message import Message
+
+log = logging.getLogger("ugv_to_ground")
+
+def encode_msg(msg: Message):
+    data = msg.SerializeToString()
+    return b64encode(data).decode('utf-8')
+
+
+def ping_thread_entry(ugv_cli):
+    while ugv_cli.is_running and ugv_cli.ugv.ser.is_open:
+        try:
+            ugv_cli.get_status()
+            time.sleep(5.0)
+        except IOError as e:
+            log.error("Error pinging UGV: {}".format(e))
+
+def start_ugv_to_ground():
+    server_ip = 'localhost'
+
+    sio = socketio.Client()
+
+    def on_msg_received(msg: messages.UGV_Message):
+        sio.emit('UGV_MESSAGE', encode_msg(msg), namespace='/ugv')
+        pass
+
+    ugv_cli = UGV_CLI(on_msg_received)
+    ugv_cli.start()
+
+    @sio.on('connect', namespace='/ugv')
+    def on_connect():
+        log.info("connected to ground server")
+
+    @sio.on('disconnect', namespace='/ugv')
+    def on_disconnect():
+        log.info("disconnected from ground server!")
+
+    @sio.on('SET_TARGET', namespace='/ugv')
+    def set_target(msg):
+        log.info("Setting UGV target")
+        try:
+            ugv_cli.set_target(msg['lat'], msg['lng'])
+        except IOError as e:
+            log.error("Error setting target: {}".format(e))
+
+    @sio.on('DRIVE_TO_TARGET', namespace='/ugv')
+    def drive_to_target():
+        log.info("Driving to target!")
+        try:
+            ugv_cli.drive_to_target()
+        except IOError as e:
+            log.error("Error setting target: {}".format(e))
+
+    while True:
+        try:
+            sio.connect('http://'+server_ip+':8081', namespaces=['/ugv'], transports='websocket')
+            break
+        except:
+            print("Can't connect to ground server. Retrying in 2 seconds...")
+            sio.sleep(2)
+
+    ping_thread = Thread(target=ping_thread_entry, args=(ugv_cli, ), daemon=True)
+    ping_thread.start()
+
+    ugv_cli.run_cli()
+
+
+if __name__ == "__main__":
+    logging.basicConfig(format='%(asctime)s [%(name)s] %(levelname)s: %(message)s', datefmt='%Y-%b-%d %H:%M:%S')
+    logging.getLogger().setLevel(logging.INFO)
+
+    start_ugv_to_ground()
+

uas-ugv.code-workspace

@@ -13,6 +13,15 @@
 		"C_Cpp.autoAddFileAssociations": false,
 		"files.associations": {
 			"Dockerfile.*": "dockerfile"
+		},
+		"[cpp]": {
+			"editor.tabSize": 2
+		},
+		"[c]": {
+			"editor.tabSize": 2
+		},
+		"[python]": {
+			"editor.tabSize": 4
 		}
 	},
 	"extensions": {

ugv2_moving.csv

@@ -0,0 +1,857 @@
+AX,AY,AZ,GX,GY,GZ,MX,MY,MZ
+0.186041,-0.008484,-0.962920,4.318369,2.807703,0.885037,95.511116,-22.940660,19.642004
+0.181219,-0.003906,-0.964324,4.242073,2.868740,0.869778,95.511116,-22.940660,19.642004
+0.185614,-0.004578,-0.964324,4.333628,2.792444,0.885037,96.260811,-23.990232,19.192186
+0.181951,-0.003174,-0.970244,4.318369,2.807703,0.839259,96.260811,-23.990232,19.192186
+0.182806,-0.003052,-0.968169,4.379406,2.761925,0.793481,96.260811,-23.990232,19.192186
+0.184271,-0.005188,-0.965972,4.364147,2.792444,0.915555,95.661057,-23.690355,19.492064
+0.179815,-0.005493,-0.966582,4.318369,2.731407,0.869778,95.661057,-23.690355,19.492064
+0.185125,-0.003906,-0.966643,4.364147,2.777184,0.839259,96.560684,-23.390476,18.892309
+0.182806,-0.000977,-0.967132,4.440443,2.746666,0.808740,96.560684,-23.390476,18.892309
+0.184515,-0.001404,-0.964629,4.440443,2.792444,0.839259,96.560684,-23.390476,18.892309
+0.180670,-0.008179,-0.962859,4.486221,2.822962,0.869778,95.061295,-23.990232,19.492064
+0.184759,-0.008911,-0.966399,4.440443,2.853481,0.991852,95.061295,-23.990232,19.492064
+0.177557,-0.005005,-0.964141,4.364147,2.731407,0.762963,95.960930,-23.390476,19.192186
+0.189215,-0.000183,-0.965911,4.348887,2.792444,0.854518,95.960930,-23.390476,19.192186
+0.177190,-0.003113,-0.969207,4.348887,2.746666,0.885037,95.960930,-23.390476,19.192186
+0.189276,-0.011719,-0.966155,4.318369,2.792444,0.869778,96.710625,-22.940660,19.941881
+0.179266,-0.004761,-0.966094,4.348887,2.807703,0.854518,96.710625,-22.940660,19.941881
+0.185919,0.001892,-0.967803,4.303110,2.777184,0.854518,96.110870,-22.940660,19.342125
+0.466506,0.016541,-0.540239,2.777184,4.333628,0.442518,96.110870,-22.940660,19.342125
+0.172735,-0.001648,-0.887722,4.028443,3.295999,3.448591,96.110870,-22.940660,19.342125
+0.229804,0.014527,-0.839625,3.967406,4.440443,1.068148,95.061295,-23.690355,18.892309
+0.213446,-0.043031,-0.995392,4.699851,2.670370,0.427259,95.061295,-23.690355,18.892309
+0.219428,-0.042787,-0.971343,6.241035,-2.594073,-4.593036,95.661057,-23.990232,20.391699
+0.184820,-0.029969,-1.086337,21.332438,7.400739,-34.592731,95.661057,-23.990232,20.391699
+0.287301,-0.148625,-0.909513,50.798058,-57.054352,-53.392132,95.661057,-23.990232,20.391699
+0.102969,-0.048952,-1.051485,39.338360,-96.667381,-74.846642,96.260811,-21.891088,18.292553
+0.073244,-0.158696,-1.143468,22.080141,-62.639240,-73.900574,96.260811,-21.891088,18.292553
+0.073366,-0.224677,-0.998627,3.250221,-49.668873,-92.944122,99.259590,-14.993896,16.493284
+-0.050295,-0.102542,-0.934904,0.457778,-61.571095,-106.250191,99.259590,-14.993896,16.493284
+-0.134220,-0.132939,-1.005097,-5.966369,-53.834652,-106.021301,99.259590,-14.993896,16.493284
+-0.179022,0.001526,-1.026582,-35.813469,-42.741173,-80.202644,102.708183,-6.747253,16.043468
+-0.270333,0.012452,-1.048921,-68.559830,-46.037170,-69.780571,102.708183,-6.747253,16.043468
+-0.351634,-0.021912,-0.995453,-107.181007,-69.460129,-63.005463,101.958488,-2.399023,18.892309
+-0.334910,-0.066347,-0.899808,-113.834038,-92.822044,-61.326946,101.958488,-2.399023,18.892309
+-0.348460,0.078310,-0.802820,-129.352707,-113.879822,-96.743675,101.958488,-2.399023,18.892309
+-0.419263,0.223579,-0.788537,-147.114471,-126.010925,-106.540115,103.757759,3.298657,28.788280
+-0.602435,0.413953,-0.659383,-131.931519,-111.713005,-78.081604,103.757759,3.298657,28.788280
+-0.725303,0.305185,-0.450819,-94.515823,-137.974182,-51.911983,105.107208,7.646887,38.234432
+-0.797571,0.350902,-0.413770,-70.772423,-132.526627,-42.817471,105.107208,7.646887,38.234432
+-0.839198,0.271615,-0.268929,-83.376564,-166.692093,-36.500137,105.107208,7.646887,38.234432
+-0.877712,0.185553,-0.113102,-61.281166,-201.940979,-74.785606,107.356293,6.597314,48.580223
+-0.942900,0.302316,0.052309,-41.123692,-167.867065,-118.091370,107.356293,6.597314,48.580223
+-0.945952,0.217353,0.219794,-23.773918,-57.390057,-108.371223,115.752876,2.698901,59.675705
+-0.964141,-0.096866,0.249947,0.595111,-6.546220,-127.994629,115.752876,2.698901,59.675705
+-1.020661,-0.241707,0.195929,22.080141,8.560442,-169.072540,115.752876,2.698901,59.675705
+-1.074374,-0.384899,0.066286,15.396588,18.005920,-182.866913,120.550919,2.399023,60.875217
+-1.037263,-0.368053,0.010498,-25.238808,49.424725,-156.468399,120.550919,2.399023,60.875217
+-0.948820,-0.463210,0.021668,-32.334362,69.353310,-120.319221,128.647629,3.598535,58.476192
+-0.841823,-0.523331,-0.012452,-26.581621,157.551804,-87.145607,128.647629,3.598535,58.476192
+-0.774621,-0.603168,-0.048097,-21.881771,168.599503,-45.869320,133.145798,5.997558,51.878880
+-0.792322,-0.453261,-0.349071,-6.530961,158.574173,3.326518,133.145798,5.997558,51.878880
+-0.695273,-0.639119,-0.361461,-9.887997,166.035950,65.157021,133.145798,5.997558,51.878880
+-0.714194,-0.505631,-0.391064,-43.488876,175.847656,114.230782,131.946274,7.496948,43.182419
+-0.663411,-0.362926,-0.726646,-72.649315,185.415207,148.472549,131.946274,7.496948,43.182419
+-0.677999,-0.275704,-0.714682,-122.760704,212.286758,164.799957,124.599274,10.045910,30.137730
+-0.568621,-0.230537,-0.760399,-143.086029,148.365738,157.979065,124.599274,10.045910,30.137730
+-0.409497,0.015625,-0.881741,-156.666763,151.692245,147.831665,124.599274,10.045910,30.137730
+-0.213141,0.106632,-0.982513,-173.177277,136.677139,150.013733,111.704521,4.348230,23.240538
+-0.194159,0.277474,-0.816126,-197.576828,129.291672,139.225433,111.704521,4.348230,23.240538
+0.045045,0.429395,-0.733421,-186.544388,82.659386,132.587662,98.959709,-4.798047,24.290112
+0.251961,0.565264,-0.649556,-167.531357,64.455093,107.715080,98.959709,-4.798047,24.290112
+0.376904,0.728050,-0.507157,-169.026764,61.418499,101.519821,98.959709,-4.798047,24.290112
+0.462661,0.758385,-0.355419,-150.212097,52.919094,74.724571,89.513557,-10.945544,31.337242
+0.552751,0.965850,-0.336009,-138.386185,37.507248,67.033905,89.513557,-10.945544,31.337242
+0.470107,0.874172,-0.178594,-118.717003,34.638508,96.713158,89.363617,-16.793163,41.982906
+0.499649,0.885830,-0.088992,-107.898193,13.000885,149.784851,89.363617,-16.793163,41.982906
+0.578448,0.692709,-0.289193,-93.417160,1.403851,161.244537,89.363617,-16.793163,41.982906
+0.827662,0.501419,0.052858,-119.815666,15.686514,177.632980,91.762642,-25.789501,48.280342
+0.954802,0.359081,0.001343,-78.173164,32.441177,133.472702,91.762642,-25.789501,48.280342
+1.074496,0.213935,0.186895,-39.155247,39.384136,60.655537,95.061295,-33.586327,54.577782
+1.069369,0.326060,0.190374,-32.303841,52.415539,-8.880886,95.061295,-33.586327,54.577782
+0.917570,0.400769,0.234016,15.045625,119.067963,-38.926357,95.061295,-33.586327,54.577782
+0.880032,0.399792,0.732810,34.607990,83.117157,-65.614792,97.010506,-27.438829,56.526985
+0.652486,0.488418,0.437635,63.539536,-137.165436,-195.410019,97.010506,-27.438829,56.526985
+0.531266,0.510636,0.040529,131.321152,-160.420547,-271.202728,94.611481,-19.342125,51.129185
+0.438429,0.730125,-0.254158,120.349739,-133.701584,-298.242126,94.611481,-19.342125,51.129185
+0.124638,1.093417,-0.313547,86.016418,20.142216,-244.239624,94.611481,-19.342125,51.129185
+-0.095340,1.122715,0.128727,60.457169,-4.531999,-154.087952,93.262032,-7.496948,45.581444
+-0.160588,0.817652,-0.280465,108.890045,-103.350929,-146.000549,93.262032,-7.496948,45.581444
+-0.303476,0.662679,-0.237373,111.682487,-123.081146,-186.696976,96.410751,2.249084,41.533092
+-0.525224,0.669759,-0.385205,180.089722,-185.552536,-227.027191,96.410751,2.249084,41.533092
+-0.698386,0.385144,-0.426649,184.881134,-183.950317,-250.724808,96.410751,2.249084,41.533092
+-0.940275,0.295236,-0.379406,142.475662,-185.842468,-254.524368,107.506233,7.047131,45.731380
+-1.016388,0.096561,-0.415662,101.672417,-182.821136,-224.753555,107.506233,7.047131,45.731380
+-0.999786,-0.008179,-0.175848,19.608143,-156.575211,-181.798767,119.501350,8.246642,53.828087
+-0.970855,-0.108890,-0.030885,-15.198217,-127.155373,-136.173584,119.501350,8.246642,53.828087
+-0.918973,-0.138127,0.095950,2.822962,5.218665,-113.742485,119.501350,8.246642,53.828087
+-0.875820,-0.425245,0.130741,84.124268,63.463242,-103.946045,123.699638,3.748474,56.526985
+-0.985870,-0.513382,-0.245125,53.361614,183.980835,-103.946045,123.699638,3.748474,56.526985
+-0.938017,-0.498123,-0.266854,35.523544,210.715042,-97.567673,128.197800,7.946764,52.028816
+-0.827235,-0.630390,-0.333140,44.236580,183.339951,-95.538193,128.197800,7.946764,52.028816
+-0.630573,-0.809168,-0.291635,105.258339,226.737259,-77.227089,128.197800,7.946764,52.028816
+-0.561724,-0.652120,-0.511002,118.671227,240.287491,-64.546646,131.496460,10.945544,42.132847
+-0.488174,-0.816004,-0.422437,112.094482,241.492966,-24.643696,131.496460,10.945544,42.132847
+-0.376659,-0.894375,-0.574969,62.898647,224.616226,8.224738,134.045425,6.897192,30.587547
+-0.289438,-0.887906,-0.319834,15.198217,115.756706,-15.732291,134.045425,6.897192,30.587547
+-0.371471,-0.763573,-0.318979,35.935543,-60.609760,-27.405621,134.045425,6.897192,30.587547
+-0.328929,-0.990936,-0.191046,72.069458,-245.872375,-88.579971,134.045425,6.897192,30.587547
+-0.355235,-0.957244,-0.498733,68.208870,-269.432648,-105.746635,134.045425,6.897192,30.587547
+-0.108707,-0.842860,-0.564714,23.438215,-225.623337,-84.902496,140.642746,2.998779,43.482296
+-0.115238,-0.945402,0.011170,-5.355999,-244.392227,-49.501022,140.642746,2.998779,43.482296
+-0.210456,-0.973235,-0.064882,2.792444,-230.109558,0.442518,140.642746,2.998779,43.482296
+-0.027406,-1.333415,0.831263,-37.446213,-19.867550,23.453474,143.791458,-5.247863,51.429062
+-0.328501,-0.862148,-0.452284,9.247108,-186.056091,-59.602650,143.791458,-5.247863,51.429062
+0.134465,-0.846156,-0.013550,15.503403,-72.817162,5.600146,145.290848,-9.146276,55.327477
+-0.249580,-0.966948,-0.053407,64.317757,-80.141609,-91.708122,145.290848,-9.146276,55.327477
+-0.247261,-1.076632,-0.168096,-5.523850,-106.814781,-90.945160,145.290848,-9.146276,55.327477
+-0.411878,-0.856716,-0.296518,-72.801903,-113.116852,-40.192879,146.040543,-14.694017,57.276680
+0.091372,-0.911222,-0.067385,-68.102051,-29.007843,17.899105,146.040543,-14.694017,57.276680
+-0.236213,-0.809473,-0.206244,-47.486801,-74.633018,-10.238960,148.289627,-19.342125,51.728939
+0.031007,-0.844813,-0.159856,-24.201178,-48.951691,-5.539109,148.289627,-19.342125,51.728939
+-0.137333,-0.907498,-0.069704,-0.137333,-38.132877,9.735405,148.289627,-19.342125,51.728939
+0.082461,-0.997894,0.088198,120.609146,-50.813316,18.387402,148.439560,-20.991453,50.679367
+0.171636,-1.474960,0.911161,272.988068,-44.938503,-21.958067,148.439560,-20.991453,50.679367
+-0.041627,-1.125278,0.310678,242.393265,-93.920715,-63.402203,140.792679,-19.642004,59.825642
+-0.046022,-1.058870,0.273751,298.455750,-79.851677,-46.632282,140.792679,-19.642004,59.825642
+-0.057924,-0.969146,0.255928,260.963776,-21.301920,-55.421612,140.792679,-19.642004,59.825642
+0.038453,-0.630146,0.757347,194.265579,19.287699,-41.734062,124.599274,-27.738707,66.722832
+0.051393,-0.434828,0.683981,175.649277,2.502518,-15.274514,124.599274,-27.738707,66.722832
+0.014893,-0.289254,0.720664,191.045868,25.162512,-22.156437,113.503792,-27.438829,67.322594
+0.047914,-0.498795,1.868892,216.895050,62.654499,-29.847103,113.503792,-27.438829,67.322594
+-0.017579,0.077517,0.877285,149.037140,54.795982,-30.533770,113.503792,-27.438829,67.322594
+-0.111026,0.161565,0.821131,164.860992,74.205757,-36.820583,103.158005,-23.090599,64.473747
+-0.221320,0.388134,0.975433,201.879944,95.645012,-61.403240,103.158005,-23.090599,64.473747
+-0.147526,0.560808,0.996246,244.804230,78.600418,-87.038788,93.112091,-15.143834,54.427841
+-0.247688,0.784265,0.959563,251.243622,40.330212,-84.887238,93.112091,-15.143834,54.427841
+-0.357067,0.860317,0.842189,213.110748,15.655995,-73.213905,93.112091,-15.143834,54.427841
+-0.289376,1.042634,0.644673,168.904694,6.698813,-54.536575,90.263252,-8.996337,38.684250
+-0.340587,0.987335,0.613544,150.303665,-8.957183,-28.351696,90.263252,-8.996337,38.684250
+-0.387280,0.935942,0.365429,162.663651,-16.632587,-20.783106,91.462761,-5.397802,29.088158
+-0.401990,0.878811,0.306650,175.252533,-11.459701,-36.271248,91.462761,-5.397802,29.088158
+-0.358165,0.924100,0.217536,150.227356,-15.930662,-44.190804,98.509895,-4.648108,22.041027
+-0.428236,0.874294,-0.048402,136.432999,-31.006805,-30.762657,98.509895,-4.648108,22.041027
+-0.525590,0.790063,-0.151311,134.601883,-30.335398,-30.213324,98.509895,-4.648108,22.041027
+-0.469497,0.748131,-0.096011,89.739677,-10.650960,-25.452436,106.306717,-3.148718,16.943102
+-0.493240,0.685690,-0.266427,-46.449173,-35.065769,-2.746666,106.306717,-3.148718,16.943102
+-0.452712,0.811182,-0.066225,-231.345566,-10.315256,58.839687,100.908920,-2.848840,21.141394
+-0.485061,0.805444,0.299142,-365.443878,-5.951109,90.853600,100.908920,-2.848840,21.141394
+-0.237861,0.819849,0.575091,-427.228607,-5.951109,106.616409,100.908920,-2.848840,21.141394
+-0.180853,0.621235,0.953093,-376.995148,-25.650806,119.983521,91.462761,-7.496948,41.683029
+-0.128666,0.276559,0.996063,-288.445679,-33.783989,80.065308,91.462761,-7.496948,41.683029
+-0.186834,0.185064,1.039216,-247.001556,-40.894802,45.731987,99.259590,-11.995116,58.776070
+-0.059877,-0.098880,1.089572,-217.856384,-39.826656,47.639393,99.259590,-11.995116,58.776070
+-0.003845,-0.260689,1.055757,-226.569412,-46.220284,41.856136,99.259590,-11.995116,58.776070
+-0.060244,-0.473708,0.881619,-234.092224,-37.629322,39.658802,110.355072,-15.893529,68.372162
+-0.012635,-0.566973,0.885098,-233.283493,-36.713768,24.582659,110.355072,-15.893529,68.372162
+-0.027833,-0.620502,0.780602,-244.590591,-40.360729,32.197029,125.798782,-18.442492,68.522102
+-0.040284,-0.765709,0.698813,-213.782166,-53.071690,50.004578,125.798782,-18.442492,68.522102
+-0.014466,-0.855312,0.464125,-152.226318,-63.081760,37.766655,125.798782,-18.442492,68.522102
+-0.150334,-0.909146,0.646504,-144.581436,-47.425762,56.001465,136.294510,-20.541637,62.824421
+-0.038453,-1.026643,0.262276,-157.536545,-64.760277,53.270058,136.294510,-20.541637,62.824421
+-0.172002,-1.063143,0.013733,-177.358322,-71.184425,45.655689,145.890610,-20.841515,55.327477
+-0.174810,-1.041292,-0.157903,-192.052979,-66.438797,40.940582,145.890610,-20.841515,55.327477
+-0.280099,-1.089145,-0.599750,-167.958618,-70.848717,17.548143,145.890610,-20.841515,55.327477
+-0.269784,-0.696860,-0.571856,-177.022614,-58.870205,1.739555,148.889389,-22.041027,42.732601
+-0.278268,-0.749168,-0.413648,-137.653732,-53.056427,3.631703,148.889389,-22.041027,42.732601
+-0.330210,-0.597919,-0.491043,-141.376999,-72.374641,1.510666,147.090118,-22.340904,32.236874
+-0.372509,-0.540117,-0.745811,-145.466476,-79.989014,21.073030,147.090118,-22.340904,32.236874
+-0.436110,-0.522111,-0.775048,-173.329880,-62.364574,9.796441,147.090118,-22.340904,32.236874
+-0.475051,-0.262032,-0.941801,-159.596542,-47.639393,0.839259,142.891830,-19.941881,22.640781
+-0.569781,-0.331187,-1.049104,-128.330338,5.035554,15.198217,142.891830,-19.941881,22.640781
+-0.496048,-0.336375,-0.879971,-60.426647,39.979248,-29.450361,138.543594,-20.091820,15.293774
+-0.529374,-0.124699,-0.926237,-34.607990,39.521469,-51.988281,138.543594,-20.091820,15.293774
+-0.348888,-0.248421,-1.027314,-55.299538,51.072727,-42.115543,138.543594,-20.091820,15.293774
+-0.329600,-0.280648,-0.972381,-86.092712,101.931824,-43.763542,136.294510,-24.140171,14.544079
+-0.382580,-0.276803,-0.960356,-143.589584,122.959076,-49.180576,136.294510,-24.140171,14.544079
+-0.058596,-0.079104,-0.610675,-177.190460,104.144417,-36.545914,126.698418,-31.936998,13.344567
+0.169805,0.022828,-0.631306,-203.680527,58.076725,-52.018799,126.698418,-31.936998,13.344567
+-0.032228,0.304697,-1.035798,-116.687523,46.678059,-63.600574,126.698418,-31.936998,13.344567
+-0.024598,0.548601,-0.915067,5.172887,84.261604,-75.411240,115.752876,-35.385593,14.993896
+-0.024537,0.442824,-1.003693,19.882809,104.480118,-53.773613,115.752876,-35.385593,14.993896
+-0.014222,0.463088,-0.936674,59.022797,141.697433,-56.199837,114.703300,-38.834190,20.541637
+0.134648,0.474197,-0.841029,84.566788,140.583511,-53.880428,114.703300,-38.834190,20.541637
+0.251839,0.497269,-0.996185,66.377762,135.029144,-26.123844,114.703300,-38.834190,20.541637
+0.203986,0.406140,-0.856166,13.199255,142.185730,7.110813,113.953606,-42.882542,25.489622
+0.508377,0.556169,-0.858425,22.537920,127.780998,32.227547,113.953606,-42.882542,25.489622
+0.560869,0.294504,-0.820887,73.274940,122.638634,47.151096,116.502571,-46.031258,32.536755
+0.597003,0.238533,-0.798303,103.106781,131.168549,33.555099,116.502571,-46.031258,32.536755
+0.772790,0.155400,-0.378307,125.034332,165.532394,49.851986,116.502571,-46.031258,32.536755
+0.795373,-0.003723,-0.292062,122.592850,137.226471,49.638355,120.251045,-46.780952,39.883762
+1.049348,-0.168584,-0.034425,129.947815,85.528122,46.571247,120.251045,-46.780952,39.883762
+0.892727,-0.312815,-0.231330,136.326187,39.063690,-0.244148,120.850800,-45.581444,47.680588
+0.924345,-0.346934,-0.201849,53.834652,31.907101,-29.923399,120.850800,-45.581444,47.680588
+0.991607,-0.479873,-0.266488,-16.830957,13.626514,-77.333900,120.850800,-45.581444,47.680588
+1.018586,-0.371654,-0.196112,-204.657120,1.571703,-118.259224,115.902809,-44.531872,48.130405
+0.940458,0.030824,-0.183111,-219.611191,-7.415998,-165.959656,115.902809,-44.531872,48.130405
+0.880825,0.173894,-0.197943,-138.630325,-11.215552,-199.270615,110.804886,-43.632236,50.829308
+0.817164,0.329234,-0.005921,-130.893890,-12.634663,-204.779205,110.804886,-43.632236,50.829308
+0.624775,0.465407,0.064943,-77.517014,-43.900875,-247.627182,110.804886,-43.632236,50.829308
+0.293161,0.528275,0.006409,-66.682945,-64.699242,-268.883331,103.607819,-38.834190,52.028816
+-0.044862,0.833461,0.003540,-97.994934,-68.147835,-279.381683,103.607819,-38.834190,52.028816
+-0.307443,0.909391,-0.118900,-59.694206,-45.854061,-283.593262,96.410751,-28.938219,52.928452
+-0.554949,0.945708,-0.048280,-47.944580,-16.388439,-273.049103,96.410751,-28.938219,52.928452
+-0.651814,0.952605,-0.006531,-48.188725,18.997772,-243.583481,96.410751,-28.938219,52.928452
+-0.642048,0.936186,0.122684,27.024139,35.294655,-207.632675,95.061295,-14.694017,54.877659
+-0.717307,0.908170,0.131962,108.172859,5.462813,-168.599503,95.061295,-14.694017,54.877659
+-0.766076,0.789514,0.095950,191.381577,-38.254951,-124.225594,94.161667,-5.097925,50.679367
+-0.916715,0.665548,-0.146184,233.725998,-81.759087,-79.531235,94.161667,-5.097925,50.679367
+-1.141148,0.544328,-0.329783,226.462601,-73.397018,-26.841028,94.161667,-5.097925,50.679367
+-1.013520,0.469863,-0.345836,299.493378,-75.457016,-33.433025,93.411972,-2.249084,40.333580
+-0.902860,0.389477,-0.381115,268.822296,-21.530807,10.498367,93.411972,-2.249084,40.333580
+-0.916288,0.242683,-0.253304,259.529419,-14.175848,22.156437,96.110870,-2.848840,28.038586
+-0.942534,0.114017,-0.456679,287.377533,-8.758812,5.432295,96.110870,-2.848840,28.038586
+-0.854823,0.048158,-0.410657,262.825409,47.608875,-8.270516,96.110870,-2.848840,28.038586
+-0.998688,-0.026063,-0.433058,261.421539,91.952271,-40.940582,106.606598,-4.048352,18.742369
+-0.776879,-0.185797,-0.436110,259.865112,90.334785,-54.704430,106.606598,-4.048352,18.742369
+-0.634907,-0.380810,-0.340037,218.436234,90.365303,-52.247688,121.900368,-8.546520,12.444933
+-0.674764,-0.548967,-0.361400,167.027802,74.663536,-67.995239,121.900368,-8.546520,12.444933
+-0.621723,-0.662862,-0.528703,110.797447,76.662498,-88.412125,121.900368,-8.546520,12.444933
+-0.351512,-0.962188,-0.495560,84.414200,75.197609,-86.916718,130.596832,-12.744811,13.344567
+-0.577471,-0.976287,-0.639363,46.967987,117.908264,-142.139954,130.596832,-12.744811,13.344567
+-0.545732,-0.844752,-0.787317,86.779381,115.237892,-175.267807,135.095001,-22.940660,16.043468
+-0.273873,-0.881863,-0.588031,119.434189,143.284409,-168.187500,135.095001,-22.940660,16.043468
+-0.032350,-0.901273,-0.791406,85.741753,173.192535,-153.660690,134.195358,-34.935776,20.241758
+-0.072512,-0.848354,-0.590350,75.838493,169.347214,-151.356552,134.195358,-34.935776,20.241758
+0.176763,-0.797327,-0.373058,24.307993,191.000092,-126.911224,134.195358,-34.935776,20.241758
+0.463637,-0.840846,-0.222175,29.877621,153.172394,-127.063812,131.046646,-43.482296,29.987791
+0.324839,-0.820276,-0.160894,-35.493027,173.100983,-121.585739,131.046646,-43.482296,29.987791
+0.554704,-0.776391,0.099673,-26.795252,167.622910,-144.779816,122.050308,-46.481075,40.783398
+0.571795,-0.570025,0.222236,-97.598190,159.337143,-132.465591,122.050308,-46.481075,40.783398
+0.734153,-0.540788,0.319712,-104.266487,156.910919,-127.063812,122.050308,-46.481075,40.783398
+0.865627,-0.437513,0.422437,-40.147099,121.387375,-121.387375,116.802444,-43.332359,49.929672
+0.933500,-0.377819,0.378674,-33.555099,100.177010,-112.598038,116.802444,-43.332359,49.929672
+0.909268,-0.261361,0.574602,-35.142063,70.909760,-98.284859,111.854462,-40.183640,57.576561
+0.857936,-0.166875,0.525529,19.913328,14.221625,-103.015228,111.854462,-40.183640,57.576561
+0.781396,-0.149174,0.236579,54.506058,15.030366,-86.977753,111.854462,-40.183640,57.576561
+0.843471,-0.046022,0.332835,93.279823,47.273170,-58.610798,105.407089,-36.435165,57.426620
+0.972198,0.162420,0.498489,109.363075,66.911835,-14.648885,105.407089,-36.435165,57.426620
+0.870937,0.460036,0.463881,91.875969,69.963684,74.495682,101.808548,-33.436386,55.627354
+0.903775,0.401318,0.517045,123.310036,78.722496,170.827362,101.808548,-33.436386,55.627354
+0.648762,0.157353,0.562761,131.839966,89.846489,241.203033,101.808548,-33.436386,55.627354
+0.402234,-0.095462,0.646077,153.523361,104.022339,254.158142,106.306717,-38.834190,55.327477
+0.262703,-0.350047,0.760704,164.052246,128.620255,225.089264,106.306717,-38.834190,55.327477
+0.338267,-0.487991,0.942351,93.050934,130.451370,161.809143,111.554581,-39.583885,58.776070
+0.254830,-0.371288,0.996185,-14.175848,106.997894,123.966187,111.554581,-39.583885,58.776070
+0.315806,-0.404797,1.088900,-70.467239,66.957611,113.238930,111.554581,-39.583885,58.776070
+0.052187,-0.535539,0.975372,-152.516251,31.327250,113.437302,118.001961,-36.735043,61.624912
+-0.035463,-0.668477,0.907620,-179.174164,17.700735,102.710045,118.001961,-36.735043,61.624912
+0.021363,-0.798791,0.982574,-212.515640,14.053774,106.265450,130.746765,-34.485958,60.575336
+-0.152470,-0.916837,0.842006,-217.276535,-27.420881,84.353157,130.746765,-34.485958,60.575336
+-0.121830,-1.043489,0.645772,-267.403168,-52.751244,69.994202,130.746765,-34.485958,60.575336
+0.034425,-1.059603,0.465102,-288.979767,-109.439377,41.062653,143.641525,-34.186081,47.980465
+-0.185492,-0.960173,-0.098697,-319.315155,-129.398483,21.347698,143.641525,-34.186081,47.980465
+-0.063967,-0.873257,-0.279122,-318.582733,-102.984711,-22.644733,144.091339,-31.637119,28.038586
+-0.097720,-0.694906,-0.436659,-357.386993,-122.821739,-32.593769,144.091339,-31.637119,28.038586
+-0.276376,-0.540117,-0.694357,-369.151886,-106.784264,-72.542496,144.091339,-31.637119,28.038586
+-0.171880,-0.097476,-0.654866,-338.663910,-86.855675,-93.783379,124.899147,-29.238096,12.444933
+-0.375256,0.110660,-0.739952,-354.808197,-71.718498,-72.359383,124.899147,-29.238096,12.444933
+-0.515519,0.305856,-0.379528,-360.286255,-79.058197,-89.938049,103.307938,-23.540417,15.143834
+-0.774560,0.374157,-0.458632,-306.588959,-99.887085,-133.060699,103.307938,-23.540417,15.143834
+-0.688131,0.561602,-0.168035,-293.023468,-88.518936,-155.644394,103.307938,-23.540417,15.143834
+-0.565447,0.890774,0.023865,-327.677246,-73.122345,-138.004700,91.162888,-12.294994,33.286449
+-0.950163,0.833827,0.021058,-371.486542,-39.872433,-106.677452,91.162888,-12.294994,33.286449
+-1.036409,0.752159,0.182318,-367.107147,18.509476,-59.343243,95.661057,-5.997558,53.678146
+-1.040742,0.724631,0.335948,-330.408630,36.362804,-0.350963,95.661057,-5.997558,53.678146
+-0.856044,0.707846,0.391247,-236.594742,24.857327,70.101013,95.661057,-5.997558,53.678146
+-0.777490,0.685751,0.435072,-118.076111,-16.922514,111.880859,108.855682,-3.898413,65.373383
+-0.701559,0.580645,0.511917,-48.631245,-57.695244,153.340256,108.855682,-3.898413,65.373383
+-0.692648,0.535234,0.447279,-71.535385,-91.875969,155.903809,110.355072,-8.396582,67.472527
+-0.491592,0.593890,0.619343,-60.762352,-98.467972,148.426773,110.355072,-8.396582,67.472527
+-0.339671,0.705771,0.728416,-5.615406,-102.053894,137.333298,110.355072,-8.396582,67.472527
+-0.269539,0.711325,0.668477,32.105473,-114.185005,156.453140,113.353851,-18.292553,68.372162
+-0.018311,0.650716,0.646321,64.546646,-107.608261,159.428696,113.353851,-18.292553,68.372162
+0.125065,0.686850,0.529618,58.580280,-80.874046,136.020996,112.754097,-27.588768,64.773628
+0.232917,0.668477,0.524064,65.080719,-47.318951,110.080261,112.754097,-27.588768,64.773628
+0.299448,0.695090,0.494522,159.932251,-13.962218,59.602650,112.754097,-27.588768,64.773628
+0.360607,0.787133,0.566607,144.169434,-15.350810,41.566212,109.005623,-31.337242,61.624912
+0.325266,0.695273,0.266488,152.577286,-20.798365,-5.935850,109.005623,-31.337242,61.624912
+0.268746,1.034700,0.195379,169.209869,-19.882809,-26.871548,104.057632,-32.386814,56.976803
+0.201605,0.976653,0.023072,175.603500,-35.187840,-29.221474,104.057632,-32.386814,56.976803
+0.153874,0.851588,0.012696,189.657272,-64.775536,-56.733910,104.057632,-32.386814,56.976803
+-0.010681,0.903104,-0.099185,139.118622,-109.607224,-68.376717,97.760201,-32.386814,48.880100
+-0.147832,1.068270,-0.323679,27.939695,-145.908997,-47.379986,97.760201,-32.386814,48.880100
+-0.162358,1.045625,-0.588946,89.007233,-126.544998,-30.884731,93.411972,-31.637119,40.033703
+0.051576,0.917753,-0.543413,207.052826,-93.722343,7.629627,93.411972,-31.637119,40.033703
+-0.072512,0.706320,-0.844447,234.382156,-88.976715,50.782799,93.411972,-31.637119,40.033703
+-0.047548,0.397412,-0.885159,233.939636,-85.283974,49.134800,97.310379,-31.637119,25.339684
+-0.125553,0.238472,-0.917936,241.248810,-50.889614,43.870358,97.310379,-31.637119,25.339684
+-0.133061,-0.022401,-0.872219,266.930145,-44.022949,34.516434,97.310379,-31.637119,25.339684
+-0.128300,-0.190435,-0.853481,268.654449,-40.650654,9.720145,97.310379,-31.637119,25.339684
+-0.171270,-0.338206,-0.844508,230.430008,-37.415691,17.365032,97.310379,-31.637119,25.339684
+-0.220038,-0.590960,-1.011505,223.090302,-14.465774,8.087405,124.449333,-31.187304,10.495727
+-0.172735,-0.601032,-0.770531,226.844086,38.682209,-1.220740,124.449333,-31.187304,10.495727
+-0.209906,-0.911588,-0.770165,225.180817,47.853024,6.958220,136.444443,-29.687914,17.392920
+-0.086917,-1.063997,-0.643025,215.277573,81.209755,-34.501175,136.444443,-29.687914,17.392920
+-0.180670,-0.999847,-0.398328,205.664230,85.528122,-54.216133,136.444443,-29.687914,17.392920
+-0.072390,-1.064913,0.068545,209.860535,79.989014,-76.097900,143.941391,-34.485958,27.588768
+0.075076,-0.999176,-0.044557,214.270462,63.814201,-89.449753,143.941391,-34.485958,27.588768
+0.058596,-0.857875,0.140629,203.039642,64.104126,-99.978638,141.542374,-36.285229,40.783398
+0.292184,-0.869045,0.350475,197.515793,48.203987,-76.357307,141.542374,-36.285229,40.783398
+0.121769,-0.756066,0.333872,165.776550,22.614216,-76.799828,141.542374,-36.285229,40.783398
+0.365184,-0.812403,0.603168,122.791222,29.068880,-66.270943,134.045425,-39.883762,48.880100
+0.176458,-0.751671,0.633747,115.634636,20.523697,-71.794792,134.045425,-39.883762,48.880100
+0.338633,-0.768639,0.701743,103.488266,5.279702,-80.568863,129.247375,-41.383152,51.579002
+0.320017,-0.616535,0.736534,92.684715,-8.621479,-88.229012,129.247375,-41.383152,51.579002
+0.309275,-0.467360,0.741661,123.126923,0.549333,-82.857750,120.700859,-42.432724,54.427841
+0.450575,-0.283578,0.733177,186.544388,21.256142,-77.318642,120.700859,-42.432724,54.427841
+0.386853,-0.203681,0.806848,146.992401,52.323986,-58.488724,120.700859,-42.432724,54.427841
+0.349132,-0.045778,0.884060,86.199532,47.929317,-66.652428,111.854462,-40.483517,54.277901
+0.320688,0.122684,0.969573,41.627247,50.965912,-44.557022,111.854462,-40.483517,54.277901
+0.170171,0.128849,0.964812,32.975250,36.896877,-17.197180,107.956047,-38.084496,54.577782
+0.227424,0.043153,1.011872,-33.005768,26.078066,4.135258,107.956047,-38.084496,54.577782
+0.240852,-0.043947,1.180700,-120.059814,13.260292,14.862514,107.956047,-38.084496,54.577782
+0.218818,-0.095035,1.143345,-172.292252,-26.261177,17.700735,109.305496,-37.034924,59.225887
+0.242561,-0.164251,1.072787,-210.058899,-48.570210,41.886654,109.305496,-37.034924,59.225887
+0.295175,-0.334117,0.962737,-206.595047,-73.046051,84.749901,118.751656,-40.783398,59.075947
+0.236579,-0.560747,0.724631,-153.843811,-88.656273,113.971375,118.751656,-40.783398,59.075947
+0.249763,-0.763573,0.535295,-73.274940,-60.304573,122.699669,118.751656,-40.783398,59.075947
+0.255135,-0.943999,0.594562,-36.454361,-25.513474,117.801445,130.296951,-41.533092,51.728939
+0.327769,-0.995453,0.568804,-68.269905,-36.866360,102.786339,130.296951,-41.533092,51.728939
+0.177862,-0.876675,0.464919,-190.664383,-44.434948,94.347969,138.693527,-38.834190,47.830528
+0.139897,-0.797510,0.381909,-247.810303,-95.233009,103.381454,138.693527,-38.834190,47.830528
+0.008484,-0.769189,0.041993,-232.993561,-137.134918,112.521744,138.693527,-38.834190,47.830528
+-0.052492,-0.898221,-0.314402,-263.054291,-127.140114,126.133003,143.491577,-33.436386,32.536755
+-0.152776,-1.018952,-0.484451,-313.318268,-105.395676,104.449600,143.491577,-33.436386,32.536755
+-0.290780,-0.928983,-0.728843,-339.655762,-75.487534,34.089176,135.844696,-24.889866,14.394140
+-0.306894,-0.630879,-0.881558,-309.915466,-0.427259,-20.523697,135.844696,-24.889866,14.394140
+-0.423597,-0.315867,-1.165380,-189.016388,90.792564,-39.307838,135.844696,-24.889866,14.394140
+-0.317087,-0.165532,-1.086398,-97.323524,159.535507,-18.219551,118.601715,-26.839073,10.945544
+-0.258492,-0.086001,-0.926908,-73.122345,212.332535,21.256142,118.601715,-26.839073,10.945544
+-0.030580,-0.003113,-0.568133,-95.492416,187.780396,20.249031,114.403427,-36.435165,17.842735
+0.279366,-0.110172,-0.214545,-201.971497,63.554794,17.410810,114.403427,-36.435165,17.842735
+0.094546,0.023988,-0.350658,-246.955780,-86.474197,-48.432873,114.403427,-36.435165,17.842735
+-0.218940,0.397229,-0.549883,-185.079498,-160.054321,-148.609879,105.557022,-37.484737,20.691576
+-0.477554,0.568011,-0.737877,-88.930939,-166.127502,-179.143646,105.557022,-37.484737,20.691576
+-0.800073,0.412976,-0.847804,75.136574,-179.342026,-201.910461,96.560684,-23.390476,20.391699
+-0.796777,0.479324,-0.844569,161.656540,-206.198303,-168.019653,96.560684,-23.390476,20.391699
+-0.907865,0.377331,-0.941740,53.865170,-201.773132,-74.510941,96.560684,-23.390476,20.391699
+-1.129795,0.486404,-0.656453,-104.419083,-147.724838,-36.484879,102.258369,-11.395361,15.593652
+-1.144383,0.491897,-0.372387,-49.958801,-156.682022,-13.931700,102.258369,-11.395361,15.593652
+-0.994476,0.271981,-0.461379,93.554489,-132.618179,-25.605030,100.609039,-1.049573,22.640781
+-0.768273,0.057375,-0.462355,156.361588,-86.550491,-35.431988,100.609039,-1.049573,22.640781
+-0.928251,-0.166326,-0.509903,136.066772,-16.006958,-63.829464,100.609039,-1.049573,22.640781
+-1.041170,-0.196722,-0.591266,160.512100,44.877468,-99.673454,107.506233,2.848840,19.941881
+-0.800256,-0.036317,-0.462966,231.635483,88.976715,-118.335518,107.506233,2.848840,19.941881
+-0.851161,-0.158452,-0.265755,235.831787,103.473007,-95.263527,120.700859,0.749695,14.843957
+-0.863063,-0.363109,-0.167058,198.400833,101.702934,-87.786491,120.700859,0.749695,14.843957
+-0.858730,-0.607440,-0.372509,103.061005,71.520126,-74.510941,120.700859,0.749695,14.843957
+-0.745872,-0.616474,-0.288156,20.203253,91.143532,-85.741753,133.295731,-5.547741,14.843957
+-0.687460,-0.780236,-0.448866,69.643242,95.156715,-106.997894,133.295731,-5.547741,14.843957
+-0.652730,-0.820887,-0.403882,105.685600,136.082031,-147.816406,137.344086,-14.694017,15.293774
+-0.430738,-0.835902,-0.409192,129.001740,134.968109,-155.018768,137.344086,-14.694017,15.293774
+-0.129948,-0.571062,-0.293954,130.466629,161.580246,-152.592545,137.344086,-14.694017,15.293774
+-0.134648,-0.973724,-0.422193,124.607071,113.513596,-124.088257,140.192917,-27.138950,19.642004
+0.091800,-0.930326,0.122929,114.642784,141.331223,-122.455521,140.192917,-27.138950,19.642004
+0.099246,-1.052583,-0.180120,61.601612,91.097748,-137.043365,140.642746,-35.085716,28.488401
+0.235115,-0.853603,0.289376,20.233772,76.509903,-113.711967,140.642746,-35.085716,28.488401
+0.194220,-1.000885,0.074160,74.633018,27.802362,-92.440567,140.642746,-35.085716,28.488401
+0.537248,-0.926725,0.287179,95.858635,27.863400,-93.798637,135.095001,-40.633457,33.736263
+0.487503,-0.990326,0.111637,15.411847,27.008881,-106.006042,135.095001,-40.633457,33.736263
+0.605670,-0.901639,0.251839,-127.750481,-13.122959,-98.651085,131.196579,-44.531872,34.336021
+0.460036,-0.758263,0.087527,-258.949554,-56.367687,-80.782494,131.196579,-44.531872,34.336021
+0.672201,-0.516373,-0.090335,-292.321533,-99.185158,-65.858940,131.196579,-44.531872,34.336021
+0.667440,-0.419935,-0.375317,-308.359009,-117.847221,-39.155247,121.600494,-43.632236,27.738707
+0.487808,-0.196112,-0.641560,-313.547180,-130.512405,-24.079103,121.600494,-43.632236,27.738707
+0.444044,0.015076,-0.837611,-239.661850,-145.817444,-35.309914,109.755318,-40.183640,24.290112
+0.302927,0.253365,-0.990875,-196.127197,-148.884552,-28.534807,109.755318,-40.183640,24.290112
+0.235420,0.369030,-0.837245,-170.339066,-152.668839,-29.358807,109.755318,-40.183640,24.290112
+0.065188,0.547441,-0.885708,-197.668381,-149.708542,-29.725029,102.858124,-32.386814,23.390476
+-0.012879,0.610736,-0.756798,-196.035645,-132.435074,-31.922361,102.858124,-32.386814,23.390476
+-0.023865,0.983184,-0.720786,-238.212219,-97.231972,-28.519547,102.858124,-32.386814,23.390476
+-0.051088,1.051668,-0.617695,-224.768829,-61.265907,10.315256,102.858124,-32.386814,23.390476
+-0.035707,0.940397,-0.590350,-160.573135,-26.001770,57.176426,102.858124,-32.386814,23.390476
+-0.045289,0.857692,-0.431104,-136.875519,9.918515,100.100708,93.861786,-27.588768,40.483517
+-0.001099,0.763207,-0.268563,-145.084991,15.045625,114.307076,93.861786,-27.588768,40.483517
+0.172979,0.766503,-0.068361,-166.035950,-0.274667,104.998932,97.310379,-32.536755,48.130405
+0.131779,0.907682,0.007202,-169.591354,1.251259,87.908569,97.310379,-32.536755,48.130405
+0.197394,0.961028,0.035218,-159.733871,3.402814,91.067230,97.310379,-32.536755,48.130405
+0.282418,0.909146,0.068117,-123.661003,34.745323,91.448715,105.557022,-36.285229,55.777290
+0.447218,0.954375,0.373913,-103.274635,57.893612,85.711235,105.557022,-36.285229,55.777290
+0.549944,0.847438,0.596149,-61.433758,52.247688,90.990936,113.953606,-36.585106,60.575336
+0.358104,0.768822,0.563372,-58.610798,48.615986,95.858635,113.953606,-36.585106,60.575336
+0.380139,0.580584,0.709189,-85.634941,68.346199,87.679680,113.953606,-36.585106,60.575336
+0.558550,0.604389,0.944243,-67.171242,44.404430,88.839378,120.850800,-34.186081,62.674484
+0.397656,0.453200,0.893460,-2.746666,14.618366,103.137306,120.850800,-34.186081,62.674484
+0.570147,0.341624,0.962981,1.800592,-29.023102,124.362923,126.848358,-31.187304,62.974361
+0.467849,0.330638,0.876247,16.296885,-74.617752,143.055511,126.848358,-31.187304,62.974361
+0.407483,0.209296,0.750816,49.943542,-110.156563,166.142761,130.746765,-32.386814,60.875217
+0.496109,-0.087954,0.312571,79.577011,-90.502640,160.466324,130.746765,-32.386814,60.875217
+0.886074,-0.075442,0.577837,115.543076,-81.270790,148.380997,130.746765,-32.386814,60.875217
+0.846156,-0.155278,0.419080,109.500412,-104.281746,146.717728,132.096222,-33.436386,59.825642
+0.899808,-0.159368,0.347240,62.364574,-71.214943,108.645897,132.096222,-33.436386,59.825642
+1.034761,-0.166326,0.380749,14.465774,-52.613911,64.165169,132.995850,-33.136509,59.225887
+1.055269,-0.089908,0.445753,14.648885,-53.819393,34.745323,132.995850,-33.136509,59.225887
+1.006439,0.007691,0.387768,51.301613,-84.093750,32.898952,132.995850,-33.136509,59.225887
+0.946867,-0.059572,0.304392,80.553604,-121.265297,67.613754,135.844696,-36.585106,56.377048
+0.903531,-0.306284,0.074587,99.887085,-146.732986,69.383835,135.844696,-36.585106,56.377048
+0.866848,-0.474197,-0.303476,20.798365,-113.711967,27.329325,134.345306,-38.984127,52.778511
+0.986541,-0.455336,-0.472488,-90.533157,-51.881466,-25.971252,134.345306,-38.984127,52.778511
+1.123814,-0.374767,-0.442763,-121.448410,2.426222,-74.694054,134.345306,-38.984127,52.778511
+1.039827,-0.329722,-0.422437,-103.289894,31.144138,-116.443375,134.495239,-42.732601,47.230770
+1.008209,-0.318003,-0.079470,-121.005890,66.164131,-165.486618,134.495239,-42.732601,47.230770
+0.929472,-0.036500,0.065981,-128.696548,30.701620,-230.582596,127.747993,-45.581444,46.780952
+0.757714,0.306284,0.042848,-88.457901,-1.770074,-274.895477,127.747993,-45.581444,46.780952
+0.305002,0.492447,-0.173956,-53.880428,-0.854518,-292.046875,127.747993,-45.581444,46.780952
+0.022523,0.785363,0.015870,-68.346199,-27.115696,-302.667328,112.304276,-44.231991,46.631016
+-0.119022,0.981964,-0.014710,-47.135838,-33.906063,-300.698883,112.304276,-44.231991,46.631016
+-0.324839,1.036042,0.063723,-30.457472,-20.340586,-257.286285,97.160446,-36.585106,47.380711
+-0.500443,1.035493,0.197333,15.549181,-38.850063,-246.681107,97.160446,-36.585106,47.380711
+-0.735130,0.833399,0.149113,42.313915,-58.397167,-239.585556,97.160446,-36.585106,47.380711
+-0.744285,0.826746,0.132267,67.323830,-69.063385,-241.157257,89.663498,-22.790722,44.381931
+-0.768822,0.749718,-0.069399,117.969299,-95.477158,-216.635635,89.663498,-22.790722,44.381931
+-1.024018,0.626728,-0.326426,129.810486,-67.949463,-178.670609,88.613922,-9.146276,37.934555
+-0.937284,0.493362,-0.256294,199.011200,-43.748283,-154.545731,88.613922,-9.146276,37.934555
+-0.889370,0.492874,-0.441176,292.870880,-39.124729,-140.385147,88.613922,-9.146276,37.934555
+-0.952849,0.266060,-0.755943,329.691467,20.752586,-124.500259,95.810989,0.449817,25.639561
+-0.973662,0.103580,-0.650960,281.685852,90.975677,-131.595810,95.810989,0.449817,25.639561
+-1.066561,-0.067141,-0.663961,239.112518,123.783073,-144.291519,115.452995,0.299878,13.794384
+-0.935575,-0.190435,-0.688437,206.091492,142.994476,-150.486771,115.452995,0.299878,13.794384
+-0.909513,-0.336619,-0.616535,166.325882,160.451065,-147.419662,115.452995,0.299878,13.794384
+-0.720054,-0.578204,-0.568499,118.732262,176.091797,-128.986481,130.596832,-8.846398,11.545300
+-0.609394,-0.837306,-0.485244,86.748863,168.553726,-133.869446,130.596832,-8.846398,11.545300
+-0.507340,-0.904508,-0.487320,61.830502,169.499802,-150.395218,135.694748,-24.140171,15.143834
+0.003052,-0.850551,-0.584246,80.217903,169.530319,-161.076691,135.694748,-24.140171,15.143834
+0.104678,-0.752647,-0.506729,108.920563,165.898621,-147.053436,135.694748,-24.140171,15.143834
+0.158879,-0.815638,-0.278329,160.817291,157.811218,-134.647659,135.244934,-35.685471,20.391699
+0.370312,-0.708762,-0.258126,173.940247,177.816101,-142.490921,135.244934,-35.685471,20.391699
+0.392529,-0.591449,0.037538,233.146149,190.435501,-104.846336,132.546036,-44.981686,32.686691
+0.508927,-0.521867,0.357616,310.113831,143.131805,-52.125614,132.546036,-44.981686,32.686691
+0.267159,-0.549821,0.441176,313.760803,143.284409,-60.533463,132.546036,-44.981686,32.686691
+0.643513,-0.506912,0.948637,222.266312,121.738335,-53.468430,126.698418,-44.231991,49.629795
+0.273873,-0.410047,0.962737,101.779228,71.504868,-56.718651,126.698418,-44.231991,49.629795
+0.323191,-0.338328,1.038301,17.059847,39.582508,-68.559830,119.351410,-42.282787,56.077171
+0.175542,-0.325144,1.160314,-23.377178,35.218361,-84.673607,119.351410,-42.282787,56.077171
+0.384899,-0.375988,1.327067,-46.540726,-27.237770,-92.501602,119.351410,-42.282787,56.077171
+0.161748,-0.491348,1.074740,-61.510056,-99.536118,-150.166321,119.351410,-42.282787,56.077171
+0.313242,-0.299142,0.934172,-95.477158,-105.624557,-182.164978,119.351410,-42.282787,56.077171
+0.606769,-0.049928,0.831813,-85.802788,-81.270790,-158.177429,111.104767,-41.832970,52.928452
+0.819910,-0.017518,0.819178,-45.045319,-102.130196,-121.707817,111.104767,-41.832970,52.928452
+0.650166,-0.283639,0.726279,-96.865746,-120.349739,-81.331825,111.104767,-41.832970,52.928452
+0.693014,-0.230049,0.574664,-146.061584,-141.773743,-128.513443,106.006844,-42.132847,48.730160
+0.805628,0.102542,0.498245,-174.306473,-150.959808,-137.745285,106.006844,-42.132847,48.730160
+0.836024,0.252632,0.331614,-192.098755,-157.216095,-105.945007,102.858124,-40.483517,47.680588
+0.883206,0.276559,0.172369,-173.284103,-164.937286,-74.571976,102.858124,-40.483517,47.680588
+0.814844,0.192389,-0.004151,-131.611069,-145.908997,-46.998505,102.858124,-40.483517,47.680588
+0.873440,0.250557,-0.261849,-81.514938,-129.016998,-37.095249,101.208794,-40.033703,44.831749
+0.975738,0.222724,-0.210211,-33.310951,-103.350929,-9.887997,101.208794,-40.033703,44.831749
+0.862026,0.177679,-0.235908,-35.126804,-133.777893,-2.548295,102.258369,-39.284008,41.982906
+0.853359,0.071352,-0.241157,-55.391094,-178.594315,-26.398510,102.258369,-39.284008,41.982906
+0.488113,0.413953,-0.516312,-113.055817,-217.810608,-30.228584,102.258369,-39.284008,41.982906
+0.489334,0.419996,-0.770592,-131.046478,-204.199341,-24.491104,100.609039,-38.834190,35.835411
+0.227912,0.578875,-0.930387,-135.242767,-126.087219,-26.322214,100.609039,-38.834190,35.835411
+0.366588,0.862148,-0.522050,-94.607376,-109.561447,-9.872738,97.160446,-35.385593,35.685471
+0.267891,0.751549,-0.719138,-55.177464,-142.094177,6.897183,97.160446,-35.385593,35.685471
+0.062197,0.619160,-0.746239,-51.881466,-119.434189,-8.285775,97.160446,-35.385593,35.685471
+0.122623,0.555864,-0.543901,-31.571398,-105.410934,-44.633320,98.060074,-34.785839,33.286449
+0.002686,0.572405,-0.519852,28.077028,-158.223206,-88.137459,98.060074,-34.785839,33.286449
+-0.249214,0.659932,-0.781945,65.721611,-185.155792,-106.906342,96.560684,-28.788280,29.088158
+-0.443556,0.661702,-0.756432,54.216133,-164.754166,-80.736717,96.560684,-28.788280,29.088158
+-0.718894,0.559221,-0.848354,54.170353,-164.525284,-70.986053,96.560684,-28.788280,29.088158
+-0.601276,0.426221,-0.557329,83.559677,-148.640396,-63.676868,95.061295,-20.391699,22.490843
+-0.776147,0.257759,-0.730796,149.479660,-137.852112,-58.091984,95.061295,-20.391699,22.490843
+-0.948515,-0.233772,-0.539933,213.675339,-131.855225,-62.807091,99.559464,-12.294994,17.093040
+-0.755028,-0.284249,-0.590655,247.306747,-112.765892,-108.874779,99.559464,-12.294994,17.093040
+-0.694540,-0.221809,-0.697348,209.280685,-71.062347,-96.591080,99.559464,-12.294994,17.093040
+-0.747765,-0.257698,-0.655965,209.204376,-16.052736,-68.605606,114.703300,-4.648108,13.644445
+-0.777978,-0.475967,-0.487564,227.240814,19.363995,-58.870205,114.703300,-4.648108,13.644445
+-0.829493,-0.677206,-0.429518,152.607803,40.391247,-78.951385,128.647629,-4.498168,15.293774
+-0.830714,-0.785791,-0.545061,66.408279,43.794060,-103.259377,128.647629,-4.498168,15.293774
+-0.813990,-0.688864,-0.579608,19.486069,79.561752,-113.818779,135.394882,-8.546520,16.343346
+-0.756737,-0.671468,-0.418775,75.319679,113.559372,-94.073303,135.394882,-8.546520,16.343346
+-0.403150,-0.971282,-0.392224,156.208984,120.899078,-118.884857,135.394882,-8.546520,16.343346
+-0.495560,-0.908963,-0.331797,125.186928,128.604996,-144.627213,139.593170,-18.742369,17.542858
+-0.490249,-0.902860,-0.261116,76.494644,158.146912,-125.782036,139.593170,-18.742369,17.542858
+-0.201300,-1.046480,-0.006714,123.355812,144.398331,-118.259224,141.842255,-29.987791,25.489622
+-0.118412,-1.049287,0.114139,127.857292,146.244690,-157.689133,141.842255,-29.987791,25.489622
+0.070803,-1.070406,0.137211,116.870628,90.899384,-169.743942,141.842255,-29.987791,25.489622
+0.196661,-0.782800,0.192938,118.274483,89.541306,-168.752090,138.543594,-40.483517,34.186081
+0.305795,-0.851100,0.267403,140.583511,78.691978,-157.444992,138.543594,-40.483517,34.186081
+0.449660,-0.732994,0.416211,152.165283,76.021606,-172.444839,130.746765,-45.281563,42.582664
+0.534257,-0.624287,0.342112,114.703819,39.857174,-170.354324,130.746765,-45.281563,42.582664
+0.597430,-0.441176,0.565630,48.432873,40.772728,-158.085876,130.746765,-45.281563,42.582664
+0.622456,-0.100101,0.405896,68.575089,20.294809,-140.049438,119.651283,-45.881321,46.181198
+0.687521,0.183538,0.654134,142.155212,68.758202,-84.185310,119.651283,-45.881321,46.181198
+0.542863,0.196905,0.696005,220.297867,86.840416,-28.321177,111.854462,-44.681808,48.280342
+0.539201,0.210700,0.883389,216.956085,91.189308,11.276589,111.854462,-44.681808,48.280342
+0.430006,0.276009,0.736473,102.740562,87.572861,35.706657,111.854462,-44.681808,48.280342
+0.420911,0.292123,0.816614,-52.629169,102.694786,112.720116,109.455437,-44.082054,50.679367
+0.132694,0.024293,1.070284,-145.802185,77.394943,179.296249,109.455437,-44.082054,50.679367
+0.035524,-0.284127,0.956145,-191.061127,80.263680,198.004089,120.550919,-42.582664,55.477413
+0.066286,-0.748070,0.908780,-170.857880,93.997009,188.024536,120.550919,-42.582664,55.477413
+0.082583,-0.783288,0.683554,-113.925598,103.076263,155.034027,120.550919,-42.582664,55.477413
+0.087588,-1.040376,0.707785,-209.967346,78.035828,153.492844,134.795120,-36.435165,56.227108
+-0.247993,-0.539262,0.498917,-332.575470,1.373333,172.246460,134.795120,-36.435165,56.227108
+-0.197089,-0.926054,0.347972,-320.291748,-5.310221,193.090607,147.539932,-26.689135,44.082054
+-0.428175,-1.077487,0.249336,-231.116669,-51.271095,212.302017,147.539932,-26.689135,44.082054
+-0.528275,-1.098788,-0.186590,-223.593857,-55.528427,149.784851,147.539932,-26.689135,44.082054
+-0.606159,-0.877712,-0.404553,-270.806000,-48.692284,124.225594,147.689865,-15.443712,28.638340
+-0.594989,-0.717978,-0.318491,-382.732635,-43.702507,120.288704,147.689865,-15.443712,28.638340
+-0.732017,-0.300424,-0.462416,-341.975159,-50.813316,78.127380,132.695969,-7.646887,13.344567
+-0.863613,-0.165105,-0.572283,-275.185394,-25.086214,68.178352,132.695969,-7.646887,13.344567
+-0.750938,-0.133854,-0.729759,-88.641014,16.495255,16.617329,132.695969,-7.646887,13.344567
+-0.899258,-0.130619,-0.619587,34.745323,80.904572,-23.056734,118.451775,-6.597314,11.995116
+-0.775414,-0.190985,-0.652364,98.559525,95.767082,-60.228279,118.451775,-6.597314,11.995116
+-0.928861,-0.184271,-0.645283,-93.935974,110.309151,-23.392437,120.850800,-12.294994,11.995116
+-0.802942,-0.299203,-0.758202,-217.841125,77.105011,-13.046662,120.850800,-12.294994,11.995116
+-0.611774,-0.031861,-0.373852,-329.645691,99.963379,10.895108,120.850800,-12.294994,11.995116
+-0.711692,0.289438,-0.558367,-302.194275,46.418652,-5.310221,106.606598,-18.142614,13.644445
+-0.259468,0.503800,-0.120060,-379.009369,54.170353,45.991394,106.606598,-18.142614,13.644445
+-0.633808,0.971038,-0.190741,-326.273376,16.617329,58.000427,91.162888,-18.892309,32.386814
+-0.574847,0.950774,-0.393323,-254.692215,21.652882,61.906796,91.162888,-18.892309,32.386814
+-0.561480,0.947172,-0.113895,-217.627487,48.554947,80.675682,91.162888,-18.892309,32.386814
+-0.413343,1.114109,-0.199896,-110.019226,66.530350,82.049011,92.062515,-23.390476,48.580223
+-0.340587,1.079806,-0.011536,-73.717461,87.405014,137.623215,92.062515,-23.390476,48.580223
+-0.167364,0.915616,0.117069,-63.432720,77.669609,172.658463,96.710625,-28.038586,55.927231
+0.090457,0.807886,0.177801,-46.342358,52.369762,165.532394,96.710625,-28.038586,55.927231
+0.097354,0.780175,0.147771,-36.561172,31.876583,155.430771,96.710625,-28.038586,55.927231
+0.326243,0.885891,0.240486,23.453474,37.049469,142.857147,103.158005,-31.787058,58.776070
+0.413953,0.973662,0.398389,196.218750,29.999695,144.169434,103.158005,-31.787058,58.776070
+0.391797,0.783349,0.138127,184.865875,11.291848,145.374924,104.357513,-35.985348,55.177536
+0.703024,0.776330,0.093509,134.952850,-3.509629,96.652122,104.357513,-35.985348,55.177536
+0.618793,0.816309,0.008667,154.164246,-10.544145,94.912567,104.357513,-35.985348,55.177536
+0.675680,0.583880,-0.090213,266.121399,-43.122654,102.526932,106.156784,-40.483517,48.880100
+0.655293,0.229438,-0.182989,320.154419,-54.277168,95.400864,106.156784,-40.483517,48.880100
+0.609272,0.041322,-0.565630,237.235626,-81.591232,66.621902,106.156784,-43.182419,37.484737
+0.555498,-0.117557,-0.862087,161.259811,-81.118195,30.991547,106.156784,-43.182419,37.484737
+0.636372,-0.089846,-0.828578,156.804108,-58.153019,13.016145,106.156784,-43.182419,37.484737
+0.526749,-0.080691,-0.947966,235.480820,-29.099400,25.238808,112.154343,-44.082054,28.488401
+0.575274,-0.263497,-0.859340,259.666748,-3.997925,48.432873,112.154343,-44.082054,28.488401
+0.578448,-0.512040,-0.724693,245.719772,13.611256,53.834652,121.450554,-44.082054,27.288891
+0.463942,-0.722800,-0.740379,195.883057,32.837917,46.311836,121.450554,-44.082054,27.288891
+0.555803,-0.797815,-0.397351,186.574905,48.417614,29.938658,121.450554,-44.082054,27.288891
+0.548173,-0.859645,-0.225593,204.779205,37.903988,14.572588,126.098663,-43.032482,29.837852
+0.524369,-0.834803,-0.124210,191.244232,28.199102,10.757774,126.098663,-43.032482,29.837852
+0.558245,-0.956694,0.091922,181.142609,29.816584,-2.243110,130.896713,-45.131626,35.535534
+0.595782,-0.904141,0.167730,178.868988,19.150364,-18.555254,130.896713,-45.131626,35.535534
+0.581378,-0.728355,0.290414,183.278915,13.321329,-15.152440,130.896713,-45.131626,35.535534
+0.538102,-0.643086,0.342845,176.686905,13.855403,-3.097629,130.596832,-45.131626,39.733822
+0.514176,-0.618915,0.426588,139.194916,23.560289,8.529923,130.596832,-45.131626,39.733822
+0.477248,-0.609027,0.539018,98.620560,35.706657,13.290811,128.197800,-45.731380,44.831749
+0.459426,-0.629536,0.607318,35.767693,32.532730,11.108737,128.197800,-45.731380,44.831749
+0.454299,-0.672079,0.590411,-103.366188,21.744438,29.511398,128.197800,-45.731380,44.831749
+0.300607,-0.819300,0.590289,-195.577866,-3.585925,27.603992,129.097443,-43.332359,46.031258
+0.370434,-0.812708,0.382275,-152.760406,-21.195105,36.332287,129.097443,-43.332359,46.031258
+0.384716,-1.071322,0.142094,-170.323807,-18.280586,51.484726,133.145798,-42.582664,42.282787
+0.351024,-1.033723,0.044923,-215.948975,-22.324289,39.551987,133.145798,-42.582664,42.282787
+0.370678,-1.032441,-0.004151,-328.547028,-56.352428,43.061615,133.145798,-42.582664,42.282787
+0.225471,-0.874538,-0.209052,-398.754852,-103.762932,18.936735,135.844696,-41.383152,31.187304
+0.262154,-0.474990,-0.401990,-339.259003,-145.954773,-9.918515,135.844696,-41.383152,31.187304
+0.177984,-0.275155,-0.786279,-237.525558,-157.246613,-1.556444,125.199028,-37.034924,17.242979
+0.070986,-0.159856,-0.982574,-205.420090,-160.359512,-0.930815,125.199028,-37.034924,17.242979
+-0.141057,-0.118534,-0.989776,-252.754288,-155.110321,9.369183,112.154343,-28.488401,13.494506
+-0.423780,0.061586,-0.847011,-341.837830,-170.064392,-25.391399,112.154343,-28.488401,13.494506
+-0.546159,0.507462,-0.645039,-368.633087,-165.562912,-67.842644,112.154343,-28.488401,13.494506
+-0.623249,0.938810,-0.507035,-323.633545,-134.769745,-79.409164,95.511116,-19.941881,23.840294
+-0.685751,1.061495,-0.346812,-257.652527,-90.777306,-49.729912,95.511116,-19.941881,23.840294
+-0.999664,1.169164,-0.050356,-197.027496,-42.603840,3.982666,90.113312,-13.644445,40.633457
+-0.638936,1.132603,0.009827,-113.635674,-12.329478,81.011383,90.113312,-13.644445,40.633457
+-0.653829,1.052644,0.011719,2.761925,10.284738,147.587509,90.113312,-13.644445,40.633457
+-0.561357,0.848476,-0.108524,44.038208,22.965178,197.195343,91.462761,-20.991453,44.981686
+-0.549516,0.684591,-0.284005,-36.286507,37.736137,225.287643,91.462761,-20.991453,44.981686
+-0.370617,0.732261,-0.133793,-70.360420,42.222359,227.164520,96.410751,-32.836632,48.130405
+-0.217780,0.861171,-0.166448,44.389172,40.498062,222.312088,96.410751,-32.836632,48.130405
+0.217109,0.764672,0.113102,153.798035,42.435986,228.339493,96.410751,-32.836632,48.130405
+0.402661,0.527482,-0.036744,145.069733,23.453474,208.456680,104.357513,-42.282787,46.181198
+0.687216,0.443739,-0.026917,195.822021,4.638813,147.083954,104.357513,-42.282787,46.181198
+0.804956,0.335459,-0.082461,201.437424,-21.347698,91.753899,111.554581,-45.581444,41.383152
+0.765770,0.350291,-0.324839,161.824402,-61.037018,32.197029,111.554581,-45.581444,41.383152
+0.641194,0.433790,-0.586200,192.861725,-91.174049,-21.393475,111.554581,-45.581444,41.383152
+0.480422,0.328318,-0.744774,199.804688,-84.154793,-37.919247,111.554581,-45.881321,32.386814
+0.391552,0.274361,-0.925748,185.247345,-103.076263,-36.576435,111.554581,-45.881321,32.386814
+0.293893,0.199408,-1.134373,107.745598,-103.473007,-23.331400,113.053970,-44.681808,26.389256
+0.221809,0.068117,-1.096835,39.033173,-78.981903,-18.173773,113.053970,-44.681808,26.389256
+0.170110,0.094119,-0.899319,25.116734,-74.983978,-22.171698,113.053970,-44.681808,26.389256
+0.097659,0.054445,-0.944731,49.409466,-115.131081,-24.704733,114.553360,-41.683029,22.190966
+0.002441,0.153569,-0.930143,-10.742516,-120.944855,-20.065920,114.553360,-41.683029,22.190966
+-0.196722,0.171697,-1.082614,-66.957611,-133.518478,-12.756737,114.103546,-35.835411,14.843957
+-0.164006,0.044313,-1.103610,32.151249,-107.593002,-29.786066,114.103546,-35.835411,14.843957
+-0.258370,-0.239326,-1.098422,238.547928,-53.117466,-48.982208,114.103546,-35.835411,14.843957
+-0.104495,-0.541215,-0.837611,364.772491,-23.270363,-53.529465,120.101105,-29.837852,11.845178
+-0.101260,-0.918790,-0.815943,368.205811,-13.794366,-56.794945,120.101105,-29.837852,11.845178
+0.150822,-1.325968,-0.215339,274.468201,17.441328,-52.262947,137.643967,-31.187304,17.692797
+0.078555,-1.123203,-0.222785,216.666153,15.717032,-61.616871,137.643967,-31.187304,17.692797
+0.059633,-1.220618,-0.355968,158.116394,2.716147,-79.424423,137.643967,-31.187304,17.692797
+0.135685,-0.956389,-0.227729,164.021729,30.396435,-98.513748,141.842255,-34.186081,26.689135
+0.158086,-0.884487,-0.122074,255.577255,63.463242,-102.496414,141.842255,-34.186081,26.689135
+0.048463,-1.011261,-0.051210,310.403748,82.476273,-94.347969,140.792679,-37.634678,38.534313
+0.272408,-0.804102,0.336192,323.938721,78.844566,-103.503525,140.792679,-37.634678,38.534313
+0.194403,-0.568926,0.542131,346.247742,48.463394,-94.683678,140.792679,-37.634678,38.534313
+0.304392,-0.436659,0.720542,304.208496,47.029022,-59.724724,130.746765,-39.883762,51.579002
+0.385693,-0.314829,0.925626,206.274612,47.120579,-61.494797,130.746765,-39.883762,51.579002
+0.173406,0.051027,1.023286,120.441299,47.654652,-78.783531,119.351410,-38.384373,57.876438
+0.206122,0.289560,1.168981,104.281746,44.663837,-48.142948,119.351410,-38.384373,57.876438
+0.123722,0.199835,0.837977,97.537155,35.218361,-28.061769,119.351410,-38.384373,57.876438
+-0.164434,0.118595,0.628071,5.615406,102.435379,-23.529770,113.203911,-36.435165,58.026375
+-0.454604,-0.057680,0.887600,-129.413742,142.384109,3.143406,113.203911,-36.435165,58.026375
+-0.428175,-0.310251,1.139012,-192.831207,131.260117,8.041627,119.051529,-29.987791,64.173874
+-0.535783,-0.506424,0.867153,-186.925873,135.364853,-14.496292,119.051529,-29.987791,64.173874
+-0.494583,-0.666524,1.157750,-223.044525,153.920105,-21.668142,119.051529,-29.987791,64.173874
+-0.581072,-0.693625,1.026521,-221.564377,107.455673,-39.826656,119.051529,-29.987791,64.173874
+-0.457839,-0.950468,0.918790,-227.530746,50.065617,-52.476578,119.051529,-29.987791,64.173874
+-0.494644,-0.899686,0.686361,-302.529999,24.796289,-55.681019,141.692307,-19.342125,60.425400
+-0.453383,-0.606342,0.513138,-225.043488,5.508591,-47.425762,141.692307,-19.342125,60.425400
+-0.090396,-0.609821,0.508744,-174.001282,-39.567249,18.906216,141.692307,-19.342125,60.425400
+-0.528336,-1.074984,0.038148,-263.679932,-140.995514,52.278206,148.889389,-22.340904,48.730160
+-0.503983,-0.633015,-0.110782,-339.808350,-113.910339,-7.095553,148.889389,-22.340904,48.730160
+-0.616047,-0.153935,-0.441054,-282.494568,-90.334785,55.833614,147.989746,-20.541637,26.239317
+-0.696310,-0.330027,-0.324168,-66.881317,-70.940277,99.215675,147.989746,-20.541637,26.239317
+-0.763512,-0.813318,-0.180548,-74.587234,-95.309303,109.591965,147.989746,-20.541637,26.239317
+-0.974700,-0.513138,-0.016663,-178.060242,-152.546768,35.096287,144.391220,-13.044689,19.941881
+-0.935636,-0.034120,-0.376232,-178.792694,-181.722458,49.409466,144.391220,-13.044689,19.941881
+-0.904813,-0.411328,-0.390637,-193.838318,-132.313004,69.414352,135.694748,-1.949206,17.242979
+-1.162938,-0.129948,-0.221381,-299.203461,-127.201149,48.234505,135.694748,-1.949206,17.242979
+-1.222144,0.124943,-0.132511,-298.821991,-148.472549,32.578510,135.694748,-1.949206,17.242979
+-1.086947,0.134709,-0.349437,-150.975067,-112.857445,35.508286,119.201469,4.947986,16.643225
+-1.107700,0.082583,-0.124332,-170.125427,-57.130650,57.466354,119.201469,4.947986,16.643225
+-0.969268,0.343699,0.083438,-225.135040,-52.354504,60.075687,110.505013,8.846398,26.239317
+-1.046846,0.445387,0.018250,-184.820099,-74.083679,121.982483,110.505013,8.846398,26.239317
+-0.871242,0.090762,0.018983,-172.566910,-11.169774,109.393600,110.505013,8.846398,26.239317
+-0.923856,-0.070070,0.367382,-234.916229,30.793177,52.980133,100.609039,6.147497,34.935776
+-0.786523,0.022095,0.424635,-291.009247,36.576435,6.073184,100.609039,6.147497,34.935776
+-0.804834,0.224311,0.524064,-295.236053,26.184881,15.106662,98.060074,5.097925,46.780952
+-0.865017,0.208747,0.572954,-321.176788,0.228889,66.423538,98.060074,5.097925,46.780952
+-1.116001,0.232185,0.251595,-323.526733,-48.280281,110.003967,98.060074,5.097925,46.780952
+-1.077609,0.153020,0.091372,-273.598450,-54.582355,167.195648,104.957268,1.499390,59.975582
+-1.129917,0.089419,0.070254,-171.086761,-26.017029,164.723648,104.957268,1.499390,59.975582
+-1.095737,0.207892,0.242134,-166.539505,-2.883999,145.084991,111.554581,-3.298657,67.472527
+-1.012055,0.364147,0.401624,-134.601883,-5.630665,157.307663,111.554581,-3.298657,67.472527
+-0.911283,0.299081,0.395032,-65.034943,-5.005035,171.224091,111.554581,-3.298657,67.472527
+-0.685629,0.351146,0.595172,-62.685017,-10.925627,177.556686,115.902809,-6.447375,67.922348
+-0.659078,0.416883,0.739219,-32.273323,-70.802940,187.734604,115.902809,-6.447375,67.922348
+-0.633442,0.373302,0.733116,-1.937925,-111.316261,218.466751,116.652504,-12.294994,68.971916
+-0.328074,0.393933,0.715110,8.606219,-128.788116,206.610306,116.652504,-12.294994,68.971916
+-0.297250,0.456923,0.506851,59.053314,-139.515366,175.923950,113.803665,-20.241758,68.222221
+0.087466,0.624958,0.282601,70.757164,-99.353004,164.403214,113.803665,-20.241758,68.222221
+0.182684,0.706687,0.504715,91.372414,-64.790794,135.959961,113.803665,-20.241758,68.222221
+0.217658,0.724998,0.380016,203.482162,3.494369,101.397751,112.754097,-27.288891,65.073509
+0.411817,0.783837,0.529618,222.540970,23.819696,37.522507,112.754097,-27.288891,65.073509
+0.119144,1.005524,0.311594,281.151764,5.554368,-22.263252,102.408310,-28.338463,58.326252
+0.228889,1.136876,0.154546,347.849976,-9.643849,-24.246956,102.408310,-28.338463,58.326252
+0.352977,0.749535,-0.048097,376.110107,-86.245308,-32.609028,102.408310,-28.338463,58.326252
+-0.252327,0.693381,-0.822291,194.799652,-117.160561,-0.564592,91.762642,-27.888645,40.483517
+-0.174444,0.539384,-1.064608,140.781891,-85.467087,-9.048738,91.762642,-27.888645,40.483517
+-0.067568,0.686422,-0.939970,169.530319,-40.391247,-2.395703,95.661057,-25.489622,28.788280
+0.018860,0.724509,-1.003204,180.501724,-25.711843,46.861172,95.661057,-25.489622,28.788280
+0.011536,0.410718,-1.006134,224.524673,-2.792444,96.423233,95.661057,-25.489622,28.788280
+0.196356,0.010926,-0.998871,277.596375,42.710655,129.734177,105.257149,-30.887424,19.192186
+0.111698,-0.277108,-0.597613,347.254852,109.149452,157.139801,105.257149,-30.887424,19.192186
+0.173467,-0.764672,-0.424879,472.960602,109.103668,141.163361,127.897926,-37.634678,16.943102
+0.117374,-1.215064,-0.162175,465.742981,147.526474,78.859825,127.897926,-37.634678,16.943102
+0.188238,-1.012177,0.198004,401.821960,186.101868,-14.343699,127.897926,-37.634678,16.943102
+0.454176,-0.832057,0.347423,383.098846,139.408554,-33.158360,139.743103,-40.783398,39.883762
+0.256844,-0.726157,0.591632,359.416473,146.839813,-34.073917,139.743103,-40.783398,39.883762
+0.155705,-0.584796,0.874172,317.590881,121.723076,-55.177464,128.047867,-35.985348,58.176315
+0.153386,-0.435438,0.896634,235.297714,118.030334,-51.362652,128.047867,-35.985348,58.176315
+0.015809,-0.128544,1.152135,227.973267,116.061890,-52.201912,128.047867,-35.985348,58.176315
+-0.090152,-0.055971,0.958892,222.052673,141.010773,-30.564287,115.003181,-30.437609,63.424179
+-0.260872,-0.101199,1.009980,149.327072,126.850182,-36.454361,115.003181,-30.437609,63.424179
+-0.604633,-0.156499,0.884793,21.225624,112.018188,-60.350353,106.906479,-23.840294,63.124302
+-0.644307,-0.219123,0.801416,-29.206213,111.804558,-81.133461,106.906479,-23.840294,63.124302
+-0.784814,-0.345042,0.793176,-97.918640,133.976257,-92.242195,106.906479,-23.840294,63.124302
+-0.610981,-0.352062,0.950224,-107.806633,130.771820,-114.810631,109.305496,-14.244201,65.223442
+-0.781457,-0.596088,0.774438,-3.173925,89.266640,-140.034180,109.305496,-14.244201,65.223442
+-0.809656,-0.985443,0.736106,-58.748131,103.869743,-248.863190,111.854462,-3.598535,65.973137
+-1.085788,-0.316538,0.829371,-59.495834,121.982483,-300.805695,111.854462,-3.598535,65.973137
+-0.704123,-0.384777,0.771630,78.325752,49.790947,-284.646149,111.854462,-3.598535,65.973137
+-0.556108,-0.764550,0.745994,-1.281777,-6.164739,-237.708664,125.798782,1.949206,66.722832
+-0.342112,-0.698569,1.065889,-111.407822,-64.104126,-213.461716,125.798782,1.949206,66.722832
+-0.423841,-0.682150,0.735313,-61.128574,-157.307663,-174.596390,138.393661,-10.045910,67.322594
+-0.168096,-0.548112,0.558184,-34.348583,-176.503799,-134.083069,138.393661,-10.045910,67.322594
+0.021668,-0.619037,0.556352,-16.785181,-215.765869,-71.291237,138.393661,-10.045910,67.322594
+0.126835,-0.691427,0.402661,44.816433,-260.826447,-18.677328,139.293289,-26.239317,64.323814
+-0.032289,-0.796228,0.119205,96.926788,-276.314575,34.669025,139.293289,-26.239317,64.323814
+0.050356,-0.965423,0.012818,62.227242,-258.552826,76.662498,134.795120,-38.834190,56.826866
+0.029481,-1.067049,-0.051271,-54.872280,-239.280380,58.427685,134.795120,-38.834190,56.826866
+0.120853,-1.119419,-0.407178,-247.398300,-251.060516,31.266212,134.795120,-38.834190,56.826866
+0.128971,-0.688070,-0.217475,-325.357819,-216.620377,-24.475845,140.492798,-41.533092,38.834190
+-0.215888,-0.400037,-0.541642,-110.477005,-280.007324,-43.076878,140.492798,-41.533092,38.834190
+-0.342479,-0.589251,-1.052950,7.034516,-249.198883,13.687551,135.544815,-36.884983,20.991453
+-0.293771,-0.863552,-1.024873,-93.401901,-243.003632,40.177616,135.544815,-36.884983,20.991453
+-0.446852,-0.329722,-1.011505,-326.227600,-196.371353,30.625324,135.544815,-36.884983,20.991453
+-0.444838,-0.049318,-0.920194,-399.624634,-207.388535,54.521317,126.398537,-25.339684,9.746032
+-0.794397,0.063845,-0.855983,-419.705811,-163.167206,15.488144,126.398537,-25.339684,9.746032
+-0.700034,0.519730,-0.727561,-376.552643,-127.491074,-49.790947,100.609039,-14.544079,15.443712
+-0.876003,0.417188,-0.665487,-373.348175,-73.061310,-45.243690,100.609039,-14.544079,15.443712
+-0.921659,0.704611,-0.401257,-357.371735,54.551834,-43.885616,100.609039,-14.544079,15.443712
+-0.658528,0.671834,0.108280,-387.783447,75.395981,26.413771,90.113312,-8.546520,36.735043
+-0.684774,0.630024,0.252266,-368.617828,38.880581,93.218788,90.113312,-8.546520,36.735043
+-0.631306,0.773217,0.099429,-152.668839,6.805628,126.911224,97.010506,-10.945544,58.926010
+-0.426344,0.713401,0.170660,319.772949,40.253914,154.148987,97.010506,-10.945544,58.926010
+-0.337046,0.248360,0.355846,500.000000,45.838802,99.337746,97.010506,-10.945544,58.926010
+-0.701682,-0.218696,-0.282907,500.000000,-9.445478,24.842066,90.563126,-17.392920,35.985348
+-0.750511,-0.284616,-0.968841,352.977081,-12.329478,-78.203682,90.563126,-17.392920,35.985348
+-0.594195,0.026246,-0.802637,237.067780,-93.096710,-75.121307,106.606598,-17.842735,11.245421
+-0.620136,-0.355602,-0.885159,313.043610,-97.720268,-39.368877,106.606598,-17.842735,11.245421
+-0.651387,-0.535661,-0.886319,295.327606,-41.550949,-54.338207,106.606598,-17.842735,11.245421
+-0.546098,-0.491775,-0.561235,270.470276,0.213630,-57.390057,124.899147,-13.344567,10.045910
+-0.483413,-0.692770,-0.422559,278.374573,21.454512,-45.213173,124.899147,-13.344567,10.045910
+-0.575518,-0.874722,-0.203192,264.534454,47.578358,-33.478806,140.342865,-16.193407,18.592430
+-0.518509,-0.993622,-0.078921,254.737991,58.061462,-48.142948,140.342865,-16.193407,18.592430
+-0.558611,-1.177648,0.000671,212.042603,50.645466,-54.399242,140.342865,-16.193407,18.592430
+-0.477187,-1.090426,0.079409,109.561447,126.270332,-66.942352,146.190475,-22.640781,30.137730
+-0.279794,-1.271096,0.743614,184.682755,78.295235,-101.977600,146.190475,-22.640781,30.137730
+-0.312082,-1.204077,0.228217,216.376236,11.062960,-157.261887,142.292068,-26.539196,36.735043
+-0.102969,-0.887906,0.339793,219.489120,22.110661,-145.100250,142.292068,-26.539196,36.735043
+-0.593158,-0.926969,0.074160,175.649277,15.121922,-114.001892,142.292068,-26.539196,36.735043
+-0.203986,-0.407544,0.347606,95.049896,29.175695,-79.027679,141.242493,-29.388035,47.980465
+-0.454237,-0.352794,0.377148,36.103397,57.435833,17.487106,141.242493,-29.388035,47.980465
+-0.337168,-0.492386,0.572588,25.055696,72.634056,55.177464,138.093781,-29.837852,52.928452
+-0.364696,-0.602557,0.760033,-127.155373,77.822197,88.061157,138.093781,-29.837852,52.928452
+-0.506912,-1.348979,0.820826,-473.143707,8.896146,123.798332,138.093781,-29.837852,52.928452
+-0.478652,-0.718223,0.683126,-500.015259,-115.146339,87.328712,141.092560,-24.140171,45.131626
+-0.845485,-0.507035,0.188726,-331.781982,-193.533127,125.934631,141.092560,-24.140171,45.131626
+-1.029939,-0.619037,-0.374279,-195.852539,-237.220367,149.418625,138.993408,-21.141394,21.441271
+-0.864711,-0.463515,-0.399487,-267.571045,-233.939636,164.464249,138.993408,-21.141394,21.441271
+-0.966155,-0.255379,-0.417371,-302.606293,-217.749557,137.623215,128.197800,0.149939,14.544079
+-1.148961,0.071657,-0.395764,-256.202881,-189.107941,151.814331,128.197800,0.149939,14.544079
+-1.230811,-0.097781,-0.699789,-128.177734,-137.272263,154.911957,128.197800,0.149939,14.544079
+-1.134312,0.010743,-0.611347,-132.068848,-33.188881,137.714767,113.053970,9.296215,22.490843
+-1.028901,0.113712,-0.323130,-187.459946,48.173466,158.070618,113.053970,9.296215,22.490843
+-1.188269,0.053652,-0.227241,-206.015198,83.651237,146.687210,99.259590,2.399023,29.088158
+-1.034639,0.226508,-0.055788,-233.298752,120.334480,147.312851,99.259590,2.399023,29.088158
+-1.115818,0.128849,-0.057314,-169.850769,88.167976,149.266037,99.259590,2.399023,29.088158
+-1.051668,0.172491,-0.269295,-83.834343,113.559372,115.115814,92.812210,-6.147497,37.034924
+-0.884243,0.508866,-0.156438,-45.564137,158.238464,114.062927,92.812210,-6.147497,37.034924
+-0.670247,0.668599,0.027528,-91.509750,127.475815,143.818481,90.263252,-12.594872,40.483517
+-0.746605,0.685629,0.066530,-225.226593,58.229317,169.835510,90.263252,-12.594872,40.483517
+-0.505020,0.596637,0.085757,-285.119171,16.922514,153.080841,90.263252,-12.594872,40.483517
+-0.325816,0.659932,0.224982,-156.437881,27.115696,115.390488,96.560684,-23.690355,55.777290
+-0.083193,0.813807,0.526749,113.086334,21.423994,62.746056,96.560684,-23.690355,55.777290
+-0.072390,0.641987,0.430677,109.530930,-11.780145,48.326061,97.010506,-25.339684,55.627354
+-0.146977,0.586383,0.209967,155.827515,-32.029175,-41.352581,97.010506,-25.339684,55.627354
+-0.420911,0.759667,-0.084964,365.092926,-66.026794,-119.617294,97.010506,-25.339684,55.627354
+-0.772240,0.163457,-0.975982,500.000000,-77.196571,-154.103210,91.912582,-22.041027,40.333580
+-0.577410,0.344920,-1.128391,412.991730,43.458359,-168.187500,91.912582,-22.041027,40.333580
+-0.493301,0.275704,-0.756554,359.859009,-14.267404,-133.869446,103.307938,-17.842735,14.544079
+-0.580645,-0.140873,-1.249611,239.982300,-38.605915,-80.599380,103.307938,-17.842735,14.544079
+-0.562883,-0.223212,-1.653981,113.238930,39.201027,-92.150642,103.307938,-17.842735,14.544079
+-0.493362,0.021241,-1.353862,74.480423,165.639206,-84.688866,116.502571,-15.143834,9.446155
+-0.509476,-0.312265,-1.023896,144.871368,219.931641,-62.974945,116.502571,-15.143834,9.446155
+-0.255562,-0.620136,-0.819971,104.876854,219.824829,-79.882202,120.850800,-27.888645,11.395361
+-0.181219,-0.528336,-0.750328,33.478806,239.631332,-101.123077,120.850800,-27.888645,11.395361
+0.271432,-0.477859,-0.584552,123.050629,207.403793,-103.259377,120.850800,-27.888645,11.395361
+0.420789,-0.547075,-0.366405,153.767517,223.441269,-95.889160,121.750435,-38.984127,18.592430
+0.656026,-0.877957,-0.245369,191.152679,141.071808,-101.275673,121.750435,-38.984127,18.592430
+0.555315,-0.714438,-0.387829,138.843964,144.581436,-141.758484,121.900368,-43.932114,26.539196
+0.700400,-0.153874,-0.062136,223.807495,166.982025,-137.668991,121.900368,-43.932114,26.539196
+0.916471,-0.143559,0.110477,333.918274,148.213135,-89.907532,121.900368,-43.932114,26.539196
+0.744835,-0.067934,0.269845,342.616058,175.054169,-70.299385,116.502571,-48.130405,36.135288
+0.747581,0.000244,0.493973,320.581696,169.728683,-46.617023,116.502571,-48.130405,36.135288
+0.597125,0.110355,0.610797,224.311050,177.358322,-23.422956,112.154343,-45.881321,44.082054
+0.403027,0.071718,0.777734,121.982483,180.272842,-30.884731,112.154343,-45.881321,44.082054
+0.155034,0.204230,0.700827,34.531693,175.771362,-56.917019,112.154343,-45.881321,44.082054
+0.119633,0.025819,0.989288,-89.465012,167.806030,-55.345318,109.005623,-40.333580,52.628574
+0.351878,0.005860,1.536973,-109.927673,104.434341,-40.437023,109.005623,-40.333580,52.628574
+-0.046022,0.028504,1.183813,-2.533036,77.394943,-60.640278,110.055191,-35.685471,59.675705
+-0.362255,-0.145634,0.856471,-57.710503,99.169899,-62.852871,110.055191,-35.685471,59.675705
+-0.042299,-0.246467,1.541978,-342.341370,83.223976,-52.171391,110.055191,-35.685471,59.675705
+0.135929,-0.197699,1.644154,-432.874542,-36.118656,-48.585468,115.003181,-29.537973,66.123077
+-0.159368,-0.178777,0.882962,-231.498154,-125.354774,-1.953185,115.003181,-29.537973,66.123077
+-0.341990,-0.709677,0.378124,-208.136230,-104.647972,116.748558,134.045425,-33.586327,60.575336
+-0.056093,-1.304971,0.450087,-208.594009,-71.855827,66.225166,134.045425,-33.586327,60.575336
+-0.113163,-1.142674,0.297067,-347.788940,-76.204720,-0.061037,134.045425,-33.586327,60.575336
+0.054018,-0.900479,0.365673,-401.150543,-97.689751,-19.043550,143.791458,-33.736263,45.431503
+-0.266488,-0.927885,-0.007263,-401.577820,-157.490768,-8.728293,143.791458,-33.736263,45.431503
+-0.391552,-0.760826,-0.481094,-442.686249,-187.429428,-37.888729,138.543594,-33.286449,20.391699
+-0.288400,-0.405469,-0.618854,-440.809357,-144.062622,-73.641167,138.543594,-33.286449,20.391699
+-0.378613,0.296030,-0.936369,-352.259888,-85.192421,-74.159981,138.543594,-33.286449,20.391699
+-0.236885,0.263009,-1.091220,-254.814301,-37.003693,-31.937620,112.604156,-27.738707,10.945544
+-0.325999,0.459670,-0.798181,-316.354858,40.849026,-28.336435,112.604156,-27.738707,10.945544
+-0.746422,0.973785,-0.559404,-357.158112,68.025757,-11.429182,98.210014,-24.739927,21.141394
+-0.344981,0.782006,-0.148259,-389.614563,54.765465,13.534959,98.210014,-24.739927,21.141394
+-0.512589,0.700278,0.045534,-432.767731,38.514359,8.941923,98.210014,-24.739927,21.141394
+-0.411267,0.910306,0.085574,-326.837982,54.094059,-12.085330,92.962151,-26.689135,47.680588
+-0.328929,0.949126,0.181951,-141.514328,87.420273,7.538072,92.962151,-26.689135,47.680588
+-0.209479,0.766259,0.364025,-96.957306,78.325752,21.454512,100.009285,-23.540417,60.425400
+-0.241523,0.583209,0.394055,-50.553909,56.566059,-42.191841,100.009285,-23.540417,60.425400
+-0.410474,0.569231,0.259957,12.527848,71.184425,-112.430191,100.009285,-23.540417,60.425400
+-0.723228,0.593097,0.181097,142.582474,86.230049,-162.144836,102.708183,-18.442492,63.124302
+-1.139073,0.494339,-0.006775,163.243500,82.842491,-236.259048,102.708183,-18.442492,63.124302
+-1.377606,0.610492,-0.090396,156.178467,86.993011,-274.117249,102.708183,-18.442492,63.124302
+-1.126377,0.946867,-0.084414,214.529861,81.820122,-222.052673,102.708183,-18.442492,63.124302
+-0.879604,0.913480,-0.075869,306.283752,71.947388,-101.275673,102.708183,-18.442492,63.124302
+-1.020234,0.474624,-0.210456,415.295868,61.204872,-13.290811,98.959709,5.997558,42.582664
+-1.152867,0.011414,-0.473464,438.276306,82.384720,-7.980590,98.959709,5.997558,42.582664
+-1.024567,-0.302927,-0.462050,409.802551,115.085297,-47.395245,106.306717,4.947986,23.240538
+-0.855373,-0.017212,-0.475661,323.572510,136.173584,-105.319374,106.306717,4.947986,23.240538
+-0.733177,0.090701,-0.346507,271.126434,99.536118,-16.800440,106.306717,4.947986,23.240538
+-0.829249,-0.273019,-0.439772,259.804077,106.021301,42.161320,124.749214,-2.099145,15.293774
+-0.946806,-0.585345,-0.464614,233.741257,157.628098,8.484145,124.749214,-2.099145,15.293774
+-0.977386,-0.309336,0.050050,227.225555,201.055939,-21.088289,137.194138,-9.446155,16.643225
+-0.620991,-0.635945,-0.011292,316.354858,155.110321,-18.265327,137.194138,-9.446155,16.643225
+-0.440016,-1.012055,0.061281,292.321533,197.698898,-91.738640,137.194138,-9.446155,16.643225
+-0.624409,-0.972259,0.486282,284.493530,173.238312,-173.375656,149.189255,-17.242979,31.037363
+-0.381237,-0.825770,0.684652,351.680054,91.402939,-205.648972,149.189255,-17.242979,31.037363
+-0.346812,-0.793481,1.019562,389.187286,38.468582,-191.518906,145.890610,-27.438829,49.929672
+-0.147893,-0.894925,0.864467,361.110260,-60.441910,-166.753128,145.890610,-27.438829,49.929672
+0.172552,-0.653829,1.068575,184.759064,-73.366493,-131.046478,128.797562,-34.336021,60.725277
+0.112796,-0.543046,0.898404,139.851074,-106.341743,-90.563675,128.797562,-34.336021,60.725277
+0.214423,-0.328684,1.069918,198.690750,-73.198647,-74.129463,128.797562,-34.336021,60.725277
+0.285836,-0.234993,0.826380,228.476822,-74.556717,-51.210060,118.901588,-40.633457,57.126743
+0.317209,-0.067934,1.023530,105.166786,-84.383682,-29.068880,118.901588,-40.633457,57.126743
+0.345958,0.056093,0.804224,-4.974517,-131.260117,-8.514664,114.103546,-42.732601,51.429062
+0.378491,-0.016358,0.776208,-112.949005,-114.291817,5.172887,114.103546,-42.732601,51.429062
+0.512101,-0.359813,0.754967,-247.276230,-104.693748,-10.330516,114.103546,-42.732601,51.429062
+0.626606,-0.130070,0.964507,-356.822418,-124.515518,-37.217323,115.902809,-46.631016,46.331139
+0.620380,-0.241157,0.692465,-364.528351,-147.724838,-16.113773,115.902809,-46.631016,46.331139
+0.689535,-0.029969,0.723777,-312.005981,-175.099945,3.555406,117.252266,-48.580223,38.684250
+0.348399,-0.207343,0.190191,-158.970917,-245.506149,55.009613,117.252266,-48.580223,38.684250
+0.627949,-0.307505,0.153386,-40.284431,-208.426163,96.819969,117.252266,-48.580223,38.684250
+0.434706,-0.808618,-0.085635,-19.180883,-223.319199,127.597885,120.550919,-44.082054,25.489622
+0.044069,-0.607074,-0.264901,-135.929443,-273.659485,118.106628,120.550919,-44.082054,25.489622
+-0.159673,-0.400037,-0.760155,-195.272690,-302.423157,131.672104,121.300613,-31.037363,13.344567
+-0.449416,-0.085635,-1.105808,-196.844391,-310.007019,134.861298,121.300613,-31.037363,13.344567
+-0.874111,-0.004456,-0.975860,-173.039948,-314.081238,147.709579,121.300613,-31.037363,13.344567
+-1.159642,-0.030274,-0.964385,-156.407364,-304.773102,123.264259,112.154343,-14.394140,12.594872
+-1.314860,0.029054,-0.949065,-153.660690,-268.288208,78.249458,112.154343,-14.394140,12.594872
+-1.166051,0.262032,-0.819483,-146.290482,-195.928833,39.185764,101.658615,-3.298657,20.091820
+-1.004486,0.524430,-0.375866,-149.800110,-166.066467,68.224129,101.658615,-3.298657,20.091820
+-1.057283,0.533769,-0.472915,-176.808990,-142.246765,97.079376,101.658615,-3.298657,20.091820
+-0.773095,0.559771,-0.342723,-138.019958,-44.404430,93.752861,95.061295,-0.899634,34.186081
+-0.807398,0.606586,-0.237068,-165.074615,54.826504,113.467819,95.061295,-0.899634,34.186081
+-0.810633,0.478591,-0.021851,-191.290024,86.947235,152.134766,91.012947,-7.047131,43.932114
+-0.801660,0.712729,0.096683,-254.081848,64.271980,195.135345,91.012947,-7.047131,43.932114
+-0.805628,0.585406,0.148869,-314.859467,4.669332,228.125854,91.012947,-7.047131,43.932114
+-0.839015,0.638508,0.016114,-310.541107,-19.455549,224.982452,98.060074,-16.793163,60.575336
+-0.728355,0.622395,0.135197,-74.617752,68.071533,203.665268,98.060074,-16.793163,60.575336
+0.052248,0.384106,0.648579,8.896146,91.692863,180.684830,105.856903,-21.591209,65.673264
+0.031068,0.630207,0.907926,-1.510666,24.338511,127.903076,105.856903,-21.591209,65.673264
+-0.189825,0.753319,0.768212,87.862785,-21.317179,96.957306,105.856903,-21.591209,65.673264
+-0.060915,0.937712,0.738487,35.920284,-35.660877,102.710045,107.356293,-24.589989,65.373383
+-0.001160,1.026093,0.584368,80.385757,-44.846951,95.263527,107.356293,-24.589989,65.373383
+0.210944,1.078829,0.533952,213.309128,-34.302803,108.783226,106.756538,-30.287668,62.974361
+0.353160,0.832240,0.443922,367.961670,-37.400433,118.152412,106.756538,-30.287668,62.974361
+0.179327,0.747032,-0.103458,250.877411,-38.621174,103.518784,106.756538,-30.287668,62.974361
+0.311472,0.628803,-0.334300,285.866882,-42.817471,54.322948,101.208794,-36.735043,48.430283
+0.317331,0.563189,-0.595538,342.677094,-17.014069,45.396282,101.208794,-36.735043,48.430283
+0.402295,0.402661,-0.732322,395.809814,17.593920,37.232582,101.958488,-38.084496,32.386814
+0.412976,0.122868,-0.882046,414.212463,53.788872,47.730949,101.958488,-38.084496,32.386814
+0.625507,-0.202643,-0.860683,414.990692,74.144722,35.538803,101.958488,-38.084496,32.386814
+0.660482,-0.464919,-0.570696,373.775452,105.197304,55.787834,114.403427,-42.132847,24.140171
+0.529374,-0.830103,-0.580584,376.171143,124.256111,30.137028,114.403427,-42.132847,24.140171
+0.715598,-0.903897,-0.062563,377.315582,152.439957,-8.621479,126.098663,-45.431503,30.137730
+0.451857,-0.492935,0.112125,360.515167,161.168243,-27.298807,126.098663,-45.431503,30.137730
+0.714255,-0.320017,0.563433,308.862579,132.389297,50.035095,126.098663,-45.431503,30.137730
+0.343455,-0.723838,0.667135,130.863373,15.076143,154.774628,128.497681,-45.431503,43.932114
+0.232185,-0.867031,0.527421,-106.494339,-18.372143,173.131500,128.497681,-45.431503,43.932114
+0.244697,-0.994659,0.544450,-274.239319,-29.496140,172.338028,137.044205,-41.982906,44.381931
+0.109500,-1.077487,0.348888,-313.791321,-110.858482,181.676682,137.044205,-41.982906,44.381931
+-0.019105,-1.089938,-0.057558,-275.200653,-145.420700,146.855072,137.044205,-41.982906,44.381931
+-0.133854,-0.962615,-0.364879,-186.788544,-163.319809,107.806633,140.492798,-34.036144,25.939440
+-0.221259,-0.716453,-0.575518,-106.784264,-121.524704,104.083374,140.492798,-34.036144,25.939440
+-0.281686,-0.742698,-0.575152,-60.228279,-85.238197,136.432999,138.543594,-23.690355,17.692797
+-0.525163,-0.902127,-0.504288,-136.661880,-71.916870,145.084991,138.543594,-23.690355,17.692797
+-0.632832,-0.628681,-0.562273,-217.688522,-90.929901,119.403671,138.543594,-23.690355,17.692797
+-0.699423,-0.475784,-0.722983,-217.261276,-91.219826,115.726189,131.496460,-16.943102,12.444933
+-0.777062,-0.327342,-0.804407,-213.995789,-65.614792,111.255226,131.496460,-16.943102,12.444933
+-0.802149,-0.204596,-0.780847,-236.182739,-36.622211,87.679680,115.902809,-9.446155,10.345788
+-0.747642,0.062136,-0.885464,-260.292358,12.115849,60.975983,115.902809,-9.446155,10.345788
+-0.718406,0.279061,-0.799524,-267.464203,68.971832,60.640278,115.902809,-9.446155,10.345788
+-0.712302,0.425733,-0.613727,-241.859192,117.526779,84.383682,100.908920,-11.245421,18.742369
+-0.739464,0.521928,-0.575091,-117.343666,142.445145,99.414047,100.908920,-11.245421,18.742369
+-0.640461,0.585345,-0.582354,18.738365,176.900543,104.281746,100.908920,-11.245421,18.742369
+-0.436415,0.500076,-0.496170,47.837765,169.713425,113.559372,100.908920,-11.245421,18.742369
+-0.120609,0.372021,-0.426221,31.418806,128.040405,115.955078,100.908920,-11.245421,18.742369
+-0.118473,0.393139,-0.564470,36.912136,100.299080,96.926788,96.410751,-30.137730,28.938219

ugv2_rest.csv

@@ -0,0 +1,731 @@
+AX,AY,AZ,GX,GY,GZ,MX,MY,MZ
+0.183355,-0.004883,-0.973540,4.318369,2.822962,0.885037,94.911362,-16.943102,14.843957
+0.181524,-0.001343,-0.969329,4.379406,2.700888,0.930815,94.911362,-16.943102,14.843957
+0.184393,0.000000,-0.967925,4.348887,2.777184,0.915555,94.911362,-16.943102,14.843957
+0.184881,-0.003967,-0.967376,4.333628,2.746666,0.854518,96.710625,-16.043468,14.843957
+0.182501,-0.003784,-0.972747,4.348887,2.700888,0.930815,96.710625,-16.043468,14.843957
+0.185430,-0.001770,-0.966155,4.348887,2.777184,0.869778,95.211235,-16.043468,15.143834
+0.183050,-0.002197,-0.967864,4.257332,2.883999,0.915555,95.211235,-16.043468,15.143834
+0.183660,-0.001587,-0.970550,4.303110,2.731407,0.930815,95.211235,-16.043468,15.143834
+0.183660,-0.004212,-0.970794,4.379406,2.731407,0.946074,94.761421,-16.793163,13.794384
+0.185492,-0.004395,-0.969207,4.287850,2.807703,0.885037,94.761421,-16.793163,13.794384
+0.180548,-0.000183,-0.967559,4.318369,2.761925,0.885037,95.810989,-16.043468,15.443712
+0.180486,0.000000,-0.970428,4.379406,2.853481,0.885037,95.810989,-16.043468,15.443712
+0.183721,-0.003601,-0.968596,4.333628,2.700888,0.930815,95.810989,-16.043468,15.443712
+0.187201,-0.007935,-0.968169,4.348887,2.792444,0.930815,95.361176,-15.893529,14.094262
+0.182073,-0.000610,-0.967681,4.333628,2.822962,0.930815,95.361176,-15.893529,14.094262
+0.182379,0.000000,-0.964568,4.379406,2.792444,0.885037,95.061295,-16.493284,15.893529
+0.182379,-0.004456,-0.970489,4.394665,2.853481,0.976592,95.061295,-16.493284,15.893529
+0.184393,-0.001465,-0.968535,4.364147,2.792444,0.930815,95.061295,-16.493284,15.893529
+0.182684,-0.002625,-0.969695,4.287850,2.731407,0.915555,94.911362,-16.643225,13.944323
+0.183538,-0.000549,-0.967315,4.318369,2.883999,0.854518,94.911362,-16.643225,13.944323
+0.183721,-0.002319,-0.968047,4.348887,2.853481,0.946074,94.611481,-16.643225,15.743590
+0.186529,-0.002625,-0.967132,4.379406,2.792444,0.930815,94.611481,-16.643225,15.743590
+0.180059,-0.003784,-0.970183,4.364147,2.807703,0.946074,94.611481,-16.643225,15.743590
+0.183966,-0.002503,-0.967498,4.379406,2.761925,0.961333,95.511116,-15.743590,15.143834
+0.182989,-0.002808,-0.966582,4.287850,2.685629,0.915555,95.511116,-15.743590,15.143834
+0.185064,-0.005554,-0.966521,4.333628,2.731407,0.991852,93.411972,-16.343346,14.843957
+0.181829,-0.001160,-0.970611,4.364147,2.777184,0.991852,93.411972,-16.343346,14.843957
+0.183050,0.001770,-0.967498,4.348887,2.746666,0.869778,93.411972,-16.343346,14.843957
+0.184515,-0.002380,-0.965178,4.333628,2.731407,0.885037,95.211235,-16.643225,15.143834
+0.184576,-0.002869,-0.969146,4.318369,2.792444,0.930815,95.211235,-16.643225,15.143834
+0.182928,-0.001953,-0.966094,4.394665,2.792444,0.854518,94.761421,-16.793163,15.293774
+0.181768,-0.000488,-0.968596,4.287850,2.838221,0.869778,94.761421,-16.793163,15.293774
+0.184637,-0.000610,-0.972076,4.333628,2.822962,0.915555,94.761421,-16.793163,15.293774
+0.181951,-0.003052,-0.970672,4.257332,2.731407,0.869778,95.511116,-16.043468,14.544079
+0.186529,-0.003845,-0.963652,4.440443,2.700888,0.869778,95.511116,-16.043468,14.544079
+0.180609,-0.001648,-0.967071,4.318369,2.792444,0.915555,95.211235,-18.142614,14.843957
+0.185064,0.000916,-0.966521,4.333628,2.777184,0.885037,95.211235,-18.142614,14.843957
+0.180853,-0.004517,-0.968169,4.379406,2.761925,0.991852,95.211235,-18.142614,14.843957
+0.187323,-0.004700,-0.962310,4.348887,2.716147,0.946074,94.911362,-16.643225,14.544079
+0.180731,-0.000305,-0.968291,4.318369,2.685629,0.808740,94.911362,-16.643225,14.544079
+0.182318,0.000793,-0.967803,4.287850,2.868740,0.885037,94.911362,-16.643225,15.443712
+0.181463,-0.003723,-0.971587,4.364147,2.746666,0.915555,94.911362,-16.643225,15.443712
+0.188788,-0.002136,-0.969817,4.379406,2.838221,0.915555,94.461540,-15.893529,14.094262
+0.180486,-0.000671,-0.971038,4.379406,2.792444,0.869778,94.461540,-15.893529,14.094262
+0.184881,-0.000427,-0.967009,4.364147,2.792444,0.900296,94.461540,-15.893529,14.094262
+0.183111,0.000916,-0.967498,4.394665,2.807703,0.961333,94.761421,-16.193407,14.394140
+0.183721,-0.005249,-0.968169,4.440443,2.838221,0.885037,94.761421,-16.193407,14.394140
+0.182073,-0.003296,-0.967193,4.333628,2.731407,0.900296,95.960930,-16.193407,15.293774
+0.183111,0.001648,-0.967742,4.364147,2.868740,0.961333,95.960930,-16.193407,15.293774
+0.181280,-0.002136,-0.969329,4.272591,2.777184,0.961333,95.960930,-16.193407,15.293774
+0.183905,-0.004883,-0.963164,4.379406,2.746666,0.839259,93.711845,-16.343346,15.743590
+0.184210,-0.003479,-0.965850,4.196295,2.746666,0.915555,93.711845,-16.343346,15.743590
+0.182073,-0.000183,-0.970672,4.272591,2.822962,0.869778,94.761421,-16.793163,15.593652
+0.182867,-0.003418,-0.967437,4.364147,2.700888,0.961333,94.761421,-16.793163,15.593652
+0.183782,-0.001526,-0.971038,4.394665,2.761925,0.915555,94.761421,-16.793163,15.593652
+0.182012,-0.001282,-0.972503,4.425184,2.761925,0.915555,93.861786,-16.193407,16.193407
+0.184698,-0.004639,-0.971160,4.272591,2.761925,0.824000,93.861786,-16.193407,16.193407
+0.181585,-0.002564,-0.963836,4.303110,2.731407,0.961333,94.161667,-15.893529,15.293774
+0.184515,0.000427,-0.965972,4.303110,2.761925,0.915555,94.161667,-15.893529,15.293774
+0.182134,-0.002869,-0.967742,4.287850,2.746666,0.946074,94.161667,-15.893529,15.293774
+0.185858,-0.004883,-0.965178,4.333628,2.822962,0.946074,94.311600,-18.142614,13.944323
+0.179998,-0.001099,-0.968657,4.348887,2.716147,0.824000,94.311600,-18.142614,13.944323
+0.185003,-0.002075,-0.965789,4.364147,2.731407,0.976592,95.211235,-16.043468,13.944323
+0.179632,-0.002808,-0.965972,4.287850,2.761925,0.885037,95.211235,-16.043468,13.944323
+0.185797,-0.000977,-0.967986,4.379406,2.777184,0.869778,95.211235,-16.043468,13.944323
+0.181646,-0.001587,-0.968719,4.394665,2.822962,0.885037,95.810989,-16.343346,14.544079
+0.184942,-0.001526,-0.969085,4.318369,2.777184,0.976592,95.810989,-16.343346,14.544079
+0.182867,-0.002014,-0.965361,4.318369,2.761925,0.915555,94.761421,-16.793163,14.993896
+0.183966,-0.000916,-0.967803,4.287850,2.853481,0.900296,94.761421,-16.793163,14.993896
+0.181829,-0.002014,-0.967864,4.333628,2.853481,0.930815,94.761421,-16.793163,14.993896
+0.186346,0.000366,-0.968291,4.348887,2.700888,0.900296,95.511116,-16.643225,15.443712
+0.181646,-0.000793,-0.967498,4.379406,2.822962,0.854518,95.511116,-16.643225,15.443712
+0.182318,-0.003479,-0.966460,4.303110,2.807703,0.991852,95.511116,-16.643225,15.443712
+0.184454,-0.002930,-0.969634,4.303110,2.761925,0.961333,95.511116,-16.643225,15.443712
+0.183782,-0.002380,-0.969634,4.257332,2.731407,0.900296,95.511116,-16.643225,15.443712
+0.181585,-0.001160,-0.972442,4.348887,2.868740,0.900296,94.761421,-16.793163,14.993896
+0.186224,-0.002869,-0.966155,4.364147,2.883999,0.946074,94.761421,-16.793163,14.993896
+0.183233,-0.002625,-0.966521,4.242073,2.777184,0.869778,96.260811,-16.193407,14.993896
+0.180731,-0.002503,-0.968719,4.303110,2.716147,0.839259,96.260811,-16.193407,14.993896
+0.184820,-0.001770,-0.964934,4.333628,2.639851,0.930815,96.260811,-16.193407,14.993896
+0.183233,-0.000610,-0.969268,4.348887,2.822962,0.854518,95.061295,-16.493284,15.293774
+0.183172,-0.002503,-0.965850,4.333628,2.761925,0.915555,95.061295,-16.493284,15.293774
+0.183721,-0.002441,-0.965423,4.318369,2.792444,0.946074,95.061295,-14.993896,15.293774
+0.182806,-0.001892,-0.965117,4.348887,2.731407,0.915555,95.061295,-14.993896,15.293774
+0.183477,-0.004639,-0.969817,4.394665,2.807703,0.930815,95.061295,-14.993896,15.293774
+0.183721,-0.001709,-0.965361,4.287850,2.731407,0.930815,95.061295,-16.493284,15.293774
+0.184088,-0.002380,-0.968596,4.272591,2.716147,0.885037,95.061295,-16.493284,15.293774
+0.181097,-0.002503,-0.966399,4.333628,2.746666,0.946074,92.512337,-16.943102,16.643225
+0.182684,-0.002869,-0.969268,4.272591,2.731407,0.869778,92.512337,-16.943102,16.643225
+0.185553,-0.000977,-0.969878,4.287850,2.822962,0.869778,92.512337,-16.943102,16.643225
+0.184515,-0.005982,-0.970122,4.364147,2.822962,0.900296,93.411972,-16.943102,15.143834
+0.183111,-0.000855,-0.968291,4.333628,2.822962,0.915555,93.411972,-16.943102,15.143834
+0.184454,-0.001282,-0.966399,4.379406,2.777184,0.946074,95.061295,-16.493284,15.293774
+0.184393,-0.003662,-0.967376,4.318369,2.761925,0.885037,95.061295,-16.493284,15.293774
+0.184515,-0.002075,-0.968719,4.242073,2.700888,0.885037,95.061295,-16.493284,15.293774
+0.183172,-0.001038,-0.969451,4.257332,2.716147,0.930815,95.960930,-16.493284,14.394140
+0.183905,-0.001221,-0.965300,4.318369,2.899258,0.900296,95.960930,-16.493284,14.394140
+0.183294,-0.003479,-0.965178,4.287850,2.807703,0.976592,94.311600,-16.343346,14.843957
+0.184149,-0.001465,-0.967925,4.394665,2.792444,0.915555,94.311600,-16.343346,14.843957
+0.180120,-0.001282,-0.968657,4.318369,2.746666,0.930815,94.311600,-16.343346,14.843957
+0.184698,-0.000549,-0.971038,4.409925,2.822962,0.869778,95.960930,-16.793163,13.794384
+0.181463,-0.003357,-0.970916,4.440443,2.761925,0.885037,95.960930,-16.793163,13.794384
+0.186041,-0.000366,-0.963713,4.333628,2.792444,0.976592,95.511116,-16.643225,15.443712
+0.180975,-0.000793,-0.969207,4.318369,2.792444,0.900296,95.511116,-16.643225,15.443712
+0.185125,-0.001953,-0.968169,4.364147,2.746666,0.991852,95.511116,-16.643225,15.443712
+0.180853,-0.001953,-0.969451,4.318369,2.761925,0.869778,94.911362,-16.643225,15.443712
+0.186224,-0.001953,-0.969634,4.348887,2.761925,0.976592,94.911362,-16.643225,15.443712
+0.182257,-0.002808,-0.964873,4.364147,2.746666,0.869778,94.161667,-15.593652,15.293774
+0.185980,-0.001282,-0.963103,4.333628,2.716147,0.869778,94.161667,-15.593652,15.293774
+0.183172,-0.002991,-0.971404,4.455702,2.853481,0.915555,94.161667,-15.593652,15.293774
+0.184149,-0.003357,-0.963530,4.379406,2.746666,0.930815,94.761421,-16.493284,14.694017
+0.180792,-0.003540,-0.971282,4.333628,2.853481,0.915555,94.761421,-16.493284,14.694017
+0.183966,-0.003174,-0.969390,4.379406,2.685629,1.007111,95.361176,-16.493284,14.993896
+0.184088,-0.003784,-0.968535,4.409925,2.838221,0.839259,95.361176,-16.493284,14.993896
+0.182501,-0.002991,-0.967742,4.364147,2.777184,0.885037,95.361176,-16.493284,14.993896
+0.180792,-0.002319,-0.966643,4.303110,2.746666,0.885037,93.861786,-17.392920,14.694017
+0.180853,-0.001465,-0.967071,4.348887,2.838221,0.900296,93.861786,-17.392920,14.694017
+0.185003,-0.001770,-0.964141,4.333628,2.716147,0.915555,94.761421,-17.392920,14.993896
+0.183111,-0.004334,-0.971221,4.333628,2.838221,0.854518,94.761421,-17.392920,14.993896
+0.182440,0.000366,-0.969573,4.379406,2.822962,0.915555,94.761421,-17.392920,14.993896
+0.183416,-0.001648,-0.971038,4.333628,2.700888,0.885037,95.211235,-16.643225,15.443712
+0.182989,-0.003540,-0.968474,4.303110,2.685629,0.946074,95.211235,-16.643225,15.443712
+0.184515,-0.004639,-0.966338,4.272591,2.807703,0.885037,94.461540,-16.193407,14.394140
+0.183111,0.000183,-0.967559,4.364147,2.716147,0.900296,94.461540,-16.193407,14.394140
+0.180792,-0.002075,-0.970061,4.303110,2.792444,0.915555,95.061295,-15.293774,15.293774
+0.183905,-0.003052,-0.968535,4.318369,2.777184,0.885037,95.061295,-15.293774,15.293774
+0.183538,-0.002380,-0.967681,4.303110,2.792444,0.869778,95.061295,-15.293774,15.293774
+0.183172,-0.002380,-0.965911,4.272591,2.792444,0.839259,95.361176,-17.093040,15.593652
+0.182562,-0.002380,-0.968474,4.364147,2.853481,0.915555,95.361176,-17.093040,15.593652
+0.182684,-0.000549,-0.966643,4.364147,2.761925,0.961333,95.960930,-15.593652,14.694017
+0.184576,-0.004395,-0.964568,4.333628,2.761925,0.900296,95.960930,-15.593652,14.694017
+0.183843,-0.003296,-0.969085,4.333628,2.731407,0.900296,95.960930,-15.593652,14.694017
+0.183905,-0.001160,-0.965728,4.242073,2.731407,0.930815,95.211235,-17.542858,15.443712
+0.181646,0.000305,-0.970489,4.348887,2.868740,0.930815,95.211235,-17.542858,15.443712
+0.185980,-0.002503,-0.970733,4.303110,2.777184,0.976592,95.211235,-16.643225,14.843957
+0.181646,-0.001770,-0.964690,4.333628,2.792444,0.900296,95.211235,-16.643225,14.843957
+0.184881,-0.001343,-0.971099,4.333628,2.746666,0.976592,95.211235,-16.643225,14.843957
+0.182745,-0.001404,-0.966094,4.364147,2.761925,0.930815,95.810989,-16.043468,16.343346
+0.183905,-0.002136,-0.968413,4.303110,2.853481,0.900296,95.810989,-16.043468,16.343346
+0.184759,-0.002441,-0.969573,4.348887,2.746666,0.854518,95.211235,-17.842735,14.244201
+0.181707,-0.002441,-0.969268,4.303110,2.700888,0.930815,95.211235,-17.842735,14.244201
+0.182501,-0.000122,-0.969817,4.257332,2.761925,0.930815,95.211235,-17.842735,14.244201
+0.184454,-0.001526,-0.967254,4.303110,2.700888,0.946074,95.211235,-16.643225,16.343346
+0.182623,-0.000549,-0.970733,4.379406,2.731407,0.915555,95.211235,-16.643225,16.343346
+0.182379,-0.002869,-0.969512,4.303110,2.716147,0.946074,94.611481,-16.943102,16.043468
+0.183966,0.001221,-0.966399,4.364147,2.807703,0.946074,94.611481,-16.943102,16.043468
+0.181524,-0.001892,-0.968474,4.318369,2.838221,0.946074,94.611481,-16.943102,16.043468
+0.187262,-0.002869,-0.962127,4.211555,2.716147,0.885037,95.960930,-17.093040,15.593652
+0.182562,-0.002930,-0.967681,4.272591,2.792444,0.869778,95.960930,-17.093040,15.593652
+0.184454,-0.001404,-0.969817,4.379406,2.731407,0.930815,95.361176,-16.193407,15.293774
+0.179998,-0.001526,-0.967009,4.257332,2.761925,0.915555,95.361176,-16.193407,15.293774
+0.185858,-0.001953,-0.970061,4.303110,2.731407,0.915555,95.361176,-16.193407,15.293774
+0.183355,-0.001648,-0.972076,4.303110,2.655110,0.869778,95.511116,-17.242979,15.743590
+0.183721,-0.002258,-0.969329,4.211555,2.761925,0.824000,95.511116,-17.242979,15.743590
+0.182012,-0.000427,-0.967071,4.348887,2.670370,0.930815,94.761421,-16.193407,14.694017
+0.183172,-0.000977,-0.964751,4.287850,2.731407,0.930815,94.761421,-16.193407,14.694017
+0.182928,-0.001343,-0.970611,4.303110,2.761925,0.869778,94.761421,-16.193407,14.694017
+0.182928,-0.003662,-0.966521,4.318369,2.746666,0.900296,94.311600,-16.343346,16.043468
+0.181707,-0.001038,-0.964751,4.364147,2.777184,0.946074,94.311600,-16.343346,16.043468
+0.185430,-0.001953,-0.966887,4.348887,2.761925,0.915555,94.461540,-16.193407,14.993896
+0.181524,-0.000427,-0.966033,4.348887,2.822962,1.022370,94.461540,-16.193407,14.993896
+0.185186,0.000305,-0.970000,4.379406,2.822962,0.839259,94.461540,-16.193407,14.993896
+0.181768,-0.001892,-0.969329,4.272591,2.777184,0.915555,95.511116,-16.643225,15.143834
+0.184271,-0.001953,-0.967071,4.364147,2.868740,0.900296,95.511116,-16.643225,15.143834
+0.183843,-0.000427,-0.968169,4.318369,2.761925,0.930815,95.061295,-16.193407,15.593652
+0.183233,-0.001526,-0.968108,4.303110,2.807703,0.854518,95.061295,-16.193407,15.593652
+0.184332,-0.001160,-0.969390,4.318369,2.792444,0.915555,95.061295,-16.193407,15.593652
+0.182684,-0.002014,-0.970672,4.364147,2.777184,0.900296,95.960930,-17.692797,14.993896
+0.185614,-0.001953,-0.967925,4.287850,2.746666,0.869778,95.960930,-17.692797,14.993896
+0.181829,-0.003296,-0.967009,4.318369,2.792444,0.946074,94.611481,-17.242979,15.143834
+0.184027,-0.000977,-0.966521,4.272591,2.670370,0.946074,94.611481,-17.242979,15.143834
+0.181402,-0.000732,-0.968413,4.348887,2.777184,0.854518,94.611481,-17.242979,15.143834
+0.185797,-0.002380,-0.968047,4.425184,2.807703,0.946074,94.611481,-17.242979,15.143834
+0.181585,-0.003357,-0.970061,4.272591,2.777184,0.915555,94.611481,-17.242979,15.143834
+0.180975,-0.001892,-0.971770,4.348887,2.838221,0.961333,95.361176,-16.493284,15.293774
+0.181585,0.004089,-0.960051,4.379406,2.761925,0.961333,95.361176,-16.493284,15.293774
+0.181402,-0.011475,-0.970550,4.242073,2.853481,1.068148,95.211235,-16.943102,14.544079
+0.188910,-0.001465,-0.965606,4.242073,2.822962,0.869778,95.211235,-16.943102,14.544079
+0.174200,0.002197,-0.972442,4.470962,2.807703,0.732444,95.211235,-16.943102,14.544079
+0.187323,0.000732,-0.964873,4.242073,2.761925,0.976592,94.461540,-16.493284,14.694017
+0.176885,-0.007141,-0.968902,4.318369,2.716147,0.839259,94.461540,-16.493284,14.694017
+0.191778,-0.001404,-0.977203,4.470962,2.777184,0.900296,94.461540,-15.893529,16.193407
+0.174444,-0.002136,-0.964324,4.425184,2.777184,0.808740,94.461540,-15.893529,16.193407
+0.192816,-0.000488,-0.970611,4.272591,2.807703,0.946074,94.461540,-15.893529,16.193407
+0.175298,-0.002747,-0.969207,4.348887,2.700888,0.793481,95.960930,-17.392920,14.993896
+0.187750,-0.002014,-0.970244,4.333628,2.868740,1.007111,95.960930,-17.392920,14.993896
+0.180181,-0.004212,-0.968413,4.440443,2.777184,0.930815,94.311600,-16.343346,15.743590
+0.190924,-0.002869,-0.967864,4.272591,2.822962,0.991852,94.311600,-16.343346,15.743590
+0.179266,-0.002014,-0.968291,4.409925,2.685629,0.824000,94.311600,-16.343346,15.743590
+0.187078,-0.001770,-0.966460,4.348887,2.777184,0.915555,95.211235,-16.343346,15.443712
+0.179022,-0.003357,-0.972381,4.348887,2.807703,0.762963,95.211235,-16.343346,15.443712
+0.185797,-0.001648,-0.962615,4.333628,2.868740,0.946074,96.260811,-16.193407,16.193407
+0.181768,-0.002625,-0.971587,4.348887,2.777184,0.839259,96.260811,-16.193407,16.193407
+0.184454,-0.001038,-0.968230,4.394665,2.761925,0.900296,96.260811,-16.193407,16.193407
+0.181829,-0.000916,-0.970367,4.272591,2.700888,0.930815,96.110870,-16.643225,15.143834
+0.186407,-0.003479,-0.964080,4.303110,2.746666,0.915555,96.110870,-16.643225,15.143834
+0.182501,-0.004944,-0.968841,4.379406,2.838221,0.854518,94.911362,-16.343346,15.743590
+0.184637,-0.002686,-0.965300,4.196295,2.761925,0.930815,94.911362,-16.343346,15.743590
+0.184149,0.000610,-0.966765,4.226814,2.777184,0.915555,94.911362,-16.343346,15.743590
+0.181219,-0.002380,-0.967437,4.303110,2.761925,0.900296,94.611481,-15.743590,15.143834
+0.188604,-0.003845,-0.964141,4.303110,2.792444,0.930815,94.611481,-15.743590,15.143834
+0.180914,-0.003296,-0.969573,4.287850,2.777184,0.885037,94.611481,-16.943102,15.443712
+0.182257,0.000061,-0.971038,4.303110,2.807703,0.885037,94.611481,-16.943102,15.443712
+0.181402,-0.003784,-0.971526,4.364147,2.731407,0.839259,94.611481,-16.943102,15.443712
+0.184149,-0.002503,-0.966582,4.394665,2.746666,0.869778,95.211235,-15.743590,15.743590
+0.182501,-0.003418,-0.966765,4.379406,2.746666,0.854518,95.211235,-15.743590,15.743590
+0.184698,0.000122,-0.966887,4.364147,2.838221,0.900296,94.911362,-15.143834,14.544079
+0.181524,-0.000793,-0.970122,4.364147,2.777184,0.900296,94.911362,-15.143834,14.544079
+0.186163,-0.002564,-0.966338,4.303110,2.807703,0.854518,94.911362,-15.143834,14.544079
+0.180486,-0.002930,-0.969512,4.333628,2.838221,0.854518,95.211235,-16.943102,14.544079
+0.185919,-0.001831,-0.967925,4.425184,2.700888,0.976592,95.211235,-16.943102,14.544079
+0.178899,-0.000610,-0.968657,4.425184,2.685629,0.885037,94.311600,-16.043468,14.843957
+0.185553,-0.002075,-0.967803,4.287850,2.792444,0.885037,94.311600,-16.043468,14.843957
+0.180914,-0.003845,-0.967925,4.287850,2.700888,0.946074,94.311600,-16.043468,14.843957
+0.184454,-0.000549,-0.967986,4.318369,2.777184,0.885037,95.211235,-16.043468,15.143834
+0.180425,-0.000183,-0.972503,4.272591,2.838221,0.824000,95.211235,-16.043468,15.143834
+0.184515,-0.004822,-0.978118,4.211555,2.685629,1.068148,94.311600,-16.643225,14.843957
+0.182806,0.003906,-0.962127,4.379406,2.777184,0.885037,94.311600,-16.643225,14.843957
+0.189459,-0.001953,-0.972808,4.272591,2.746666,0.732444,94.311600,-16.643225,14.843957
+0.177557,-0.007874,-0.972503,4.379406,2.792444,0.808740,94.911362,-16.043468,16.043468
+0.186163,-0.002441,-0.973540,4.364147,2.899258,1.129185,94.911362,-16.043468,16.043468
+0.180120,0.000732,-0.971831,4.425184,2.731407,0.854518,95.960930,-15.893529,15.593652
+0.182806,0.002930,-0.963775,4.425184,2.899258,0.900296,95.960930,-15.893529,15.593652
+0.184698,-0.010743,-0.977752,4.562517,2.868740,0.976592,96.110870,-15.743590,13.944323
+0.192083,-0.004639,-0.967986,4.440443,2.792444,0.961333,96.110870,-15.743590,13.944323
+0.176641,0.006287,-0.973235,4.394665,2.838221,0.732444,96.110870,-15.743590,13.944323
+0.185736,-0.004089,-0.965728,4.348887,2.807703,0.976592,93.711845,-15.743590,15.143834
+0.184515,-0.004700,-0.975066,4.425184,2.914518,0.991852,93.711845,-15.743590,15.143834
+0.188604,-0.004395,-0.962004,4.440443,2.853481,0.732444,96.260811,-16.193407,14.394140
+0.177374,-0.000977,-0.975616,4.409925,2.822962,0.885037,96.260811,-16.193407,14.394140
+0.187994,-0.002319,-0.963164,4.348887,2.792444,1.007111,96.260811,-16.193407,14.394140
+0.183905,-0.004578,-0.956572,4.547258,2.945036,0.900296,94.611481,-15.743590,15.443712
+0.188055,-0.006226,-0.977813,4.425184,2.914518,0.762963,94.611481,-15.743590,15.443712
+0.178228,-0.004639,-0.968596,4.364147,2.731407,0.976592,95.511116,-17.842735,14.544079
+0.193304,0.001892,-0.966765,4.379406,2.838221,0.900296,95.511116,-17.842735,14.544079
+0.179266,-0.003662,-0.963164,4.318369,2.746666,0.686666,95.511116,-17.842735,14.544079
+0.184027,-0.008423,-0.970855,4.379406,2.792444,0.976592,96.410751,-16.343346,14.544079
+0.188665,0.001038,-0.974883,4.272591,2.700888,0.900296,96.410751,-16.343346,14.544079
+0.186773,-0.001587,-0.971221,4.364147,2.777184,0.915555,94.611481,-16.043468,14.544079
+0.181097,-0.003723,-0.965545,4.470962,2.761925,0.839259,94.611481,-16.043468,14.544079
+0.190863,-0.002808,-0.973662,4.287850,2.700888,0.991852,94.611481,-16.043468,14.544079
+0.181890,-0.001709,-0.959136,4.333628,2.899258,0.900296,96.110870,-16.043468,14.843957
+0.187323,-0.003113,-0.970367,4.501480,2.868740,0.900296,96.110870,-16.043468,14.843957
+0.184698,-0.003662,-0.972320,4.425184,2.822962,0.991852,94.311600,-16.043468,16.343346
+0.187628,-0.004578,-0.963347,4.364147,2.883999,0.930815,94.311600,-16.043468,16.343346
+0.182440,-0.003479,-0.972076,4.348887,2.761925,0.900296,94.311600,-16.043468,16.343346
+0.189825,-0.002991,-0.968780,4.364147,2.761925,0.915555,94.161667,-15.593652,14.993896
+0.179937,0.000855,-0.957060,4.455702,2.853481,0.854518,94.161667,-15.593652,14.993896
+0.186529,-0.003662,-0.974334,4.318369,2.731407,0.793481,95.361176,-16.493284,14.993896
+0.183660,-0.008484,-0.963164,4.272591,2.746666,0.930815,95.361176,-16.493284,14.993896
+0.188238,0.004089,-0.973601,4.425184,2.838221,0.946074,95.361176,-16.493284,14.993896
+0.182440,-0.002503,-0.966399,4.348887,2.731407,0.930815,94.611481,-17.242979,15.443712
+0.188788,-0.004761,-0.967864,4.333628,2.777184,0.946074,94.611481,-17.242979,15.443712
+0.184637,-0.000549,-0.966704,4.364147,2.807703,0.930815,94.911362,-16.643225,14.544079
+0.183477,0.001282,-0.966216,4.303110,2.777184,0.839259,94.911362,-16.643225,14.544079
+0.184393,-0.005982,-0.974334,4.272591,2.700888,0.961333,94.911362,-16.643225,14.544079
+0.187872,-0.006348,-0.961516,4.303110,2.838221,0.930815,94.161667,-16.193407,14.993896
+0.183416,-0.002930,-0.974578,4.333628,2.777184,0.885037,94.161667,-16.193407,14.993896
+0.185980,-0.001343,-0.964324,4.303110,2.761925,0.900296,96.110870,-16.643225,15.743590
+0.182012,-0.005066,-0.969512,4.287850,2.731407,0.885037,96.110870,-16.643225,15.743590
+0.187262,-0.004334,-0.968902,4.257332,2.700888,0.900296,96.110870,-16.643225,15.743590
+0.179083,-0.004028,-0.966704,4.333628,2.777184,0.824000,95.061295,-16.493284,14.993896
+0.189276,-0.004517,-0.968535,4.409925,2.838221,0.976592,95.061295,-16.493284,14.993896
+0.183172,-0.001526,-0.965361,4.379406,2.777184,0.900296,95.211235,-16.343346,15.443712
+0.182012,-0.003479,-0.964141,4.379406,2.761925,0.885037,95.211235,-16.343346,15.443712
+0.187567,-0.005371,-0.972625,4.364147,2.838221,0.930815,95.211235,-16.343346,15.443712
+0.184271,-0.000916,-0.966826,4.333628,2.868740,1.052889,94.611481,-16.343346,14.843957
+0.186285,-0.000977,-0.974639,4.303110,2.685629,0.808740,94.611481,-16.343346,14.843957
+0.186163,-0.002686,-0.963897,4.272591,2.655110,0.900296,95.361176,-15.593652,16.493284
+0.183966,-0.000671,-0.964446,4.242073,2.792444,0.839259,95.361176,-15.593652,16.493284
+0.186285,-0.006592,-0.970122,4.348887,2.761925,0.869778,95.361176,-15.593652,16.493284
+0.186529,-0.003235,-0.972320,4.272591,2.731407,0.885037,95.361176,-15.593652,16.493284
+0.185125,-0.004883,-0.967437,4.181036,2.624592,0.854518,95.361176,-15.593652,16.493284
+0.181585,0.000427,-0.971465,4.318369,2.746666,0.946074,95.061295,-16.193407,15.293774
+0.183599,-0.002319,-0.967559,4.318369,2.731407,0.915555,95.061295,-16.193407,15.293774
+0.186895,-0.004089,-0.970183,4.379406,2.777184,0.915555,95.061295,-16.193407,15.293774
+0.185125,-0.003723,-0.963469,4.455702,2.685629,0.808740,94.461540,-17.692797,14.993896
+0.181524,-0.003235,-0.973662,4.318369,2.838221,0.839259,94.461540,-17.692797,14.993896
+0.187933,-0.003540,-0.959624,4.425184,2.853481,1.068148,93.711845,-15.443712,15.143834
+0.180853,-0.001343,-0.969146,4.409925,2.777184,0.808740,93.711845,-15.443712,15.143834
+0.189947,-0.004028,-0.972686,4.379406,2.746666,0.930815,93.711845,-15.443712,15.143834
+0.179876,-0.001282,-0.963713,4.257332,2.761925,0.869778,95.361176,-17.692797,14.993896
+0.190924,-0.001038,-0.974212,4.287850,2.700888,0.976592,95.361176,-17.692797,14.993896
+0.176031,-0.004761,-0.964263,4.287850,2.670370,0.793481,95.361176,-16.793163,14.993896
+0.196844,-0.005554,-0.977935,4.364147,2.731407,0.930815,95.361176,-16.793163,14.993896
+0.170843,0.001587,-0.955901,4.333628,2.761925,0.839259,95.361176,-16.793163,14.993896
+0.195502,-0.003723,-0.978240,4.379406,2.746666,0.976592,94.911362,-18.142614,16.043468
+0.176885,0.001038,-0.954131,4.379406,2.883999,0.900296,94.911362,-18.142614,16.043468
+0.189032,-0.007385,-0.979400,4.333628,2.731407,0.915555,95.061295,-17.093040,15.293774
+0.179815,-0.000732,-0.955351,4.394665,2.838221,0.900296,95.061295,-17.093040,15.293774
+0.188604,-0.006897,-0.971343,4.425184,2.914518,0.869778,95.061295,-17.093040,15.293774
+0.180303,-0.003113,-0.960418,4.348887,2.746666,0.946074,94.611481,-16.643225,15.443712
+0.189093,-0.002441,-0.970977,4.333628,2.777184,0.946074,94.611481,-16.643225,15.443712
+0.181951,-0.001526,-0.963591,4.333628,2.761925,0.854518,94.161667,-16.493284,14.394140
+0.183538,-0.003296,-0.978362,4.333628,2.731407,0.869778,94.161667,-16.493284,14.394140
+0.186102,-0.004334,-0.961943,4.364147,2.792444,0.946074,94.161667,-16.493284,14.394140
+0.183599,-0.002014,-0.973296,4.333628,2.761925,0.824000,94.761421,-17.992674,15.293774
+0.183538,-0.005615,-0.963103,4.333628,2.807703,0.915555,94.761421,-17.992674,15.293774
+0.178411,-0.002136,-0.963713,4.211555,2.716147,0.915555,94.461540,-16.193407,14.394140
+0.193426,-0.005554,-0.979339,4.150517,2.624592,1.007111,94.461540,-16.193407,14.394140
+0.174993,-0.004089,-0.970611,3.814814,2.471999,0.915555,94.461540,-16.193407,14.394140
+0.191168,0.003601,-0.959807,3.952147,2.471999,1.190222,95.810989,-15.443712,15.443712
+0.178777,0.003845,-0.979461,4.440443,2.655110,0.717185,95.810989,-15.443712,15.443712
+0.183111,-0.004639,-0.958708,4.364147,2.639851,1.129185,94.611481,-16.343346,15.143834
+0.183721,-0.007569,-0.985015,4.348887,2.853481,0.900296,94.611481,-16.343346,15.143834
+0.195807,-0.002014,-0.959563,4.379406,2.700888,0.732444,95.361176,-15.293774,14.993896
+0.170537,0.002930,-0.977325,4.455702,2.777184,0.778222,95.361176,-15.293774,14.993896
+0.193243,-0.001770,-0.953764,4.272591,2.792444,1.098666,95.361176,-15.293774,14.993896
+0.178655,-0.004212,-0.968719,4.455702,2.899258,0.717185,94.311600,-15.743590,13.944323
+0.192450,-0.007385,-0.966460,4.379406,2.792444,0.991852,94.311600,-15.743590,13.944323
+0.174200,0.002686,-0.972503,4.364147,2.792444,0.946074,95.810989,-16.943102,14.544079
+0.190069,-0.000488,-0.964934,4.318369,2.746666,0.885037,95.810989,-16.943102,14.544079
+0.177252,-0.000977,-0.963347,4.242073,2.853481,0.946074,95.810989,-16.943102,14.544079
+0.193121,-0.009949,-0.975249,4.394665,2.746666,0.930815,95.960930,-16.193407,14.394140
+0.180914,0.003113,-0.957488,4.318369,2.731407,1.007111,95.960930,-16.193407,14.394140
+0.180609,0.001099,-0.978118,4.348887,2.807703,0.900296,94.461540,-16.493284,14.694017
+0.180609,0.000305,-0.960234,4.348887,2.853481,0.991852,94.461540,-16.493284,14.694017
+0.187262,-0.005737,-0.981780,4.318369,2.700888,0.793481,94.461540,-16.493284,14.694017
+0.182684,-0.005554,-0.953764,4.364147,2.822962,1.113926,94.011726,-15.443712,14.544079
+0.178472,0.003601,-0.973540,4.348887,2.731407,0.793481,94.011726,-15.443712,14.544079
+0.189215,-0.003418,-0.966582,4.501480,2.792444,0.946074,94.461540,-15.593652,14.694017
+0.179998,-0.003296,-0.964446,4.226814,2.807703,0.778222,94.461540,-15.593652,14.694017
+0.188299,-0.002441,-0.977142,4.333628,2.655110,0.946074,94.461540,-15.593652,14.694017
+0.182257,0.001282,-0.956084,4.181036,2.685629,0.717185,95.061295,-15.593652,14.993896
+0.186285,-0.003906,-0.982818,4.303110,2.563555,0.671407,95.061295,-15.593652,14.993896
+0.180425,-0.005493,-0.953764,4.272591,2.792444,0.946074,94.161667,-15.593652,14.993896
+0.181768,-0.004944,-0.980682,4.379406,2.761925,0.885037,94.161667,-15.593652,14.993896
+0.189825,-0.000122,-0.956999,4.318369,2.792444,0.991852,94.161667,-15.593652,14.993896
+0.173528,0.000732,-0.978301,4.409925,2.777184,0.686666,96.260811,-15.293774,15.293774
+0.192022,-0.005127,-0.962310,4.348887,2.761925,1.083407,96.260811,-15.293774,15.293774
+0.178228,-0.000793,-0.970977,4.272591,2.746666,0.793481,95.810989,-16.343346,15.143834
+0.192633,-0.001038,-0.967376,4.348887,2.731407,0.900296,95.810989,-16.343346,15.143834
+0.169317,-0.000488,-0.964995,4.379406,2.868740,0.930815,95.810989,-16.343346,15.143834
+0.202338,-0.006897,-0.969268,4.272591,2.777184,1.083407,95.061295,-17.093040,15.293774
+0.174383,-0.000916,-0.963347,4.242073,2.746666,0.900296,95.061295,-17.093040,15.293774
+0.188482,0.004456,-0.972198,4.303110,2.731407,0.869778,95.061295,-16.493284,14.394140
+0.172369,0.014710,-0.960356,4.394665,2.792444,0.793481,95.061295,-16.493284,14.394140
+0.187933,-0.009644,-0.972747,4.348887,2.853481,0.900296,95.061295,-16.493284,14.394140
+0.185125,-0.017518,-0.968230,4.455702,2.822962,1.022370,95.061295,-16.193407,14.394140
+0.190008,0.003601,-0.967132,4.409925,2.807703,0.808740,95.061295,-16.193407,14.394140
+0.170293,0.003479,-0.973174,4.470962,2.731407,0.778222,94.311600,-15.143834,15.743590
+0.154302,-0.001221,-0.998627,4.318369,2.807703,1.113926,94.311600,-15.143834,15.743590
+0.195990,-0.010193,-0.967986,4.501480,2.716147,0.793481,94.311600,-15.143834,15.743590
+0.186224,-0.005554,-0.983123,4.303110,2.639851,0.915555,96.110870,-17.542858,14.544079
+0.173772,0.006897,-0.978912,4.440443,2.761925,0.808740,96.110870,-17.542858,14.544079
+0.188604,-0.000916,-0.943327,4.211555,2.883999,1.098666,95.960930,-15.893529,15.893529
+0.180670,-0.006470,-0.968169,4.303110,2.899258,0.839259,95.960930,-15.893529,15.893529
+0.199103,-0.010559,-0.951567,4.318369,2.731407,0.961333,95.960930,-15.893529,15.893529
+0.171026,0.005127,-0.974395,4.409925,2.807703,0.869778,95.211235,-16.943102,14.244201
+0.195685,-0.002747,-0.963713,4.287850,2.883999,1.052889,95.211235,-16.943102,14.244201
+0.170965,-0.008423,-0.961272,4.547258,2.945036,0.869778,95.061295,-17.392920,14.394140
+0.195379,-0.005188,-0.950102,4.303110,2.792444,1.052889,95.061295,-17.392920,14.394140
+0.175176,0.006470,-0.987335,4.440443,2.807703,0.762963,95.061295,-17.392920,14.394140
+0.192755,-0.007324,-0.965667,4.257332,2.670370,0.961333,95.061295,-17.392920,14.394140
+0.173345,0.004456,-0.993561,4.486221,2.639851,0.854518,95.061295,-17.392920,14.394140
+0.201544,-0.008362,-0.944182,4.333628,2.883999,0.930815,95.211235,-16.343346,15.443712
+0.174688,0.000610,-0.971221,4.181036,2.960295,0.762963,95.211235,-16.343346,15.443712
+0.184942,-0.004028,-0.980987,4.333628,2.594073,1.007111,95.211235,-16.343346,15.443712
+0.180914,0.004456,-0.979705,4.272591,2.883999,0.869778,95.211235,-16.643225,14.244201
+0.189947,-0.011536,-0.975860,4.394665,2.578814,0.839259,95.211235,-16.643225,14.244201
+0.175726,0.002808,-0.971709,4.303110,2.792444,0.915555,95.061295,-16.493284,15.893529
+0.188665,-0.002747,-0.966033,4.409925,2.731407,0.747703,95.061295,-16.493284,15.893529
+0.176458,0.000855,-0.966033,4.455702,2.731407,0.961333,95.061295,-16.493284,15.893529
+0.188726,-0.006714,-0.980987,4.348887,2.639851,0.824000,94.461540,-16.493284,15.593652
+0.179937,-0.001038,-0.966338,4.287850,2.731407,0.930815,94.461540,-16.493284,15.593652
+0.184088,-0.003784,-0.976409,4.333628,2.700888,0.808740,95.061295,-17.093040,14.993896
+0.166082,0.004089,-0.978423,4.211555,2.746666,1.037629,95.061295,-17.093040,14.993896
+0.194952,-0.011414,-0.978912,4.333628,2.746666,0.930815,95.061295,-17.093040,14.993896
+0.180059,-0.002075,-0.973540,4.379406,2.670370,0.961333,95.960930,-16.493284,14.694017
+0.181707,0.000488,-0.969756,4.333628,2.731407,0.808740,95.960930,-16.493284,14.694017
+0.184515,0.002808,-0.967803,4.333628,2.792444,0.991852,95.361176,-14.694017,14.993896
+0.184515,-0.004944,-0.964812,4.318369,2.700888,0.762963,95.361176,-14.694017,14.993896
+0.186407,-0.006165,-0.969390,4.287850,2.792444,0.900296,95.361176,-14.694017,14.993896
+0.176458,0.002197,-0.978057,4.379406,2.807703,0.885037,94.911362,-16.643225,14.544079
+0.184027,0.002747,-0.975311,4.257332,2.731407,0.869778,94.911362,-16.643225,14.544079
+0.180792,-0.006958,-0.967742,4.348887,2.700888,0.808740,95.511116,-16.343346,16.043468
+0.191656,-0.002380,-0.969512,4.303110,2.792444,0.961333,95.511116,-16.343346,16.043468
+0.178106,0.000916,-0.970916,4.394665,2.822962,0.747703,95.511116,-16.343346,16.043468
+0.184271,-0.001343,-0.965789,4.257332,2.792444,1.022370,95.960930,-16.493284,15.893529
+0.181646,-0.002747,-0.970367,4.333628,2.838221,0.824000,95.960930,-16.493284,15.893529
+0.187628,-0.009156,-0.974273,4.318369,2.746666,0.976592,95.511116,-15.743590,14.843957
+0.183905,-0.002869,-0.968474,4.318369,2.807703,0.839259,95.511116,-15.743590,14.843957
+0.179937,0.000000,-0.965667,4.303110,2.777184,0.793481,95.511116,-15.743590,14.843957
+0.182379,0.003052,-0.967437,4.318369,2.822962,0.885037,95.361176,-15.593652,15.293774
+0.181707,-0.005127,-0.962371,4.257332,2.777184,0.946074,95.361176,-15.593652,15.293774
+0.187689,-0.005737,-0.975860,4.364147,2.746666,0.900296,94.911362,-17.242979,16.343346
+0.183233,0.001404,-0.963836,4.348887,2.746666,0.946074,94.911362,-17.242979,16.343346
+0.181524,0.002258,-0.969756,4.364147,2.746666,0.885037,95.661057,-16.493284,15.293774
+0.184942,-0.005615,-0.965972,4.379406,2.777184,0.991852,95.661057,-16.493284,15.293774
+0.187994,-0.002380,-0.972503,4.379406,2.807703,0.915555,95.661057,-16.493284,15.293774
+0.182684,-0.002503,-0.968963,4.303110,2.716147,0.915555,95.211235,-16.643225,16.043468
+0.181097,0.002319,-0.966826,4.287850,2.868740,0.900296,95.211235,-16.643225,16.043468
+0.185003,-0.003113,-0.972747,4.394665,2.807703,0.915555,95.211235,-17.542858,15.143834
+0.184881,-0.004212,-0.967803,4.287850,2.792444,0.930815,95.211235,-17.542858,15.143834
+0.181158,-0.002564,-0.971831,4.333628,2.746666,0.900296,95.211235,-17.542858,15.143834
+0.187628,-0.001282,-0.962432,4.333628,2.853481,0.869778,95.211235,-16.643225,15.443712
+0.179205,0.001099,-0.974822,4.364147,2.777184,0.885037,95.211235,-16.643225,15.443712
+0.186407,-0.002869,-0.966155,4.242073,2.731407,0.930815,94.611481,-16.643225,14.843957
+0.181219,-0.002991,-0.969573,4.257332,2.822962,0.885037,94.611481,-16.643225,14.843957
+0.186224,-0.003052,-0.966460,4.318369,2.731407,0.961333,94.611481,-16.643225,14.843957
+0.180975,-0.000855,-0.971038,4.409925,2.761925,0.808740,96.110870,-15.443712,15.443712
+0.186468,0.000000,-0.965056,4.272591,2.731407,0.991852,96.110870,-15.443712,15.443712
+0.180364,-0.002014,-0.967559,4.409925,2.838221,0.946074,95.361176,-17.392920,14.993896
+0.187323,-0.004700,-0.970183,4.409925,2.670370,0.930815,95.361176,-17.392920,14.993896
+0.181341,0.000305,-0.964141,4.379406,2.853481,0.900296,95.361176,-17.392920,14.993896
+0.183050,-0.002747,-0.968841,4.318369,2.777184,0.930815,95.960930,-16.793163,13.494506
+0.185980,-0.001221,-0.968841,4.272591,2.731407,0.930815,95.960930,-16.793163,13.494506
+0.178594,-0.000122,-0.970611,4.272591,2.807703,0.869778,94.761421,-15.593652,16.193407
+0.184576,0.000183,-0.966948,4.379406,2.822962,0.900296,94.761421,-15.593652,16.193407
+0.183233,-0.002380,-0.974822,4.318369,2.746666,0.900296,94.761421,-15.593652,16.193407
+0.184759,-0.000488,-0.966826,4.348887,2.761925,0.961333,95.511116,-16.343346,16.043468
+0.182684,-0.002197,-0.967559,4.272591,2.777184,0.915555,95.511116,-16.343346,16.043468
+0.186407,-0.002441,-0.967681,4.394665,2.777184,0.900296,95.661057,-15.893529,15.293774
+0.180486,-0.002625,-0.968291,4.318369,2.807703,0.930815,95.661057,-15.893529,15.293774
+0.185492,-0.001282,-0.970061,4.272591,2.822962,0.976592,95.661057,-15.893529,15.293774
+0.179510,-0.002930,-0.970916,4.379406,2.792444,0.900296,95.061295,-15.593652,14.993896
+0.188971,-0.002991,-0.967071,4.318369,2.777184,1.037629,95.061295,-15.593652,14.993896
+0.179449,-0.003113,-0.971404,4.333628,2.746666,0.946074,95.061295,-16.193407,14.394140
+0.187933,-0.003967,-0.964263,4.303110,2.731407,0.900296,95.061295,-16.193407,14.394140
+0.180792,-0.000427,-0.974090,4.333628,2.746666,0.793481,95.061295,-16.193407,14.394140
+0.180059,-0.003967,-0.964629,4.364147,2.746666,0.946074,95.361176,-17.392920,15.293774
+0.185308,-0.002136,-0.971282,4.333628,2.807703,0.915555,95.361176,-17.392920,15.293774
+0.183721,-0.004578,-0.965972,4.379406,2.792444,0.900296,95.511116,-17.542858,15.743590
+0.183050,0.001404,-0.968902,4.303110,2.761925,0.824000,95.511116,-17.542858,15.743590
+0.182012,-0.000916,-0.966399,4.409925,2.731407,0.900296,95.511116,-17.542858,15.743590
+0.183050,-0.002808,-0.965728,4.303110,2.853481,1.007111,95.661057,-15.893529,16.493284
+0.182867,-0.003601,-0.970733,4.318369,2.700888,0.824000,95.661057,-15.893529,16.493284
+0.185430,0.001038,-0.962676,4.303110,2.822962,0.854518,95.211235,-17.242979,15.143834
+0.181829,-0.001709,-0.971770,4.333628,2.822962,0.839259,95.211235,-17.242979,15.143834
+0.184759,-0.000977,-0.965361,4.287850,2.792444,1.007111,95.211235,-17.242979,15.143834
+0.182562,-0.004334,-0.972442,4.348887,2.807703,0.885037,95.661057,-17.392920,14.993896
+0.185186,-0.004761,-0.963836,4.303110,2.700888,0.946074,95.661057,-17.392920,14.993896
+0.180059,0.000183,-0.960479,4.318369,2.777184,0.839259,95.211235,-16.943102,14.843957
+0.182318,-0.000855,-0.967986,4.272591,2.761925,0.961333,95.211235,-16.943102,14.843957
+0.185430,-0.003601,-0.971160,4.364147,2.807703,0.915555,95.211235,-16.943102,14.843957
+0.183416,-0.005676,-0.970428,4.318369,2.761925,0.961333,95.061295,-16.193407,14.394140
+0.183477,-0.000183,-0.967559,4.364147,2.700888,0.961333,95.061295,-16.193407,14.394140
+0.178472,0.000610,-0.967437,4.425184,2.777184,0.900296,94.011726,-16.643225,16.043468
+0.185430,-0.002747,-0.969085,4.272591,2.822962,0.930815,94.011726,-16.643225,16.043468
+0.182257,-0.002747,-0.968047,4.287850,2.792444,0.854518,94.011726,-16.643225,16.043468
+0.188971,-0.000671,-0.965972,4.318369,2.670370,0.915555,96.860565,-16.193407,15.293774
+0.177190,0.001282,-0.967193,4.318369,2.685629,0.900296,96.860565,-16.193407,15.293774
+0.185919,-0.002258,-0.964202,4.348887,2.731407,0.991852,95.661057,-16.493284,14.694017
+0.183477,-0.004334,-0.969512,4.379406,2.746666,0.900296,95.661057,-16.493284,14.694017
+0.186590,-0.005799,-0.967071,4.333628,2.700888,0.915555,95.661057,-16.493284,14.694017
+0.178838,-0.001038,-0.962249,4.318369,2.853481,0.915555,95.960930,-16.193407,15.593652
+0.188116,-0.001587,-0.966155,4.303110,2.822962,0.930815,95.960930,-16.193407,15.593652
+0.182379,-0.006104,-0.966765,4.272591,2.761925,0.869778,95.511116,-15.443712,16.343346
+0.188543,-0.004212,-0.965789,4.333628,2.761925,0.976592,95.511116,-15.443712,16.343346
+0.180181,0.005005,-0.971221,4.394665,2.822962,0.808740,95.511116,-15.443712,16.343346
+0.184454,-0.002869,-0.966826,4.303110,2.761925,0.930815,94.911362,-16.343346,14.843957
+0.183660,-0.005310,-0.972869,4.364147,2.731407,0.946074,94.911362,-16.343346,14.843957
+0.187262,-0.003235,-0.964873,4.318369,2.716147,0.991852,95.511116,-16.043468,16.043468
+0.180853,0.001709,-0.968657,4.379406,2.807703,0.900296,95.511116,-16.043468,16.043468
+0.181951,-0.003906,-0.969817,4.333628,2.838221,0.930815,95.511116,-16.043468,16.043468
+0.185064,-0.002564,-0.970244,4.379406,2.777184,0.915555,96.710625,-15.743590,15.743590
+0.185186,-0.003906,-0.971160,4.394665,2.746666,0.869778,96.710625,-15.743590,15.743590
+0.182806,0.000305,-0.967681,4.394665,2.792444,0.900296,94.461540,-16.193407,14.694017
+0.181280,0.001221,-0.966704,4.394665,2.807703,0.839259,94.461540,-16.193407,14.694017
+0.184698,-0.005310,-0.967864,4.303110,2.822962,0.991852,94.461540,-16.193407,14.694017
+0.183111,-0.003418,-0.967071,4.318369,2.777184,0.930815,95.061295,-16.193407,14.694017
+0.182745,0.000977,-0.962981,4.303110,2.792444,0.839259,95.061295,-16.193407,14.694017
+0.181524,0.000427,-0.969695,4.272591,2.807703,0.869778,95.511116,-16.943102,15.143834
+0.184088,-0.004089,-0.969634,4.287850,2.792444,0.946074,95.511116,-16.943102,15.143834
+0.188360,-0.003967,-0.968474,4.348887,2.655110,0.930815,95.511116,-16.943102,15.143834
+0.181158,-0.000610,-0.965789,4.287850,2.792444,0.869778,94.311600,-16.643225,15.443712
+0.183538,0.002075,-0.967376,4.348887,2.761925,0.930815,94.311600,-16.643225,15.443712
+0.182806,-0.005799,-0.967498,4.364147,2.838221,0.976592,95.211235,-16.343346,14.544079
+0.188788,-0.003296,-0.966521,4.364147,2.746666,0.900296,95.211235,-16.343346,14.544079
+0.179449,-0.000610,-0.969939,4.425184,2.700888,0.824000,95.960930,-16.793163,15.593652
+0.182195,-0.000549,-0.965789,4.364147,2.822962,0.930815,95.960930,-16.793163,15.593652
+0.183355,-0.002686,-0.967925,4.318369,2.807703,0.946074,95.960930,-16.793163,15.593652
+0.181097,-0.005310,-0.966338,4.394665,2.807703,0.869778,95.810989,-16.943102,13.944323
+0.189520,-0.002441,-0.968596,4.287850,2.761925,0.961333,95.810989,-16.943102,13.944323
+0.180425,0.002808,-0.967864,4.318369,2.731407,1.037629,95.661057,-15.893529,15.893529
+0.185186,-0.003357,-0.966216,4.364147,2.716147,0.946074,95.661057,-15.893529,15.893529
+0.180670,-0.007447,-0.972259,4.348887,2.822962,0.961333,95.661057,-15.893529,15.893529
+0.184698,-0.000916,-0.964995,4.379406,2.761925,0.946074,95.960930,-17.392920,14.394140
+0.181158,-0.000671,-0.970061,4.364147,2.716147,0.885037,95.960930,-17.392920,14.394140
+0.182562,-0.000061,-0.971282,4.303110,2.731407,0.961333,95.211235,-15.443712,16.043468
+0.181707,-0.005371,-0.968963,4.318369,2.822962,0.869778,95.211235,-15.443712,16.043468
+0.186895,-0.002991,-0.965667,4.257332,2.761925,0.946074,95.211235,-15.443712,16.043468
+0.178411,0.000793,-0.967620,4.318369,2.777184,0.930815,95.960930,-16.493284,14.394140
+0.187567,0.000488,-0.968902,4.333628,2.731407,0.869778,95.960930,-16.493284,14.394140
+0.179754,-0.004456,-0.970550,4.379406,2.685629,0.900296,94.611481,-16.643225,14.843957
+0.186285,-0.002564,-0.968047,4.348887,2.716147,0.961333,94.611481,-16.643225,14.843957
+0.177923,-0.001038,-0.967009,4.379406,2.746666,0.885037,94.611481,-16.643225,14.843957
+0.187017,0.000549,-0.967986,4.333628,2.716147,0.854518,94.911362,-15.743590,15.743590
+0.179754,-0.002930,-0.969268,4.333628,2.777184,0.946074,94.911362,-15.743590,15.743590
+0.188299,-0.003052,-0.969329,4.303110,2.746666,0.946074,96.260811,-16.493284,15.593652
+0.182867,-0.002930,-0.970550,4.318369,2.700888,0.915555,96.260811,-16.493284,15.593652
+0.185492,-0.000366,-0.965911,4.348887,2.838221,0.885037,96.260811,-16.493284,15.593652
+0.182073,0.000244,-0.967620,4.470962,2.868740,0.900296,95.511116,-16.043468,15.143834
+0.187201,-0.003418,-0.968963,4.333628,2.777184,0.915555,95.511116,-16.043468,15.143834
+0.184027,-0.004700,-0.973662,4.364147,2.761925,0.900296,95.061295,-17.093040,15.293774
+0.183905,0.001099,-0.967254,4.333628,2.716147,0.915555,95.061295,-17.093040,15.293774
+0.182806,-0.002808,-0.972015,4.409925,2.761925,0.869778,95.061295,-17.093040,15.293774
+0.185064,-0.003845,-0.968535,4.333628,2.746666,0.961333,94.611481,-17.242979,14.843957
+0.183599,-0.001587,-0.966521,4.333628,2.777184,0.930815,94.611481,-17.242979,14.843957
+0.181158,-0.001038,-0.966460,4.318369,2.731407,0.839259,95.661057,-15.893529,15.593652
+0.184393,-0.000183,-0.969634,4.348887,2.731407,0.946074,95.661057,-15.893529,15.593652
+0.183966,-0.002686,-0.967498,4.394665,2.716147,0.930815,95.661057,-15.893529,15.593652
+0.183905,-0.002930,-0.969634,4.333628,2.777184,0.869778,94.911362,-16.943102,15.443712
+0.184088,-0.000122,-0.965850,4.318369,2.883999,0.915555,94.911362,-16.943102,15.443712
+0.181646,0.000183,-0.966704,4.303110,2.685629,0.900296,95.361176,-17.392920,15.293774
+0.185308,-0.005371,-0.968902,4.348887,2.761925,0.946074,95.361176,-17.392920,15.293774
+0.182318,-0.003784,-0.966277,4.333628,2.822962,0.900296,95.361176,-17.392920,15.293774
+0.183111,-0.001404,-0.968841,4.333628,2.731407,0.930815,95.361176,-16.493284,15.593652
+0.184149,0.001343,-0.966704,4.379406,2.822962,0.915555,95.361176,-16.493284,15.593652
+0.182318,-0.003418,-0.968230,4.409925,2.685629,0.900296,95.361176,-16.793163,14.993896
+0.185553,-0.002991,-0.968780,4.379406,2.716147,0.930815,95.361176,-16.793163,14.993896
+0.181524,-0.003418,-0.969146,4.394665,2.838221,0.869778,95.361176,-16.793163,14.993896
+0.184149,0.000244,-0.970733,4.303110,2.761925,0.976592,96.410751,-16.643225,15.443712
+0.180853,0.000427,-0.968230,4.394665,2.777184,0.900296,96.410751,-16.643225,15.443712
+0.184698,-0.001770,-0.968719,4.364147,2.838221,0.915555,95.361176,-16.193407,13.794384
+0.182928,-0.001953,-0.968291,4.394665,2.777184,0.900296,95.361176,-16.193407,13.794384
+0.185247,-0.000244,-0.970733,4.364147,2.761925,0.854518,95.361176,-16.193407,13.794384
+0.182379,-0.000916,-0.968474,4.364147,2.838221,0.930815,95.661057,-16.493284,14.694017
+0.186163,-0.001770,-0.968841,4.394665,2.655110,0.869778,95.661057,-16.493284,14.694017
+0.184698,-0.003967,-0.965789,4.272591,2.746666,0.839259,95.361176,-16.793163,15.293774
+0.182134,-0.000671,-0.964629,4.303110,2.777184,0.869778,95.361176,-16.793163,15.293774
+0.185003,-0.000610,-0.968963,4.333628,2.700888,1.007111,95.361176,-16.793163,15.293774
+0.183905,-0.001892,-0.968535,4.333628,2.807703,0.946074,94.911362,-16.943102,14.544079
+0.184820,-0.004639,-0.971038,4.287850,2.777184,0.915555,94.911362,-16.943102,14.544079
+0.182501,-0.002564,-0.964507,4.333628,2.731407,0.930815,95.661057,-17.093040,15.593652
+0.182989,-0.001709,-0.966033,4.318369,2.761925,0.854518,95.661057,-17.093040,15.593652
+0.182562,-0.003906,-0.968047,4.394665,2.700888,0.976592,95.661057,-17.093040,15.593652
+0.183538,-0.002075,-0.968780,4.318369,2.746666,0.900296,94.911362,-16.643225,15.143834
+0.183477,-0.001160,-0.967864,4.348887,2.761925,0.885037,94.911362,-16.643225,15.143834
+0.183966,-0.003357,-0.966582,4.333628,2.838221,0.961333,95.960930,-17.392920,14.694017
+0.183050,-0.001465,-0.971282,4.303110,2.746666,0.839259,95.960930,-17.392920,14.694017
+0.184454,-0.001648,-0.967803,4.440443,2.822962,0.991852,95.960930,-17.392920,14.694017
+0.183233,-0.002503,-0.970122,4.333628,2.777184,0.946074,95.211235,-16.643225,15.443712
+0.183782,-0.001709,-0.966948,4.409925,2.700888,0.900296,95.211235,-16.643225,15.443712
+0.181829,-0.002441,-0.963103,4.333628,2.746666,0.915555,95.511116,-16.643225,15.143834
+0.184149,-0.003174,-0.967925,4.394665,2.807703,0.900296,95.511116,-16.643225,15.143834
+0.184271,-0.001953,-0.967925,4.425184,2.838221,0.854518,95.511116,-16.643225,15.143834
+0.183172,-0.001831,-0.970550,4.379406,2.731407,0.915555,95.061295,-16.493284,15.593652
+0.183111,-0.000122,-0.968169,4.303110,2.670370,0.839259,95.061295,-16.493284,15.593652
+0.184271,-0.000122,-0.967009,4.348887,2.670370,0.900296,96.260811,-15.893529,14.694017
+0.184881,-0.001465,-0.966521,4.181036,2.716147,0.946074,96.260811,-15.893529,14.694017
+0.184149,-0.001282,-0.972930,4.318369,2.731407,0.946074,96.260811,-15.893529,14.694017
+0.183294,-0.003113,-0.968108,4.379406,2.822962,0.961333,95.361176,-15.893529,14.993896
+0.182134,-0.003174,-0.968719,4.318369,2.731407,0.869778,95.361176,-15.893529,14.993896
+0.184881,-0.000244,-0.963042,4.303110,2.716147,0.930815,96.110870,-15.743590,14.843957
+0.183233,0.001648,-0.971343,4.303110,2.761925,0.915555,96.110870,-15.743590,14.843957
+0.183966,-0.003662,-0.966765,4.348887,2.761925,0.839259,96.110870,-15.743590,14.843957
+0.185369,-0.002747,-0.966399,4.333628,2.807703,0.946074,97.010506,-16.943102,15.743590
+0.184759,0.000061,-0.968780,4.394665,2.746666,0.808740,97.010506,-16.943102,15.743590
+0.184149,-0.001282,-0.967315,4.303110,2.761925,0.900296,95.661057,-16.793163,15.293774
+0.184759,-0.004517,-0.971892,4.272591,2.716147,0.930815,95.661057,-16.793163,15.293774
+0.183477,-0.002319,-0.968719,4.348887,2.731407,0.930815,95.810989,-16.043468,15.743590
+0.183538,-0.002808,-0.967498,4.303110,2.838221,0.885037,95.810989,-16.043468,15.743590
+0.184637,-0.002503,-0.968719,4.303110,2.777184,0.930815,95.810989,-16.043468,15.743590
+0.183233,-0.001160,-0.966704,4.303110,2.899258,0.946074,95.960930,-17.093040,15.293774
+0.183782,-0.002258,-0.967009,4.394665,2.655110,0.885037,95.960930,-17.093040,15.293774
+0.182318,-0.002747,-0.967925,4.287850,2.746666,0.946074,94.611481,-16.343346,15.443712
+0.183111,0.000793,-0.970122,4.303110,2.746666,0.900296,94.611481,-16.343346,15.443712
+0.183905,-0.005127,-0.970061,4.348887,2.792444,0.976592,94.611481,-16.343346,15.443712
+0.183660,-0.003540,-0.971526,4.364147,2.822962,0.915555,95.061295,-16.793163,14.694017
+0.184332,-0.002930,-0.969268,4.318369,2.700888,0.854518,95.061295,-16.793163,14.694017
+0.179815,-0.002869,-0.970733,4.348887,2.731407,0.900296,95.061295,-17.392920,14.694017
+0.184210,-0.001892,-0.966277,4.394665,2.746666,0.930815,95.061295,-17.392920,14.694017
+0.182195,-0.002503,-0.970061,4.303110,2.807703,0.930815,95.061295,-17.392920,14.694017
+0.185430,-0.000244,-0.968047,4.364147,2.761925,0.885037,95.061295,-17.392920,14.694017
+0.182562,-0.001953,-0.967009,4.333628,2.746666,0.961333,95.061295,-17.392920,14.694017
+0.184149,-0.000427,-0.971954,4.303110,2.761925,0.900296,96.410751,-15.443712,15.443712
+0.182806,-0.003906,-0.966033,4.303110,2.700888,0.946074,96.410751,-15.443712,15.443712
+0.184942,-0.001770,-0.971038,4.242073,2.822962,0.885037,96.410751,-15.443712,15.443712
+0.184454,-0.001343,-0.968902,4.364147,2.746666,0.900296,95.511116,-16.043468,15.143834
+0.181829,-0.001953,-0.970306,4.333628,2.822962,0.869778,95.511116,-16.043468,15.143834
+0.183660,-0.001953,-0.967498,4.303110,2.792444,0.869778,94.161667,-16.793163,15.293774
+0.185675,-0.000732,-0.964202,4.303110,2.746666,0.961333,94.161667,-16.793163,15.293774
+0.183721,0.000000,-0.968841,4.348887,2.792444,0.976592,94.161667,-16.793163,15.293774
+0.183477,-0.003418,-0.965056,4.364147,2.761925,0.961333,96.110870,-16.643225,15.143834
+0.184088,-0.001526,-0.967864,4.379406,2.716147,0.808740,96.110870,-16.643225,15.143834
+0.183721,-0.001343,-0.964751,4.394665,2.807703,0.946074,95.661057,-15.893529,15.293774
+0.182257,-0.001160,-0.973418,4.348887,2.868740,0.930815,95.661057,-15.893529,15.293774
+0.183172,-0.003235,-0.966460,4.440443,2.731407,0.869778,95.661057,-15.893529,15.293774
+0.183782,-0.002197,-0.970489,4.379406,2.853481,0.900296,95.661057,-17.692797,14.694017
+0.185430,-0.001587,-0.966094,4.348887,2.746666,0.900296,95.661057,-17.692797,14.694017
+0.182745,-0.001892,-0.962188,4.287850,2.746666,0.961333,95.361176,-16.193407,15.293774
+0.185186,-0.000671,-0.970367,4.318369,2.807703,0.946074,95.361176,-16.193407,15.293774
+0.184515,-0.001221,-0.966765,4.318369,2.731407,0.930815,95.361176,-16.193407,15.293774
+0.182379,-0.002258,-0.969695,4.394665,2.792444,0.976592,95.511116,-16.943102,14.244201
+0.182257,-0.001831,-0.966704,4.318369,2.746666,0.854518,95.511116,-16.943102,14.244201
+0.183477,-0.003662,-0.970489,4.318369,2.731407,0.885037,95.061295,-16.493284,14.694017
+0.184027,-0.001892,-0.965056,4.379406,2.822962,0.900296,95.061295,-16.493284,14.694017
+0.181829,-0.002686,-0.966765,4.348887,2.716147,0.930815,95.061295,-16.493284,14.694017
+0.184759,0.000000,-0.973113,4.364147,2.792444,0.885037,95.810989,-16.943102,15.743590
+0.179632,0.000549,-0.963836,4.318369,2.716147,1.007111,95.810989,-16.943102,15.743590
+0.185369,-0.002136,-0.966521,4.409925,2.777184,0.869778,95.211235,-16.343346,14.843957
+0.183905,-0.003662,-0.965911,4.348887,2.700888,0.930815,95.211235,-16.343346,14.843957
+0.182501,0.000061,-0.969878,4.333628,2.807703,0.915555,95.211235,-16.343346,14.843957
+0.184698,-0.002014,-0.967620,4.333628,2.700888,1.007111,94.911362,-16.943102,15.743590
+0.183782,-0.003357,-0.969268,4.318369,2.853481,0.930815,94.911362,-16.943102,15.743590
+0.186712,-0.002197,-0.964873,4.379406,2.746666,0.885037,94.911362,-16.043468,15.743590
+0.183416,0.000305,-0.967681,4.303110,2.777184,0.946074,94.911362,-16.043468,15.743590
+0.184576,-0.003174,-0.969085,4.272591,2.716147,0.885037,94.911362,-16.343346,14.544079
+0.180914,-0.000244,-0.965239,4.348887,2.792444,0.976592,94.911362,-16.343346,14.544079
+0.182745,-0.002930,-0.969573,4.333628,2.853481,0.869778,94.911362,-16.343346,14.544079
+0.184942,-0.003845,-0.963897,4.272591,2.761925,0.885037,95.211235,-16.043468,14.843957
+0.181890,-0.001526,-0.971709,4.211555,2.731407,0.930815,95.211235,-16.043468,14.843957
+0.187017,-0.003479,-0.962554,4.287850,2.822962,0.915555,95.061295,-15.593652,15.893529
+0.181524,-0.004761,-0.968841,4.333628,2.761925,0.869778,95.061295,-15.593652,15.893529
+0.186346,-0.001282,-0.970244,4.409925,2.807703,0.885037,95.061295,-15.593652,15.893529
+0.182379,-0.000427,-0.967620,4.287850,2.746666,0.900296,94.461540,-17.093040,14.993896
+0.185430,-0.001709,-0.978423,4.394665,2.716147,0.915555,94.461540,-17.093040,14.993896
+0.182562,-0.004212,-0.965300,4.318369,2.685629,0.915555,95.061295,-16.493284,15.293774
+0.183538,0.000855,-0.965484,4.379406,2.655110,0.854518,95.061295,-16.493284,15.293774
+0.182745,-0.003784,-0.962737,4.348887,2.822962,1.007111,95.061295,-16.493284,15.293774
+0.181036,-0.000244,-0.972137,4.318369,2.777184,0.900296,95.661057,-16.193407,14.694017
+0.185553,-0.003784,-0.970367,4.257332,2.777184,1.022370,95.661057,-16.193407,14.694017
+0.189398,0.000549,-0.970367,4.333628,2.777184,1.037629,95.511116,-17.242979,15.143834
+0.175848,-0.004456,-0.967376,4.333628,2.716147,0.885037,95.511116,-17.242979,15.143834
+0.188482,0.002564,-0.970183,4.348887,2.899258,1.129185,95.511116,-17.242979,15.143834
+0.178167,-0.007813,-0.961638,4.333628,2.685629,0.824000,96.410751,-16.343346,15.443712
+0.187567,-0.001770,-0.975433,4.242073,2.777184,1.098666,96.410751,-16.343346,15.443712
+0.179083,-0.001770,-0.957183,4.379406,2.792444,0.915555,95.661057,-16.193407,14.694017
+0.183660,-0.002808,-0.965545,4.348887,2.853481,0.900296,95.661057,-16.193407,14.694017
+0.179754,-0.004395,-0.957305,4.287850,2.822962,0.991852,95.661057,-16.193407,14.694017
+0.186956,-0.004273,-0.965484,4.348887,2.761925,0.824000,95.960930,-15.893529,14.694017
+0.178655,-0.001953,-0.976653,4.455702,2.731407,0.885037,95.960930,-15.893529,14.694017
+0.184149,-0.002380,-0.963042,4.318369,2.777184,1.022370,95.361176,-15.893529,15.593652
+0.183599,-0.004822,-0.974883,4.409925,2.700888,0.869778,95.361176,-15.893529,15.593652
+0.184698,0.003784,-0.963286,4.409925,2.822962,0.900296,95.361176,-15.893529,15.593652
+0.176580,-0.001465,-0.974944,4.440443,2.731407,0.824000,95.810989,-16.043468,15.143834
+0.192450,-0.004212,-0.956999,4.364147,2.853481,1.083407,95.810989,-16.043468,15.143834
+0.182501,-0.005310,-0.973601,4.394665,2.777184,0.747703,96.110870,-16.943102,15.143834
+0.187689,-0.001282,-0.966582,4.333628,2.700888,0.915555,96.110870,-16.943102,15.143834
+0.174383,0.001404,-0.969939,4.287850,2.639851,0.930815,96.110870,-16.943102,15.143834
+0.193915,-0.004212,-0.962249,4.364147,2.822962,0.961333,94.761421,-15.593652,15.293774
+0.175176,-0.005310,-0.967559,4.333628,2.822962,0.778222,94.761421,-15.593652,15.293774
+0.193426,-0.003418,-0.967315,4.257332,2.761925,1.007111,95.511116,-16.943102,14.544079
+0.175298,0.001770,-0.965850,4.333628,2.792444,0.839259,95.511116,-16.943102,14.544079
+0.188849,-0.003174,-0.967620,4.318369,2.670370,0.900296,95.511116,-17.242979,15.143834
+0.185125,-0.003479,-0.964019,4.257332,2.670370,0.946074,95.511116,-17.242979,15.143834
+0.183905,-0.002686,-0.963897,4.303110,2.777184,0.869778,95.511116,-17.242979,15.143834
+0.179205,-0.002136,-0.958525,4.226814,2.868740,0.915555,94.911362,-15.743590,16.043468
+0.187689,-0.005615,-0.963775,4.318369,2.792444,1.052889,94.911362,-15.743590,16.043468
+0.178533,-0.004395,-0.965789,4.287850,2.792444,0.824000,95.661057,-15.893529,14.394140
+0.182134,-0.001770,-0.960662,4.333628,2.716147,0.946074,95.661057,-15.893529,14.394140
+0.185736,0.000549,-0.976104,4.440443,2.822962,0.854518,95.661057,-15.893529,14.394140
+0.180792,-0.005615,-0.985992,4.486221,2.700888,0.808740,94.911362,-18.142614,15.143834
+0.186224,-0.000732,-0.971831,4.318369,2.822962,1.052889,94.911362,-18.142614,15.143834
+0.179266,-0.000244,-0.963897,4.242073,2.670370,0.869778,94.911362,-15.743590,15.443712
+0.189093,-0.004700,-0.952788,4.135258,2.700888,0.946074,94.911362,-15.743590,15.443712
+0.177435,-0.005310,-0.977325,4.425184,2.670370,0.946074,94.911362,-15.743590,15.443712
+0.191778,0.001343,-0.958892,4.425184,2.868740,0.839259,95.061295,-14.993896,14.993896
+0.175909,-0.002930,-0.984649,4.470962,2.639851,0.824000,95.061295,-14.993896,14.993896
+0.190130,-0.001038,-0.972808,4.318369,2.792444,1.083407,94.761421,-16.193407,15.293774
+0.178106,0.000366,-0.983245,4.348887,2.761925,0.747703,94.761421,-16.193407,15.293774
+0.181890,0.000427,-0.967559,4.379406,2.731407,0.961333,94.761421,-16.193407,15.293774
+0.183966,-0.003601,-0.966826,4.364147,2.777184,0.930815,96.260811,-16.793163,14.094262
+0.185553,-0.003052,-0.961150,4.287850,2.838221,0.976592,96.260811,-16.793163,14.094262
+0.178777,-0.001709,-0.970794,4.287850,2.716147,0.915555,95.960930,-16.193407,14.993896
+0.184881,-0.001465,-0.969939,4.394665,2.746666,0.930815,95.960930,-16.193407,14.993896
+0.179998,-0.001221,-0.965423,4.379406,2.792444,0.900296,95.960930,-16.193407,14.993896
+0.185736,-0.003784,-0.970061,4.348887,2.670370,0.946074,95.511116,-16.043468,14.244201
+0.182989,-0.001770,-0.966338,4.364147,2.716147,0.854518,95.511116,-16.043468,14.244201
+0.183905,-0.003967,-0.967925,4.333628,2.746666,0.915555,96.710625,-16.943102,14.843957
+0.185614,-0.001709,-0.969695,4.318369,2.746666,0.854518,96.710625,-16.943102,14.843957
+0.183599,-0.000488,-0.967681,4.348887,2.716147,0.869778,96.710625,-16.943102,14.843957
+0.184576,-0.003296,-0.967681,4.303110,2.777184,0.930815,94.911362,-15.743590,14.843957
+0.184942,-0.001709,-0.968230,4.303110,2.700888,0.839259,94.911362,-15.743590,14.843957
+0.181829,0.000061,-0.968047,4.333628,2.792444,0.915555,96.110870,-16.343346,15.443712
+0.182257,0.000671,-0.969207,4.287850,2.731407,0.839259,96.110870,-16.343346,15.443712
+0.187506,-0.001160,-0.965117,4.379406,2.868740,0.885037,96.110870,-16.343346,15.443712
+0.182989,-0.006165,-0.967315,4.379406,2.746666,1.007111,95.960930,-16.793163,16.793163
+0.183294,-0.000427,-0.968963,4.318369,2.761925,0.854518,95.960930,-16.793163,16.793163
+0.181829,-0.001953,-0.965606,4.364147,2.639851,0.869778,94.311600,-17.242979,14.843957
+0.183111,-0.000427,-0.967803,4.394665,2.792444,0.915555,94.311600,-17.242979,14.843957
+0.185736,-0.002869,-0.966033,4.333628,2.838221,0.839259,94.311600,-17.242979,14.843957
+0.185919,-0.002014,-0.969817,4.379406,2.716147,0.900296,95.361176,-17.392920,15.293774
+0.182806,-0.003113,-0.965850,4.303110,2.868740,0.854518,95.361176,-17.392920,15.293774
+0.182501,-0.001038,-0.965667,4.181036,2.746666,0.885037,95.361176,-17.392920,15.293774
+0.182012,-0.001892,-0.966155,4.333628,2.822962,0.900296,95.361176,-17.392920,15.293774
+0.182684,-0.003174,-0.967376,4.287850,2.700888,0.961333,95.361176,-17.392920,15.293774
+0.184881,-0.005554,-0.966643,4.272591,2.639851,0.991852,95.211235,-16.643225,15.143834
+0.182867,0.000366,-0.968474,4.287850,2.807703,0.824000,95.211235,-16.643225,15.143834
+0.181280,0.001038,-0.968841,4.303110,2.838221,0.885037,94.911362,-16.043468,15.443712
+0.184698,-0.001709,-0.966826,4.272591,2.777184,0.900296,94.911362,-16.043468,15.443712
+0.184271,-0.005615,-0.966765,4.318369,2.761925,0.930815,94.911362,-16.043468,15.443712
+0.181280,-0.000610,-0.965117,4.455702,2.822962,0.930815,93.861786,-15.893529,15.293774
+0.181768,-0.001160,-0.964995,4.394665,2.777184,0.930815,93.861786,-15.893529,15.293774
+0.185430,-0.003052,-0.967009,4.318369,2.716147,0.961333,95.960930,-17.093040,14.694017
+0.182012,-0.005676,-0.970367,4.394665,2.716147,0.824000,95.960930,-17.093040,14.694017
+0.182562,0.000916,-0.971526,4.303110,2.746666,0.930815,95.960930,-17.093040,14.694017
+0.184393,-0.001770,-0.968291,4.272591,2.655110,0.824000,95.211235,-16.343346,14.843957
+0.183966,-0.002991,-0.966216,4.364147,2.822962,0.930815,95.211235,-16.343346,14.843957
+0.184271,-0.000793,-0.963652,4.318369,2.777184,0.930815,95.661057,-16.793163,14.694017
+0.184942,0.000488,-0.971709,4.257332,2.792444,0.961333,95.661057,-16.793163,14.694017
+0.180120,-0.002808,-0.968169,4.303110,2.685629,0.900296,95.661057,-16.793163,14.694017
+0.185675,-0.002808,-0.967498,4.272591,2.777184,0.961333,95.211235,-16.643225,15.143834
+0.183538,-0.006165,-0.965117,4.333628,2.685629,0.915555,95.211235,-16.643225,15.143834
+0.182012,0.001648,-0.963347,4.333628,2.777184,0.900296,95.211235,-17.542858,15.443712
+0.179327,-0.001831,-0.969024,4.379406,2.731407,0.854518,95.211235,-17.542858,15.443712
+0.187078,-0.001587,-0.970916,4.333628,2.731407,0.824000,95.511116,-16.043468,14.544079
+0.180548,-0.002808,-0.970367,4.364147,2.700888,0.839259,95.511116,-16.043468,14.544079
+0.185003,-0.002380,-0.966338,4.303110,2.761925,1.007111,95.511116,-16.043468,14.544079
+0.179022,-0.002991,-0.963469,4.379406,2.777184,0.839259,95.211235,-16.943102,15.743590
+0.184271,-0.001160,-0.967620,4.287850,2.792444,0.900296,95.211235,-16.943102,15.743590
+0.182684,-0.004639,-0.967132,4.318369,2.792444,0.961333,94.911362,-17.542858,15.743590
+0.187811,-0.000732,-0.964995,4.303110,2.670370,0.915555,94.911362,-17.542858,15.743590
+0.181097,-0.000061,-0.967559,4.272591,2.777184,0.885037,94.911362,-17.542858,15.743590
+0.185308,-0.001831,-0.966460,4.333628,2.761925,0.869778,95.211235,-16.343346,16.043468
+0.184088,-0.004212,-0.966704,4.272591,2.746666,0.854518,95.211235,-16.343346,16.043468
+0.181585,-0.000610,-0.969085,4.318369,2.838221,0.915555,95.810989,-16.343346,15.443712
+0.184027,-0.000183,-0.968291,4.318369,2.685629,0.900296,95.810989,-16.343346,15.443712
+0.180853,-0.003418,-0.963591,4.242073,2.777184,0.885037,95.810989,-16.343346,15.443712
+0.182867,-0.002686,-0.972381,4.394665,2.868740,0.946074,94.461540,-16.493284,15.593652
+0.181463,-0.006836,-0.965728,4.364147,2.746666,0.869778,94.461540,-16.493284,15.593652
+0.186834,-0.001648,-0.976836,4.364147,2.670370,0.808740,95.661057,-16.193407,15.893529
+0.177923,-0.000977,-0.966887,4.348887,2.731407,0.961333,95.661057,-16.193407,15.893529
+0.187201,0.000427,-0.974395,4.226814,2.716147,0.961333,95.661057,-16.193407,15.893529
+0.183721,-0.007019,-0.968719,4.409925,2.700888,0.839259,95.211235,-16.043468,16.643225
+0.183233,-0.000427,-0.967376,4.333628,2.792444,0.915555,95.211235,-16.043468,16.643225
+0.177618,-0.001526,-0.970794,4.364147,2.670370,0.961333,95.361176,-15.893529,13.794384
+0.183538,-0.001343,-0.961333,4.287850,2.746666,0.961333,95.361176,-15.893529,13.794384
+0.184515,-0.005982,-0.973235,4.318369,2.746666,0.946074,95.361176,-15.893529,13.794384
+0.183416,-0.000366,-0.960540,4.348887,2.761925,0.854518,95.661057,-15.893529,15.893529
+0.182379,-0.004761,-0.977325,4.318369,2.670370,0.900296,95.661057,-15.893529,15.893529
+0.187628,-0.000427,-0.964934,4.303110,2.746666,0.915555,95.511116,-18.442492,14.544079
+0.177313,-0.003723,-0.973662,4.287850,2.716147,0.869778,95.511116,-18.442492,14.544079
+0.189032,-0.001221,-0.969451,4.348887,2.807703,0.946074,95.511116,-18.442492,14.544079
+0.175970,-0.003418,-0.979766,4.455702,2.731407,0.732444,95.661057,-16.193407,14.394140
+0.190741,0.002197,-0.972320,4.257332,2.731407,1.022370,95.661057,-16.193407,14.394140
+0.172613,-0.001526,-0.962859,4.409925,2.670370,0.747703,94.761421,-15.893529,15.593652
+0.198614,-0.004944,-0.973052,4.272591,2.761925,1.159703,94.761421,-15.893529,15.593652
+0.173284,-0.000488,-0.961882,4.303110,2.777184,0.778222,94.761421,-15.893529,15.593652

ugv3_rest.csv

@@ -0,0 +1,654 @@
+AX,AY,AZ,GX,GY,GZ,MX,MY,MZ
+0.172735,0.012268,0.971587,4.348887,-2.838221,0.854518,33.286449,-164.632980,-23.390476
+0.174261,0.013306,0.972320,4.409925,-2.822962,0.900296,32.386814,-163.733337,-22.190966
+0.174871,0.013428,0.973601,4.318369,-2.853481,0.915555,32.386814,-163.733337,-22.190966
+0.172857,0.014405,0.972747,4.425184,-2.868740,0.869778,32.386814,-163.733337,-22.190966
+0.173528,0.012452,0.973724,4.348887,-2.777184,0.961333,32.536755,-165.382675,-22.940660
+0.175604,0.014161,0.973052,4.394665,-2.868740,0.839259,32.536755,-165.382675,-22.940660
+0.174688,0.013001,0.970794,4.394665,-2.914518,0.885037,34.485958,-164.932846,-23.090599
+0.174139,0.012085,0.971099,4.425184,-2.792444,0.869778,34.485958,-164.932846,-23.090599
+0.172124,0.012696,0.972015,4.333628,-2.838221,0.885037,33.736263,-164.782913,-21.741148
+0.173284,0.015015,0.972259,4.394665,-2.853481,0.885037,33.736263,-164.782913,-21.741148
+0.173711,0.012879,0.971770,4.379406,-2.853481,0.885037,33.736263,-164.782913,-21.741148
+0.174139,0.012757,0.974822,4.379406,-2.838221,0.900296,33.736263,-164.782913,-21.741148
+0.173772,0.012024,0.976897,4.333628,-2.777184,0.961333,33.736263,-164.782913,-21.741148
+0.174566,0.012879,0.974883,4.333628,-2.807703,0.762963,31.936998,-164.183151,-22.340904
+0.173467,0.014405,0.972686,4.348887,-2.883999,0.900296,31.936998,-164.183151,-22.340904
+0.174139,0.013977,0.975433,4.364147,-2.868740,0.900296,31.936998,-164.183151,-22.340904
+0.175115,0.015870,0.971770,4.379406,-2.822962,0.869778,33.286449,-164.033218,-23.690355
+0.172857,0.013184,0.975127,4.348887,-2.838221,0.885037,33.286449,-164.033218,-23.690355
+0.172002,0.012940,0.973724,4.409925,-2.838221,0.930815,33.586327,-164.932846,-23.090599
+0.174566,0.013123,0.976043,4.364147,-2.777184,0.946074,33.586327,-164.932846,-23.090599
+0.175665,0.012329,0.972625,4.318369,-2.761925,0.869778,33.586327,-164.932846,-23.090599
+0.174749,0.013794,0.972503,4.409925,-2.914518,0.885037,34.485958,-164.632980,-21.891088
+0.174017,0.011902,0.972259,4.394665,-2.853481,0.915555,34.485958,-164.632980,-21.891088
+0.172308,0.012818,0.969024,4.409925,-2.899258,0.961333,33.286449,-164.932846,-22.790722
+0.174993,0.013855,0.973357,4.440443,-2.838221,0.946074,33.286449,-164.932846,-22.790722
+0.175054,0.012024,0.972320,4.333628,-2.914518,0.946074,33.286449,-164.932846,-22.790722
+0.174688,0.012452,0.972625,4.333628,-2.838221,0.930815,32.686691,-164.932846,-23.090599
+0.173284,0.014710,0.973601,4.348887,-2.792444,0.900296,32.686691,-164.932846,-23.090599
+0.172491,0.015992,0.971404,4.440443,-2.899258,0.793481,32.386814,-164.033218,-23.690355
+0.175237,0.013306,0.974334,4.379406,-2.868740,0.915555,32.386814,-164.033218,-23.690355
+0.173650,0.012207,0.977447,4.425184,-2.868740,0.946074,32.386814,-164.033218,-23.690355
+0.173589,0.013123,0.976043,4.364147,-2.761925,0.915555,32.686691,-163.433456,-22.790722
+0.172735,0.012452,0.971954,4.425184,-2.853481,0.869778,32.686691,-163.433456,-22.790722
+0.174627,0.015137,0.972320,4.333628,-2.883999,0.854518,32.236874,-163.883286,-23.540417
+0.176153,0.014710,0.973113,4.440443,-2.838221,0.885037,32.236874,-163.883286,-23.540417
+0.174993,0.012879,0.973724,4.379406,-2.792444,0.915555,32.236874,-163.883286,-23.540417
+0.176214,0.014161,0.970794,4.272591,-2.807703,0.900296,33.286449,-165.232727,-22.790722
+0.174566,0.011902,0.970489,4.348887,-2.761925,0.930815,33.286449,-165.232727,-22.790722
+0.175237,0.011963,0.970428,4.318369,-2.761925,0.900296,32.686691,-164.033218,-23.090599
+0.173589,0.012757,0.971587,4.364147,-2.777184,0.869778,32.686691,-164.033218,-23.090599
+0.173772,0.011597,0.972930,4.318369,-2.807703,0.885037,32.686691,-164.033218,-23.090599
+0.175054,0.014954,0.975982,4.364147,-2.807703,0.854518,32.836632,-164.483032,-23.840294
+0.173406,0.011902,0.973235,4.394665,-2.838221,0.915555,32.836632,-164.483032,-23.840294
+0.173711,0.014161,0.970244,4.394665,-2.822962,0.885037,34.785839,-165.532608,-23.690355
+0.174627,0.013794,0.972137,4.364147,-2.868740,0.900296,34.785839,-165.532608,-23.690355
+0.173772,0.014649,0.973052,4.440443,-2.853481,0.946074,34.785839,-165.532608,-23.690355
+0.172552,0.015076,0.975738,4.394665,-2.853481,0.900296,32.086937,-165.232727,-22.790722
+0.173345,0.015198,0.971831,4.394665,-2.792444,0.930815,32.086937,-165.232727,-22.790722
+0.174017,0.013428,0.972015,4.348887,-2.853481,0.915555,33.586327,-164.632980,-23.690355
+0.174810,0.012635,0.972686,4.333628,-2.822962,0.869778,33.586327,-164.632980,-23.690355
+0.174444,0.014039,0.974883,4.379406,-2.899258,0.885037,34.186081,-164.333099,-23.090599
+0.174993,0.012696,0.971831,4.409925,-2.838221,0.808740,34.186081,-164.333099,-23.090599
+0.175542,0.012329,0.971648,4.425184,-2.792444,0.808740,34.186081,-164.333099,-23.090599
+0.174383,0.014100,0.971770,4.394665,-2.853481,0.854518,33.436386,-164.483032,-22.340904
+0.174078,0.015198,0.972137,4.425184,-2.822962,0.900296,33.436386,-164.483032,-22.340904
+0.173528,0.013001,0.970367,4.425184,-2.868740,0.885037,32.536755,-165.382675,-22.640781
+0.173589,0.011292,0.978362,4.379406,-2.761925,0.885037,32.536755,-165.382675,-22.640781
+0.173956,0.012940,0.975188,4.394665,-2.883999,0.854518,32.536755,-165.382675,-22.640781
+0.172918,0.011597,0.973540,4.348887,-2.761925,0.915555,33.136509,-165.082794,-23.240538
+0.173406,0.013062,0.975005,4.364147,-2.838221,0.839259,33.136509,-165.082794,-23.240538
+0.172369,0.014161,0.975677,4.364147,-2.822962,0.854518,32.686691,-164.033218,-23.990232
+0.176397,0.012452,0.970489,4.364147,-2.899258,0.900296,32.686691,-164.033218,-23.990232
+0.175176,0.012452,0.970672,4.394665,-2.777184,0.839259,32.686691,-164.033218,-23.990232
+0.174383,0.013245,0.969573,4.379406,-2.822962,0.839259,33.736263,-164.782913,-23.540417
+0.174322,0.014832,0.973357,4.379406,-2.838221,0.900296,33.736263,-164.782913,-23.540417
+0.173528,0.014344,0.968474,4.364147,-2.777184,0.839259,32.686691,-163.433456,-21.891088
+0.174444,0.011353,0.972991,4.440443,-2.883999,1.007111,32.686691,-163.433456,-21.891088
+0.173528,0.011597,0.973357,4.318369,-2.838221,0.854518,32.686691,-163.433456,-21.891088
+0.173406,0.013184,0.976836,4.409925,-2.914518,0.885037,33.436386,-164.183151,-22.640781
+0.174444,0.011536,0.972015,4.409925,-2.914518,0.869778,33.436386,-164.183151,-22.640781
+0.175420,0.014893,0.970672,4.455702,-3.006073,0.915555,34.336021,-165.082794,-23.240538
+0.172735,0.012452,0.974517,4.364147,-2.899258,0.961333,34.336021,-165.082794,-23.240538
+0.173772,0.013367,0.971282,4.394665,-2.883999,0.915555,32.686691,-165.232727,-23.390476
+0.173894,0.014466,0.972625,4.440443,-2.838221,0.839259,32.686691,-165.232727,-23.390476
+0.173894,0.012940,0.970855,4.425184,-2.807703,0.869778,32.686691,-165.232727,-23.390476
+0.176702,0.016114,0.971831,4.348887,-2.929777,0.976592,33.436386,-165.382675,-22.640781
+0.174200,0.013245,0.970916,4.409925,-2.807703,0.946074,33.436386,-165.382675,-22.640781
+0.173406,0.014161,0.974822,4.364147,-2.883999,0.900296,33.286449,-163.733337,-23.090599
+0.173956,0.010743,0.973235,4.348887,-2.807703,0.930815,33.286449,-163.733337,-23.090599
+0.174383,0.013916,0.975555,4.394665,-2.792444,0.885037,33.286449,-163.733337,-23.090599
+0.173101,0.013550,0.973235,4.394665,-2.822962,0.869778,32.686691,-164.033218,-23.990232
+0.173406,0.014466,0.971099,4.425184,-2.853481,0.885037,32.686691,-164.033218,-23.990232
+0.173223,0.011780,0.968352,4.394665,-2.838221,0.854518,32.836632,-165.382675,-23.540417
+0.173589,0.014344,0.969817,4.348887,-2.822962,0.915555,32.836632,-165.382675,-23.540417
+0.173284,0.012696,0.973785,4.364147,-2.746666,0.961333,32.836632,-165.382675,-23.540417
+0.173345,0.012818,0.969451,4.425184,-2.838221,0.869778,32.686691,-163.733337,-23.690355
+0.175604,0.013733,0.973846,4.333628,-2.883999,0.885037,32.686691,-163.733337,-23.690355
+0.173894,0.013611,0.975372,4.303110,-2.822962,0.808740,32.536755,-163.883286,-22.340904
+0.174261,0.013123,0.974761,4.425184,-2.899258,0.915555,32.536755,-163.883286,-22.340904
+0.173101,0.012635,0.971954,4.440443,-2.929777,0.854518,32.536755,-163.883286,-22.340904
+0.173956,0.011109,0.972076,4.364147,-2.838221,0.869778,32.986568,-164.632980,-22.790722
+0.174017,0.012879,0.973357,4.440443,-2.929777,0.869778,32.986568,-164.632980,-22.790722
+0.175848,0.014039,0.973479,4.425184,-2.868740,0.869778,32.536755,-163.583405,-22.940660
+0.172002,0.013367,0.975127,4.379406,-2.822962,0.915555,32.536755,-163.583405,-22.940660
+0.171819,0.012574,0.974029,4.440443,-2.883999,0.808740,32.536755,-163.583405,-22.940660
+0.173528,0.011963,0.970855,4.379406,-2.761925,0.839259,32.836632,-165.682541,-23.240538
+0.175359,0.012207,0.972747,4.379406,-2.792444,0.900296,32.836632,-165.682541,-23.240538
+0.172552,0.014100,0.973052,4.379406,-2.853481,0.869778,31.787058,-165.532608,-23.390476
+0.174566,0.012696,0.970611,4.394665,-2.868740,0.869778,31.787058,-165.532608,-23.390476
+0.172979,0.012513,0.974578,4.440443,-2.838221,0.946074,31.787058,-165.532608,-23.390476
+0.173528,0.012329,0.971892,4.409925,-2.899258,0.915555,32.986568,-163.433456,-22.790722
+0.173956,0.013367,0.975311,4.425184,-2.883999,0.885037,32.986568,-163.433456,-22.790722
+0.175176,0.011597,0.973113,4.379406,-2.792444,0.793481,32.986568,-164.932846,-23.990232
+0.172674,0.013611,0.973601,4.348887,-2.777184,0.869778,32.986568,-164.932846,-23.990232
+0.172552,0.013550,0.970733,4.333628,-2.868740,0.915555,32.986568,-164.932846,-23.990232
+0.173284,0.012513,0.970428,4.425184,-2.838221,0.961333,32.986568,-164.932846,-23.090599
+0.174688,0.012696,0.973113,4.516739,-2.853481,0.808740,32.986568,-164.932846,-23.090599
+0.173833,0.011658,0.972320,4.394665,-2.899258,0.885037,34.336021,-165.982422,-22.940660
+0.173101,0.012757,0.972686,4.440443,-2.853481,0.930815,34.336021,-165.982422,-22.940660
+0.173223,0.014161,0.969573,4.409925,-2.822962,0.885037,34.336021,-165.982422,-22.940660
+0.173833,0.012940,0.972076,4.364147,-2.822962,0.961333,32.386814,-163.733337,-22.790722
+0.175237,0.012635,0.975311,4.379406,-2.746666,0.854518,32.386814,-163.733337,-22.790722
+0.172308,0.014649,0.970977,4.379406,-2.792444,0.824000,32.386814,-164.632980,-23.390476
+0.175787,0.014588,0.977508,4.425184,-2.838221,0.930815,32.386814,-164.632980,-23.390476
+0.174017,0.013184,0.975982,4.364147,-2.853481,0.869778,32.236874,-164.483032,-23.540417
+0.173772,0.013367,0.974151,4.425184,-2.838221,0.854518,32.236874,-164.483032,-23.540417
+0.175542,0.014283,0.971343,4.470962,-2.807703,0.915555,32.236874,-164.483032,-23.540417
+0.176031,0.015015,0.972686,4.394665,-2.838221,0.900296,33.136509,-164.782913,-22.940660
+0.175359,0.013855,0.971343,4.409925,-2.899258,0.839259,33.136509,-164.782913,-22.940660
+0.175787,0.014649,0.972198,4.379406,-2.899258,0.930815,32.686691,-163.433456,-21.591209
+0.175115,0.012818,0.972625,4.409925,-2.822962,0.915555,32.686691,-163.433456,-21.591209
+0.173589,0.013794,0.971954,4.348887,-2.838221,0.900296,32.686691,-163.433456,-21.591209
+0.174139,0.012940,0.973235,4.470962,-2.822962,0.900296,32.386814,-164.932846,-22.790722
+0.175359,0.013672,0.975066,4.348887,-2.868740,0.900296,32.386814,-164.932846,-22.790722
+0.172796,0.013245,0.970672,4.394665,-2.853481,0.839259,32.536755,-165.382675,-22.340904
+0.174200,0.014405,0.972869,4.409925,-2.883999,0.869778,32.536755,-165.382675,-22.340904
+0.174078,0.012635,0.973846,4.348887,-2.822962,0.869778,32.536755,-165.382675,-22.340904
+0.173894,0.013611,0.975249,4.531999,-2.807703,0.854518,33.286449,-164.632980,-24.589989
+0.172246,0.012696,0.973174,4.348887,-2.822962,0.808740,33.286449,-164.632980,-24.589989
+0.175604,0.015931,0.969817,4.318369,-2.761925,0.915555,33.136509,-164.183151,-24.140171
+0.173956,0.011780,0.973724,4.394665,-2.853481,0.961333,33.136509,-164.183151,-24.140171
+0.175542,0.014710,0.973418,4.379406,-2.822962,0.915555,33.136509,-164.183151,-24.140171
+0.173101,0.013855,0.971282,4.333628,-2.792444,0.900296,32.386814,-164.932846,-23.090599
+0.174017,0.014527,0.972259,4.409925,-2.868740,0.824000,32.386814,-164.932846,-23.090599
+0.173772,0.013611,0.971404,4.394665,-2.822962,0.854518,33.136509,-164.483032,-23.240538
+0.174078,0.015503,0.972869,4.333628,-2.822962,0.930815,33.136509,-164.483032,-23.240538
+0.173833,0.014100,0.969756,4.425184,-2.838221,0.915555,33.136509,-164.483032,-23.240538
+0.173772,0.013367,0.973174,4.409925,-2.731407,0.900296,32.986568,-164.333099,-23.690355
+0.173101,0.014832,0.973662,4.348887,-2.914518,0.885037,32.986568,-164.333099,-23.690355
+0.172552,0.013916,0.972564,4.379406,-2.822962,0.854518,33.586327,-164.632980,-23.090599
+0.174566,0.012085,0.975677,4.394665,-2.914518,0.869778,33.586327,-164.632980,-23.090599
+0.175420,0.014710,0.973418,4.440443,-2.838221,0.915555,33.136509,-165.082794,-23.540417
+0.175237,0.014161,0.973052,4.470962,-2.883999,0.839259,33.136509,-165.082794,-23.540417
+0.175298,0.012574,0.971892,4.409925,-2.868740,0.869778,33.136509,-165.082794,-23.540417
+0.173162,0.015259,0.972076,4.364147,-2.990814,0.839259,33.136509,-165.082794,-24.140171
+0.175604,0.013794,0.975799,4.409925,-2.929777,0.900296,33.136509,-165.082794,-24.140171
+0.174993,0.013367,0.973968,4.409925,-2.868740,0.854518,32.836632,-164.483032,-24.140171
+0.174444,0.012818,0.974761,4.364147,-2.792444,0.900296,32.836632,-164.483032,-24.140171
+0.174200,0.012391,0.972686,4.333628,-2.899258,0.900296,32.836632,-164.483032,-24.140171
+0.172735,0.012696,0.969329,4.440443,-2.868740,0.930815,32.536755,-165.382675,-23.540417
+0.175176,0.013489,0.973785,4.425184,-2.899258,0.900296,32.536755,-165.382675,-23.540417
+0.174017,0.012696,0.970244,4.394665,-2.883999,0.900296,33.436386,-164.782913,-23.240538
+0.173833,0.014100,0.972442,4.348887,-2.777184,0.839259,33.436386,-164.782913,-23.240538
+0.174688,0.013428,0.976104,4.348887,-2.792444,0.930815,33.436386,-164.782913,-23.240538
+0.173711,0.012452,0.970977,4.440443,-2.899258,0.869778,33.286449,-165.232727,-23.990232
+0.175237,0.011963,0.973235,4.394665,-2.822962,0.869778,33.286449,-165.232727,-23.990232
+0.175237,0.011841,0.969695,4.379406,-2.883999,0.885037,33.136509,-164.183151,-22.640781
+0.174322,0.013123,0.972808,4.364147,-2.822962,0.854518,33.136509,-164.183151,-22.640781
+0.173650,0.016358,0.971404,4.379406,-2.822962,0.961333,33.136509,-164.183151,-22.640781
+0.174383,0.014222,0.972869,4.364147,-2.883999,0.793481,33.436386,-165.082794,-22.340904
+0.177190,0.014832,0.974761,4.455702,-2.822962,0.961333,33.436386,-165.082794,-22.340904
+0.174749,0.013367,0.973846,4.333628,-2.792444,0.869778,34.036144,-163.883286,-23.240538
+0.174932,0.013245,0.972076,4.318369,-2.899258,0.961333,34.036144,-163.883286,-23.240538
+0.174932,0.014100,0.972625,4.409925,-2.822962,0.915555,34.036144,-163.883286,-23.240538
+0.174383,0.014405,0.973479,4.394665,-2.883999,0.915555,33.886204,-164.632980,-23.390476
+0.174688,0.013062,0.972198,4.364147,-2.822962,0.900296,33.886204,-164.632980,-23.390476
+0.174749,0.013550,0.971587,4.348887,-2.822962,0.961333,33.436386,-165.082794,-23.840294
+0.174261,0.014649,0.970672,4.394665,-2.883999,0.900296,33.436386,-165.082794,-23.840294
+0.174322,0.014466,0.969268,4.379406,-2.883999,0.869778,33.436386,-165.082794,-23.840294
+0.174627,0.012818,0.971709,4.287850,-2.822962,0.869778,33.586327,-164.632980,-23.990232
+0.175054,0.012818,0.970916,4.364147,-2.914518,0.961333,33.586327,-164.632980,-23.990232
+0.175054,0.012024,0.970916,4.394665,-2.853481,0.946074,34.336021,-164.483032,-22.041027
+0.175909,0.011902,0.975249,4.455702,-2.838221,0.854518,34.336021,-164.483032,-22.041027
+0.174078,0.014283,0.972259,4.348887,-2.792444,0.808740,34.336021,-164.483032,-22.041027
+0.175115,0.013428,0.970183,4.394665,-2.868740,0.824000,34.036144,-165.382675,-23.840294
+0.174871,0.014039,0.970672,4.379406,-2.883999,0.930815,34.036144,-165.382675,-23.840294
+0.174871,0.012635,0.967742,4.379406,-2.945036,0.961333,32.986568,-164.932846,-22.790722
+0.176336,0.013489,0.973540,4.440443,-2.868740,0.976592,32.986568,-164.932846,-22.790722
+0.173650,0.012329,0.973724,4.501480,-2.868740,0.946074,32.386814,-164.632980,-23.390476
+0.173650,0.014283,0.972686,4.455702,-2.883999,0.930815,32.386814,-164.632980,-23.390476
+0.175054,0.014283,0.974944,4.394665,-3.006073,0.946074,32.386814,-164.632980,-23.390476
+0.172246,0.015503,0.970916,4.379406,-2.853481,0.900296,33.736263,-164.782913,-22.640781
+0.174200,0.011963,0.972869,4.409925,-2.838221,0.824000,33.736263,-164.782913,-22.640781
+0.173894,0.011231,0.973662,4.348887,-2.807703,0.946074,33.136509,-164.782913,-22.340904
+0.174444,0.012452,0.974944,4.425184,-2.822962,0.961333,33.136509,-164.782913,-22.340904
+0.172491,0.012329,0.977508,4.455702,-2.822962,0.946074,33.136509,-164.782913,-22.340904
+0.173711,0.012940,0.971770,4.440443,-2.792444,0.900296,33.736263,-164.782913,-22.940660
+0.173833,0.013184,0.971282,4.425184,-2.899258,0.946074,33.736263,-164.782913,-22.940660
+0.172491,0.012024,0.972625,4.364147,-2.868740,0.961333,34.186081,-164.632980,-22.490843
+0.174871,0.014405,0.974151,4.348887,-2.777184,0.900296,34.186081,-164.632980,-22.490843
+0.172857,0.012879,0.973724,4.379406,-2.853481,0.869778,34.186081,-164.632980,-22.490843
+0.174017,0.015076,0.970183,4.394665,-2.883999,0.885037,32.686691,-164.932846,-22.490843
+0.173589,0.013001,0.972198,4.440443,-2.929777,0.885037,32.686691,-164.932846,-22.490843
+0.174505,0.015687,0.969573,4.440443,-2.883999,0.900296,33.436386,-166.282303,-22.640781
+0.173284,0.011536,0.972564,4.379406,-2.822962,0.885037,33.436386,-166.282303,-22.640781
+0.170904,0.015015,0.976226,4.333628,-2.853481,0.839259,33.436386,-166.282303,-22.640781
+0.174261,0.012207,0.969085,4.455702,-2.929777,0.885037,33.586327,-164.033218,-23.090599
+0.175665,0.013916,0.972015,4.379406,-2.883999,0.854518,33.586327,-164.033218,-23.090599
+0.174322,0.014161,0.967742,4.333628,-2.777184,0.854518,31.787058,-164.333099,-23.990232
+0.173162,0.014893,0.971099,4.455702,-2.807703,0.808740,31.787058,-164.333099,-23.990232
+0.174566,0.013672,0.973113,4.394665,-2.853481,0.885037,31.787058,-164.333099,-23.990232
+0.173467,0.015259,0.973296,4.379406,-2.822962,0.869778,32.836632,-164.782913,-22.640781
+0.172246,0.014771,0.969939,4.409925,-2.807703,0.839259,32.836632,-164.782913,-22.640781
+0.175054,0.012940,0.972076,4.409925,-2.838221,0.900296,32.086937,-163.433456,-23.090599
+0.173040,0.014527,0.969512,4.440443,-2.853481,0.869778,32.086937,-163.433456,-23.090599
+0.173345,0.013550,0.974334,4.455702,-2.914518,0.915555,32.086937,-163.433456,-23.090599
+0.172857,0.013123,0.969573,4.425184,-2.945036,0.885037,33.586327,-163.433456,-22.790722
+0.174383,0.015015,0.973418,4.501480,-2.914518,0.946074,33.586327,-163.433456,-22.790722
+0.173833,0.014039,0.969329,4.379406,-2.822962,0.885037,31.936998,-165.082794,-23.540417
+0.174444,0.014161,0.975311,4.364147,-2.883999,0.900296,31.936998,-165.082794,-23.540417
+0.174505,0.013672,0.973296,4.425184,-2.960295,0.900296,31.936998,-165.082794,-23.540417
+0.173467,0.013611,0.972015,4.379406,-2.853481,0.854518,32.536755,-165.082794,-24.140171
+0.172308,0.011780,0.972381,4.364147,-2.822962,0.885037,32.536755,-165.082794,-24.140171
+0.174261,0.011475,0.973235,4.409925,-2.868740,0.900296,33.436386,-165.082794,-23.240538
+0.174322,0.012879,0.972442,4.394665,-2.822962,0.885037,33.436386,-165.082794,-23.240538
+0.174627,0.014039,0.974639,4.287850,-2.807703,0.839259,33.436386,-165.082794,-23.240538
+0.174078,0.012329,0.972076,4.394665,-2.822962,0.915555,33.436386,-165.082794,-23.240538
+0.175115,0.013794,0.972259,4.348887,-2.868740,0.839259,33.436386,-165.082794,-23.240538
+0.174932,0.012085,0.972808,4.409925,-2.838221,0.930815,33.586327,-164.632980,-23.690355
+0.172918,0.013672,0.975616,4.379406,-2.777184,0.961333,33.586327,-164.632980,-23.690355
+0.174139,0.011780,0.973174,4.348887,-2.822962,0.900296,32.836632,-164.782913,-22.640781
+0.172674,0.011658,0.975555,4.455702,-2.853481,0.869778,32.836632,-164.782913,-22.640781
+0.174688,0.013123,0.973968,4.303110,-2.883999,0.854518,32.836632,-164.782913,-22.640781
+0.173223,0.013794,0.973907,4.364147,-2.883999,0.900296,34.186081,-163.733337,-22.790722
+0.175604,0.013794,0.975127,4.379406,-2.822962,0.854518,34.186081,-163.733337,-22.790722
+0.172124,0.013306,0.971831,4.348887,-2.792444,0.854518,34.036144,-164.183151,-22.940660
+0.174566,0.013489,0.976165,4.379406,-2.838221,0.946074,34.036144,-164.183151,-22.940660
+0.173040,0.012757,0.974090,4.364147,-2.914518,0.869778,34.036144,-164.183151,-22.940660
+0.173162,0.013489,0.973418,4.440443,-2.822962,0.839259,33.286449,-165.532608,-23.390476
+0.172613,0.013184,0.970061,4.409925,-2.853481,0.869778,33.286449,-165.532608,-23.390476
+0.175298,0.013306,0.968841,4.333628,-2.853481,0.915555,31.787058,-164.632980,-22.790722
+0.174322,0.013123,0.973662,4.394665,-2.822962,0.869778,31.787058,-164.632980,-22.790722
+0.173894,0.013794,0.974578,4.394665,-2.899258,0.900296,31.787058,-164.632980,-22.790722
+0.173956,0.014222,0.974029,4.348887,-2.853481,0.915555,33.436386,-165.382675,-23.840294
+0.172430,0.012635,0.973540,4.348887,-2.899258,0.930815,33.436386,-165.382675,-23.840294
+0.176153,0.013611,0.971587,4.364147,-2.746666,0.824000,33.586327,-164.033218,-22.790722
+0.174017,0.013916,0.971343,4.379406,-2.883999,0.854518,33.586327,-164.033218,-22.790722
+0.174261,0.013062,0.974212,4.409925,-2.929777,0.900296,33.436386,-165.082794,-22.640781
+0.173650,0.012696,0.969390,4.379406,-2.868740,0.839259,33.436386,-165.082794,-22.640781
+0.174200,0.014405,0.973113,4.394665,-2.746666,0.869778,33.436386,-165.082794,-22.640781
+0.174139,0.013916,0.971465,4.379406,-2.853481,0.900296,32.536755,-165.082794,-23.540417
+0.173650,0.012452,0.973662,4.348887,-2.792444,0.961333,32.536755,-165.082794,-23.540417
+0.176214,0.012085,0.975433,4.379406,-2.868740,0.930815,33.886204,-163.733337,-23.690355
+0.173772,0.012940,0.973479,4.348887,-2.914518,0.839259,33.886204,-163.733337,-23.690355
+0.174810,0.013855,0.975738,4.318369,-2.807703,0.915555,33.886204,-163.733337,-23.690355
+0.174749,0.014161,0.975311,4.409925,-2.807703,0.869778,33.436386,-164.782913,-22.940660
+0.172796,0.012268,0.972442,4.394665,-2.883999,0.869778,33.436386,-164.782913,-22.940660
+0.175665,0.011719,0.972808,4.455702,-2.807703,0.900296,32.536755,-164.183151,-22.940660
+0.173406,0.013733,0.969390,4.409925,-2.899258,0.839259,32.536755,-164.183151,-22.940660
+0.175420,0.013123,0.968841,4.379406,-2.838221,0.930815,32.536755,-164.183151,-22.940660
+0.175787,0.011597,0.970000,4.425184,-2.914518,0.854518,33.136509,-165.082794,-24.440050
+0.173345,0.014710,0.972808,4.379406,-2.822962,0.885037,33.136509,-165.082794,-24.440050
+0.175420,0.012574,0.973785,4.379406,-2.914518,0.885037,31.936998,-165.382675,-23.540417
+0.174627,0.012696,0.973296,4.425184,-2.899258,0.869778,31.936998,-165.382675,-23.540417
+0.175115,0.014222,0.975677,4.348887,-2.853481,0.854518,31.936998,-165.382675,-23.540417
+0.174017,0.014344,0.968230,4.364147,-2.853481,0.930815,33.736263,-165.082794,-23.540417
+0.173894,0.013794,0.973479,4.348887,-2.883999,0.808740,33.736263,-165.082794,-23.540417
+0.175054,0.011719,0.971770,4.440443,-2.883999,0.900296,32.836632,-164.483032,-23.240538
+0.174627,0.012818,0.974029,4.333628,-2.822962,0.869778,32.836632,-164.483032,-23.240538
+0.173894,0.014161,0.973846,4.333628,-2.853481,0.869778,32.836632,-164.483032,-23.240538
+0.174993,0.011719,0.971038,4.409925,-2.868740,0.854518,33.286449,-165.232727,-22.790722
+0.174444,0.012268,0.973113,4.364147,-2.807703,0.915555,33.286449,-165.232727,-22.790722
+0.174322,0.012146,0.966155,4.455702,-2.945036,0.900296,32.986568,-164.632980,-23.690355
+0.175298,0.013855,0.972930,4.379406,-2.838221,0.824000,32.986568,-164.632980,-23.690355
+0.171270,0.012024,0.971282,4.333628,-2.883999,0.930815,32.986568,-164.632980,-23.690355
+0.173284,0.013245,0.971038,4.379406,-2.960295,0.854518,33.886204,-164.932846,-23.090599
+0.172979,0.013489,0.971343,4.394665,-2.838221,0.885037,33.886204,-164.932846,-23.090599
+0.174932,0.012513,0.970733,4.364147,-2.914518,0.930815,33.736263,-164.782913,-23.240538
+0.174322,0.014344,0.975616,4.379406,-2.899258,0.930815,33.736263,-164.782913,-23.240538
+0.174322,0.013245,0.972503,4.440443,-2.731407,0.900296,33.736263,-164.782913,-23.240538
+0.174627,0.012818,0.972442,4.379406,-2.777184,0.885037,32.536755,-164.183151,-22.640781
+0.174444,0.012940,0.973174,4.333628,-2.899258,0.915555,32.536755,-164.183151,-22.640781
+0.173894,0.012940,0.971587,4.425184,-2.899258,0.869778,32.836632,-164.782913,-23.240538
+0.173528,0.012696,0.973662,4.318369,-2.838221,0.869778,32.836632,-164.782913,-23.240538
+0.174993,0.013489,0.971282,4.409925,-2.853481,0.869778,32.836632,-164.782913,-23.240538
+0.172613,0.014405,0.974578,4.364147,-2.853481,0.885037,34.186081,-164.932846,-23.090599
+0.173650,0.012268,0.971099,4.318369,-2.807703,0.915555,34.186081,-164.932846,-23.090599
+0.172491,0.010865,0.972198,4.348887,-2.868740,0.869778,34.036144,-163.883286,-22.640781
+0.174383,0.013306,0.972930,4.455702,-2.945036,0.854518,34.036144,-163.883286,-22.640781
+0.173894,0.013001,0.970183,4.455702,-2.853481,0.961333,34.036144,-165.082794,-23.840294
+0.175359,0.013672,0.974395,4.440443,-2.777184,0.869778,34.036144,-165.082794,-23.840294
+0.172979,0.012146,0.972442,4.394665,-2.853481,0.854518,34.036144,-165.082794,-23.840294
+0.174261,0.013245,0.973174,4.486221,-2.807703,0.915555,32.986568,-165.532608,-23.090599
+0.173772,0.011963,0.973662,4.409925,-3.006073,0.885037,32.986568,-165.532608,-23.090599
+0.172613,0.012940,0.973052,4.394665,-2.838221,0.915555,32.836632,-164.782913,-24.140171
+0.174810,0.013611,0.974151,4.379406,-2.838221,0.946074,32.836632,-164.782913,-24.140171
+0.174688,0.012452,0.971099,4.425184,-2.853481,0.930815,32.836632,-164.782913,-24.140171
+0.174688,0.013428,0.970977,4.409925,-2.838221,0.946074,33.436386,-166.582184,-23.840294
+0.171575,0.014527,0.971709,4.409925,-2.868740,0.824000,33.436386,-166.582184,-23.840294
+0.173772,0.014588,0.968169,4.440443,-2.822962,0.869778,33.286449,-165.232727,-23.390476
+0.173162,0.011109,0.970489,4.364147,-2.822962,0.915555,33.286449,-165.232727,-23.390476
+0.173528,0.012207,0.975188,4.379406,-2.883999,0.885037,33.286449,-165.232727,-23.390476
+0.174932,0.012879,0.973601,4.379406,-2.822962,0.869778,33.586327,-164.932846,-22.790722
+0.172369,0.013489,0.971648,4.394665,-2.792444,0.900296,33.586327,-164.932846,-22.790722
+0.172918,0.013733,0.971648,4.409925,-2.822962,0.915555,34.036144,-164.183151,-22.640781
+0.174261,0.011902,0.975127,4.379406,-2.914518,0.900296,34.036144,-164.183151,-22.640781
+0.174078,0.014161,0.974822,4.364147,-2.883999,0.885037,34.036144,-164.183151,-22.640781
+0.173345,0.013489,0.972442,4.425184,-2.853481,0.991852,33.886204,-165.232727,-23.690355
+0.174749,0.012513,0.973235,4.394665,-2.868740,0.869778,33.886204,-165.232727,-23.690355
+0.174200,0.013062,0.969390,4.409925,-2.853481,0.869778,32.986568,-164.333099,-23.090599
+0.173589,0.014405,0.972564,4.425184,-2.853481,0.900296,32.986568,-164.333099,-23.090599
+0.175542,0.012329,0.969878,4.409925,-2.868740,0.854518,32.986568,-164.333099,-23.090599
+0.173101,0.013001,0.974334,4.394665,-2.838221,0.946074,32.836632,-165.382675,-24.140171
+0.174017,0.012879,0.974700,4.440443,-2.899258,0.961333,32.836632,-165.382675,-24.140171
+0.173040,0.013794,0.972381,4.348887,-2.960295,0.885037,32.686691,-164.333099,-23.090599
+0.174078,0.012757,0.969878,4.440443,-2.914518,0.885037,32.686691,-164.333099,-23.090599
+0.174627,0.015076,0.970489,4.409925,-2.792444,0.900296,32.686691,-164.333099,-23.090599
+0.176214,0.013733,0.971404,4.364147,-2.838221,0.869778,33.136509,-164.782913,-24.140171
+0.173894,0.011780,0.974456,4.364147,-2.792444,0.900296,33.136509,-164.782913,-24.140171
+0.173345,0.014039,0.975616,4.364147,-2.838221,0.885037,33.436386,-165.682541,-22.640781
+0.172979,0.013916,0.970183,4.425184,-2.914518,0.869778,33.436386,-165.682541,-22.640781
+0.176336,0.012207,0.974456,4.364147,-2.807703,0.900296,33.586327,-164.333099,-23.390476
+0.174017,0.012574,0.972869,4.409925,-2.853481,0.900296,33.586327,-164.333099,-23.390476
+0.174993,0.012085,0.970306,4.333628,-2.853481,0.946074,33.586327,-164.333099,-23.390476
+0.172369,0.013367,0.970306,4.318369,-2.807703,0.869778,33.736263,-164.782913,-23.840294
+0.173345,0.013184,0.975127,4.379406,-2.838221,0.900296,33.736263,-164.782913,-23.840294
+0.175237,0.011963,0.975799,4.531999,-2.929777,0.854518,34.036144,-165.382675,-22.041027
+0.176580,0.012146,0.972137,4.470962,-2.853481,0.869778,34.036144,-165.382675,-22.041027
+0.173711,0.013672,0.971831,4.409925,-2.868740,0.839259,34.036144,-165.382675,-22.041027
+0.173833,0.014527,0.971526,4.440443,-2.853481,0.885037,32.836632,-165.382675,-23.540417
+0.175115,0.012024,0.973601,4.348887,-2.822962,0.869778,32.836632,-165.382675,-23.540417
+0.173833,0.012696,0.971892,4.348887,-2.853481,0.869778,32.836632,-163.883286,-23.840294
+0.174444,0.013306,0.974822,4.364147,-2.822962,0.915555,32.836632,-163.883286,-23.840294
+0.175481,0.013550,0.974334,4.364147,-2.945036,0.793481,32.836632,-163.883286,-23.840294
+0.176275,0.013855,0.973052,4.409925,-2.868740,0.900296,34.485958,-164.033218,-23.090599
+0.173956,0.013855,0.972869,4.348887,-2.761925,0.854518,34.485958,-164.033218,-23.090599
+0.172796,0.014710,0.970061,4.409925,-2.853481,0.946074,31.936998,-163.583405,-22.940660
+0.175604,0.013733,0.975860,4.379406,-2.792444,0.961333,31.936998,-163.583405,-22.940660
+0.173284,0.013184,0.972930,4.409925,-2.868740,0.930815,31.936998,-163.583405,-22.940660
+0.172979,0.014832,0.972076,4.470962,-2.807703,0.915555,33.136509,-164.483032,-23.240538
+0.174749,0.013855,0.972320,4.440443,-2.868740,0.854518,33.136509,-164.483032,-23.240538
+0.175298,0.012452,0.969817,4.348887,-2.853481,0.869778,33.436386,-165.082794,-22.340904
+0.173467,0.012940,0.973907,4.348887,-2.792444,0.885037,33.436386,-165.082794,-22.340904
+0.173894,0.014588,0.974822,4.379406,-2.746666,0.915555,33.436386,-165.082794,-22.340904
+0.173101,0.013367,0.973052,4.409925,-2.838221,0.824000,33.586327,-164.333099,-23.390476
+0.175420,0.013245,0.979278,4.440443,-2.883999,0.839259,33.586327,-164.333099,-23.390476
+0.174017,0.012024,0.971465,4.409925,-2.838221,0.900296,33.136509,-164.183151,-23.540417
+0.174505,0.013916,0.972442,4.394665,-2.807703,0.915555,33.136509,-164.183151,-23.540417
+0.175604,0.012452,0.972747,4.379406,-2.822962,0.885037,33.136509,-164.183151,-23.540417
+0.173956,0.014222,0.971526,4.394665,-2.746666,0.915555,33.886204,-164.333099,-22.790722
+0.173772,0.013123,0.972381,4.364147,-2.822962,0.824000,33.886204,-164.333099,-22.790722
+0.172552,0.013977,0.969878,4.409925,-2.822962,0.930815,33.436386,-165.082794,-23.540417
+0.173345,0.011658,0.971770,4.394665,-2.899258,0.915555,33.436386,-165.082794,-23.540417
+0.174749,0.013123,0.974151,4.364147,-2.868740,0.854518,32.986568,-164.932846,-23.390476
+0.173406,0.013489,0.972686,4.440443,-2.853481,0.915555,32.986568,-164.932846,-23.390476
+0.174200,0.015259,0.974212,4.425184,-2.792444,0.854518,32.986568,-164.932846,-23.390476
+0.175054,0.011597,0.971282,4.394665,-2.883999,0.946074,34.485958,-165.832489,-22.790722
+0.175054,0.012696,0.973296,4.379406,-2.838221,0.854518,34.485958,-165.832489,-22.790722
+0.174932,0.011841,0.973724,4.379406,-2.777184,0.900296,33.736263,-165.082794,-22.640781
+0.173284,0.015259,0.968963,4.440443,-2.914518,0.961333,33.736263,-165.082794,-22.640781
+0.174322,0.015259,0.969512,4.379406,-2.868740,0.885037,33.736263,-165.082794,-22.640781
+0.175054,0.012452,0.972747,4.379406,-2.807703,0.915555,34.785839,-164.333099,-22.490843
+0.174749,0.013428,0.972381,4.440443,-2.899258,0.854518,34.785839,-164.333099,-22.490843
+0.173406,0.012818,0.973174,4.409925,-2.883999,0.930815,33.286449,-164.932846,-22.790722
+0.172002,0.013245,0.974090,4.348887,-2.822962,0.930815,33.286449,-164.932846,-22.790722
+0.175420,0.013611,0.972747,4.470962,-2.853481,0.854518,33.286449,-164.932846,-22.790722
+0.172918,0.013367,0.973907,4.379406,-2.807703,0.900296,33.436386,-164.782913,-22.340904
+0.175176,0.013794,0.973113,4.364147,-2.914518,0.885037,33.436386,-164.782913,-22.340904
+0.174444,0.013245,0.974395,4.348887,-2.883999,0.869778,34.336021,-164.483032,-23.240538
+0.175176,0.013672,0.976043,4.318369,-2.807703,0.854518,34.336021,-164.483032,-23.240538
+0.175115,0.015748,0.971221,4.394665,-2.883999,0.930815,34.336021,-164.483032,-23.240538
+0.175359,0.013672,0.967193,4.364147,-2.807703,0.946074,32.986568,-165.532608,-23.390476
+0.174871,0.013245,0.975127,4.318369,-2.838221,0.915555,32.986568,-165.532608,-23.390476
+0.176031,0.014161,0.971343,4.333628,-2.792444,0.885037,34.036144,-164.483032,-23.540417
+0.175054,0.013428,0.969634,4.440443,-2.929777,0.839259,34.036144,-164.483032,-23.540417
+0.175237,0.011292,0.972808,4.394665,-2.822962,0.854518,34.036144,-164.483032,-23.540417
+0.174932,0.014710,0.975005,4.318369,-2.807703,0.839259,33.136509,-165.082794,-22.940660
+0.173772,0.014344,0.972747,4.486221,-2.868740,0.869778,33.136509,-165.082794,-22.940660
+0.172979,0.013794,0.975372,4.425184,-2.853481,0.915555,34.036144,-165.082794,-24.440050
+0.172979,0.014100,0.973235,4.348887,-2.960295,0.885037,34.036144,-165.082794,-24.440050
+0.173223,0.012574,0.976836,4.409925,-2.914518,0.885037,34.036144,-165.082794,-24.440050
+0.177557,0.011963,0.972076,4.486221,-2.960295,0.885037,33.586327,-164.632980,-22.790722
+0.175359,0.012329,0.974029,4.348887,-2.838221,0.915555,33.586327,-164.632980,-22.790722
+0.175726,0.014039,0.975188,4.379406,-2.945036,0.869778,33.736263,-165.082794,-22.041027
+0.173650,0.013428,0.973601,4.455702,-2.899258,0.991852,33.736263,-165.082794,-22.041027
+0.174200,0.012818,0.974700,4.379406,-2.868740,0.946074,33.736263,-165.082794,-22.041027
+0.172796,0.014100,0.972198,4.364147,-2.899258,0.854518,32.836632,-164.183151,-23.840294
+0.173956,0.015015,0.974334,4.379406,-2.853481,0.885037,32.836632,-164.183151,-23.840294
+0.175909,0.014649,0.974090,4.318369,-2.807703,0.885037,34.336021,-164.483032,-22.940660
+0.174200,0.014161,0.973296,4.348887,-2.822962,0.946074,34.336021,-164.483032,-22.940660
+0.172735,0.012879,0.972808,4.379406,-2.883999,0.915555,34.336021,-164.483032,-22.940660
+0.174627,0.012696,0.973418,4.364147,-2.792444,0.885037,32.836632,-164.782913,-23.540417
+0.173833,0.014344,0.976165,4.318369,-2.868740,0.900296,32.836632,-164.782913,-23.540417
+0.174749,0.014527,0.971954,4.379406,-2.883999,0.900296,33.286449,-164.333099,-22.490843
+0.175359,0.014100,0.974029,4.318369,-2.807703,0.915555,33.286449,-164.333099,-22.490843
+0.172552,0.014161,0.969512,4.364147,-2.883999,0.824000,32.986568,-163.433456,-23.690355
+0.171697,0.015625,0.975982,4.455702,-2.853481,0.900296,32.986568,-163.433456,-23.690355
+0.173956,0.012146,0.973601,4.425184,-2.883999,0.885037,32.986568,-163.433456,-23.690355
+0.173040,0.011597,0.972320,4.379406,-2.792444,0.869778,33.586327,-164.632980,-23.090599
+0.173711,0.013672,0.974334,4.394665,-2.929777,0.885037,33.586327,-164.632980,-23.090599
+0.173772,0.014588,0.969512,4.394665,-2.929777,0.869778,32.836632,-164.183151,-23.540417
+0.172185,0.011475,0.972686,4.409925,-2.807703,0.839259,32.836632,-164.183151,-23.540417
+0.174017,0.014100,0.971831,4.333628,-2.822962,0.900296,32.836632,-164.183151,-23.540417
+0.174444,0.013733,0.973174,4.303110,-2.914518,0.854518,33.736263,-164.183151,-22.940660
+0.173040,0.013855,0.974151,4.379406,-2.868740,0.900296,33.736263,-164.183151,-22.940660
+0.175115,0.014222,0.970611,4.470962,-2.853481,0.900296,33.586327,-166.132370,-23.690355
+0.172796,0.015259,0.975433,4.394665,-2.883999,0.915555,33.586327,-166.132370,-23.690355
+0.172918,0.014588,0.972930,4.379406,-2.853481,0.946074,33.586327,-166.132370,-23.690355
+0.175115,0.014039,0.972259,4.303110,-2.746666,0.915555,32.686691,-164.632980,-23.990232
+0.174566,0.013794,0.975311,4.379406,-2.853481,0.915555,32.686691,-164.632980,-23.990232
+0.174139,0.014710,0.972137,4.379406,-2.899258,0.839259,33.436386,-164.183151,-23.240538
+0.175054,0.013489,0.972930,4.333628,-2.761925,0.854518,33.436386,-164.183151,-23.240538
+0.174505,0.011902,0.975799,4.394665,-2.822962,0.854518,33.436386,-164.183151,-23.240538
+0.173589,0.012879,0.975433,4.303110,-2.899258,0.869778,33.136509,-165.382675,-22.940660
+0.175420,0.014344,0.969329,4.394665,-2.899258,0.915555,33.136509,-165.382675,-22.940660
+0.172430,0.014039,0.973846,4.516739,-2.868740,0.915555,31.936998,-164.183151,-22.940660
+0.174017,0.012207,0.971465,4.455702,-2.883999,0.885037,31.936998,-164.183151,-22.940660
+0.173956,0.012268,0.973785,4.470962,-2.899258,0.885037,32.836632,-165.382675,-22.041027
+0.174627,0.013062,0.973785,4.409925,-2.761925,0.930815,32.836632,-165.382675,-22.041027
+0.174078,0.013123,0.972625,4.394665,-2.914518,0.869778,32.836632,-165.382675,-22.041027
+0.174505,0.014222,0.972503,4.333628,-2.914518,0.869778,32.536755,-164.782913,-22.640781
+0.173345,0.012268,0.970733,4.394665,-2.822962,0.915555,32.536755,-164.782913,-22.640781
+0.174017,0.013367,0.973724,4.333628,-2.777184,0.839259,32.386814,-164.033218,-23.090599
+0.172796,0.012574,0.973418,4.440443,-2.792444,0.930815,32.386814,-164.033218,-23.090599
+0.174261,0.012879,0.971770,4.333628,-2.792444,0.839259,32.386814,-164.033218,-23.090599
+0.174810,0.013001,0.972381,4.409925,-2.792444,0.961333,31.936998,-165.082794,-22.640781
+0.174139,0.012757,0.975066,4.257332,-2.777184,0.885037,31.936998,-165.082794,-22.640781
+0.175726,0.012818,0.973968,4.348887,-2.899258,0.961333,32.236874,-164.483032,-23.540417
+0.174749,0.012085,0.972076,4.440443,-2.838221,0.885037,32.236874,-164.483032,-23.540417
+0.172308,0.013001,0.972747,4.379406,-2.838221,0.869778,32.236874,-164.483032,-23.540417
+0.172857,0.013794,0.970733,4.379406,-2.777184,0.869778,33.136509,-163.283524,-23.540417
+0.174444,0.013123,0.975433,4.409925,-2.838221,0.885037,33.136509,-163.283524,-23.540417
+0.173589,0.013306,0.975249,4.379406,-2.807703,0.915555,33.136509,-163.883286,-22.940660
+0.173956,0.013916,0.970855,4.379406,-2.792444,0.824000,33.136509,-163.883286,-22.940660
+0.174505,0.013001,0.974944,4.409925,-2.899258,0.869778,33.136509,-163.883286,-22.940660
+0.174383,0.015503,0.972259,4.409925,-2.853481,0.854518,33.886204,-164.632980,-23.690355
+0.173650,0.010804,0.970794,4.394665,-2.914518,0.854518,33.886204,-164.632980,-23.690355
+0.173345,0.015137,0.972747,4.425184,-2.807703,0.854518,33.286449,-164.632980,-23.090599
+0.174139,0.012879,0.972198,4.394665,-2.868740,0.900296,33.286449,-164.632980,-23.090599
+0.174322,0.014222,0.972869,4.348887,-2.807703,0.869778,33.286449,-164.632980,-23.090599
+0.173833,0.013428,0.972076,4.379406,-2.868740,0.885037,33.586327,-165.532608,-22.790722
+0.173956,0.016358,0.972747,4.287850,-2.777184,0.900296,33.586327,-165.532608,-22.790722
+0.173956,0.012879,0.972747,4.364147,-2.838221,0.869778,33.136509,-163.883286,-23.540417
+0.174322,0.014588,0.973601,4.348887,-2.807703,0.946074,33.136509,-163.883286,-23.540417
+0.174871,0.012818,0.975677,4.364147,-2.914518,0.885037,33.136509,-163.883286,-23.540417
+0.175481,0.012879,0.975433,4.333628,-2.899258,0.915555,32.536755,-164.183151,-22.940660
+0.174078,0.013306,0.974517,4.394665,-2.853481,0.854518,32.536755,-164.183151,-22.940660
+0.173772,0.014161,0.971038,4.394665,-2.822962,0.885037,32.836632,-164.782913,-21.741148
+0.172674,0.012574,0.974029,4.333628,-2.807703,0.839259,32.836632,-164.782913,-21.741148
+0.176153,0.014527,0.977203,4.425184,-2.883999,0.900296,32.836632,-164.782913,-21.741148
+0.174139,0.013550,0.970794,4.364147,-2.716147,0.900296,32.986568,-164.333099,-21.891088
+0.177618,0.013794,0.974700,4.379406,-2.899258,0.869778,32.986568,-164.333099,-21.891088
+0.173467,0.013489,0.970244,4.440443,-2.899258,0.915555,33.736263,-165.382675,-23.240538
+0.173406,0.013306,0.970916,4.425184,-2.868740,0.915555,33.736263,-165.382675,-23.240538
+0.174078,0.014893,0.974822,4.303110,-2.868740,0.900296,33.736263,-165.382675,-23.240538
+0.174810,0.014283,0.974151,4.364147,-2.883999,0.854518,33.436386,-164.483032,-22.640781
+0.173589,0.014039,0.974761,4.409925,-2.914518,0.946074,33.436386,-164.483032,-22.640781
+0.172430,0.013977,0.971343,4.394665,-2.883999,0.946074,32.536755,-163.283524,-22.940660
+0.173162,0.012391,0.974700,4.333628,-2.868740,0.869778,32.536755,-163.283524,-22.940660
+0.173833,0.013184,0.975982,4.394665,-2.899258,0.869778,33.436386,-165.082794,-22.940660
+0.176824,0.014039,0.969451,4.409925,-2.853481,0.869778,33.436386,-165.082794,-22.940660
+0.173956,0.012940,0.971770,4.364147,-2.822962,0.900296,33.436386,-165.082794,-22.940660
+0.173162,0.012146,0.971343,4.470962,-2.853481,0.854518,33.886204,-164.932846,-22.490843
+0.173894,0.012268,0.970977,4.364147,-2.914518,0.824000,33.886204,-164.932846,-22.490843
+0.173406,0.013001,0.970061,4.348887,-2.990814,0.869778,33.886204,-164.033218,-23.390476
+0.173528,0.012574,0.972442,4.379406,-2.792444,0.915555,33.886204,-164.033218,-23.390476
+0.174566,0.014161,0.972808,4.348887,-2.868740,0.915555,33.886204,-164.033218,-23.390476
+0.175054,0.013306,0.969878,4.348887,-2.868740,0.900296,33.136509,-163.883286,-22.640781
+0.174017,0.014222,0.973846,4.364147,-2.822962,0.854518,33.136509,-163.883286,-22.640781
+0.174566,0.015503,0.972381,4.348887,-2.853481,0.885037,34.336021,-164.483032,-22.340904
+0.174932,0.013184,0.972930,4.425184,-2.822962,0.915555,34.336021,-164.483032,-22.340904
+0.175115,0.014771,0.972198,4.394665,-2.761925,0.915555,34.336021,-164.483032,-22.340904
+0.175054,0.015259,0.973174,4.364147,-2.868740,0.869778,34.336021,-165.082794,-23.240538
+0.174139,0.015137,0.973968,4.455702,-2.868740,0.915555,34.336021,-165.082794,-23.240538
+0.176092,0.013672,0.972137,4.409925,-2.853481,0.930815,33.136509,-165.082794,-22.940660
+0.174749,0.012268,0.972808,4.379406,-2.838221,0.854518,33.136509,-165.082794,-22.940660
+0.173528,0.014527,0.973357,4.333628,-2.853481,0.930815,33.136509,-165.082794,-22.940660
+0.173040,0.013977,0.971465,4.318369,-2.853481,0.854518,32.986568,-164.632980,-23.090599
+0.173894,0.013916,0.973601,4.409925,-2.945036,0.915555,32.986568,-164.632980,-23.090599
+0.175359,0.013306,0.969695,4.364147,-2.838221,0.885037,33.436386,-164.483032,-22.940660
+0.174017,0.014710,0.972198,4.348887,-2.853481,0.854518,33.436386,-164.483032,-22.940660
+0.173956,0.013916,0.971526,4.287850,-2.899258,0.900296,33.436386,-164.483032,-22.940660
+0.173772,0.013977,0.972259,4.364147,-2.883999,0.915555,33.886204,-165.532608,-22.790722
+0.175176,0.013367,0.973418,4.470962,-2.792444,0.976592,33.886204,-165.532608,-22.790722
+0.176153,0.014161,0.970306,4.409925,-2.868740,0.885037,33.436386,-164.782913,-23.240538
+0.174322,0.013733,0.972930,4.379406,-2.883999,0.885037,33.436386,-164.782913,-23.240538
+0.174505,0.012757,0.976287,4.379406,-2.868740,0.885037,33.436386,-164.782913,-23.240538
+0.174017,0.012574,0.973662,4.379406,-2.868740,0.915555,33.736263,-164.183151,-23.840294
+0.173406,0.013367,0.977020,4.333628,-2.868740,0.900296,33.736263,-164.183151,-23.840294
+0.172979,0.013306,0.974700,4.364147,-2.853481,0.961333,32.836632,-165.082794,-22.940660
+0.174444,0.014039,0.972930,4.318369,-2.807703,0.900296,32.836632,-165.082794,-22.940660
+0.176031,0.014039,0.972137,4.333628,-2.838221,0.946074,32.836632,-165.082794,-22.940660
+0.173589,0.013489,0.973785,4.425184,-2.929777,0.900296,32.836632,-165.082794,-22.940660
+0.175237,0.012940,0.972808,4.425184,-2.822962,0.900296,32.836632,-165.082794,-22.940660
+0.174261,0.014466,0.971282,4.394665,-2.853481,0.824000,34.485958,-164.333099,-22.190966
+0.173467,0.013794,0.971831,4.364147,-2.868740,0.869778,34.485958,-164.333099,-22.190966
+0.174810,0.013001,0.971709,4.425184,-2.838221,0.900296,32.836632,-165.082794,-23.540417
+0.174383,0.015076,0.973479,4.440443,-2.853481,0.946074,32.836632,-165.082794,-23.540417
+0.176275,0.012268,0.972259,4.348887,-2.883999,0.869778,32.836632,-165.082794,-23.540417
+0.173162,0.012207,0.972625,4.501480,-2.807703,0.885037,33.586327,-164.033218,-22.190966
+0.173101,0.013306,0.975066,4.425184,-2.914518,0.900296,33.586327,-164.033218,-22.190966
+0.172735,0.012879,0.970977,4.364147,-2.822962,0.854518,32.986568,-164.932846,-21.891088
+0.175726,0.014222,0.973418,4.425184,-2.807703,0.885037,32.986568,-164.932846,-21.891088
+0.173894,0.012635,0.973968,4.425184,-2.929777,0.885037,32.986568,-164.932846,-21.891088
+0.174444,0.013367,0.971038,4.425184,-2.838221,0.915555,33.436386,-163.883286,-22.640781
+0.176031,0.014649,0.975494,4.394665,-2.914518,0.885037,33.436386,-163.883286,-22.640781
+0.172918,0.011963,0.971099,4.394665,-2.868740,0.854518,33.886204,-165.232727,-22.190966
+0.173956,0.013062,0.972320,4.470962,-2.807703,0.839259,33.886204,-165.232727,-22.190966
+0.173528,0.014588,0.973846,4.364147,-2.853481,0.915555,33.886204,-165.232727,-22.190966
+0.175420,0.012818,0.970244,4.394665,-2.899258,0.824000,32.986568,-164.033218,-23.090599
+0.174261,0.014161,0.974517,4.394665,-2.914518,0.869778,32.986568,-164.033218,-23.090599
+0.175298,0.012513,0.971221,4.409925,-2.853481,0.946074,32.836632,-163.583405,-23.240538
+0.175359,0.013306,0.970855,4.364147,-2.914518,0.869778,32.836632,-163.583405,-23.240538
+0.175665,0.012452,0.972808,4.333628,-2.899258,0.869778,32.836632,-163.583405,-23.240538
+0.174383,0.012696,0.970550,4.364147,-2.822962,0.900296,32.086937,-165.232727,-22.790722
+0.176458,0.013062,0.976775,4.394665,-2.883999,0.808740,32.086937,-165.232727,-22.790722
+0.174627,0.013062,0.973113,4.364147,-2.868740,0.900296,32.986568,-164.632980,-23.390476
+0.173772,0.013672,0.972259,4.394665,-2.945036,0.869778,32.986568,-164.632980,-23.390476
+0.173284,0.013184,0.972747,4.394665,-2.822962,0.869778,32.236874,-164.183151,-22.041027
+0.173406,0.012940,0.971709,4.409925,-2.914518,0.839259,32.236874,-164.183151,-22.041027
+0.174200,0.014527,0.976165,4.318369,-2.853481,0.946074,32.236874,-164.183151,-22.041027
+0.174444,0.012391,0.971831,4.333628,-2.868740,0.854518,34.036144,-163.283524,-22.640781
+0.173956,0.013855,0.969695,4.486221,-2.914518,0.946074,34.036144,-163.283524,-22.640781
+0.174017,0.012268,0.971221,4.364147,-2.883999,0.869778,33.136509,-164.782913,-22.940660
+0.173589,0.014039,0.972076,4.440443,-2.838221,0.930815,33.136509,-164.782913,-22.940660
+0.173894,0.013062,0.974212,4.470962,-2.945036,0.885037,33.136509,-164.782913,-22.940660
+0.172369,0.012146,0.969085,4.394665,-2.868740,0.885037,33.286449,-164.932846,-23.090599
+0.174871,0.012085,0.971160,4.364147,-2.822962,0.946074,33.286449,-164.932846,-23.090599
+0.174200,0.013184,0.971282,4.394665,-2.838221,0.915555,33.286449,-165.232727,-23.090599
+0.173162,0.013672,0.973479,4.364147,-2.853481,0.885037,33.286449,-165.232727,-23.090599
+0.175176,0.014161,0.973235,4.425184,-2.853481,0.915555,33.286449,-165.232727,-23.090599
+0.172491,0.012329,0.967559,4.348887,-2.853481,0.869778,33.586327,-164.632980,-23.390476
+0.173528,0.013611,0.969512,4.394665,-2.822962,0.839259,33.586327,-164.632980,-23.390476
+0.174017,0.013001,0.971160,4.364147,-2.822962,0.869778,33.586327,-164.033218,-23.690355
+0.173772,0.013916,0.975982,4.394665,-2.883999,0.915555,33.586327,-164.033218,-23.690355
+0.174627,0.013733,0.969512,4.364147,-2.853481,0.869778,33.586327,-164.033218,-23.690355
+0.174444,0.013306,0.973479,4.440443,-2.853481,0.885037,33.286449,-163.733337,-23.090599
+0.172918,0.013550,0.974578,4.409925,-2.883999,0.854518,33.286449,-163.733337,-23.090599
+0.174566,0.013001,0.971770,4.348887,-2.899258,0.854518,33.136509,-165.082794,-22.640781
+0.173711,0.013428,0.975005,4.394665,-2.883999,0.854518,33.136509,-165.082794,-22.640781
+0.175604,0.012574,0.971160,4.333628,-2.853481,0.869778,33.136509,-165.082794,-22.640781
+0.175359,0.014222,0.973174,4.409925,-2.807703,0.885037,33.886204,-164.632980,-23.390476
+0.172918,0.011414,0.972564,4.379406,-2.868740,0.900296,33.886204,-164.632980,-23.390476
+0.172185,0.014649,0.971526,4.364147,-2.868740,0.869778,32.686691,-164.932846,-23.390476
+0.173467,0.014283,0.971526,4.333628,-2.853481,0.885037,32.686691,-164.932846,-23.390476
+0.172796,0.013672,0.975677,4.440443,-2.838221,0.808740,32.686691,-164.932846,-23.390476
+0.173040,0.012696,0.970916,4.348887,-2.822962,0.839259,32.236874,-165.082794,-23.840294
+0.175298,0.014161,0.974700,4.318369,-2.822962,0.839259,32.236874,-165.082794,-23.840294
+0.175604,0.013855,0.971648,4.348887,-2.838221,0.900296,33.136509,-164.483032,-22.940660
+0.172918,0.011841,0.972747,4.364147,-2.899258,0.824000,33.136509,-164.483032,-22.940660
+0.173162,0.012635,0.973907,4.394665,-2.838221,0.824000,33.136509,-164.483032,-22.940660
+0.173894,0.014649,0.974151,4.394665,-2.822962,0.900296,32.686691,-165.832489,-21.891088
+0.174017,0.011902,0.974517,4.394665,-2.822962,0.824000,32.686691,-165.832489,-21.891088
+0.173101,0.011780,0.971038,4.379406,-2.822962,0.793481,31.787058,-164.632980,-22.790722
+0.174810,0.013550,0.972930,4.379406,-2.746666,0.869778,31.787058,-164.632980,-22.790722
+0.173345,0.014649,0.976104,4.348887,-2.883999,0.854518,31.787058,-164.632980,-22.790722
+0.171636,0.013123,0.973479,4.425184,-2.807703,0.900296,33.136509,-165.682541,-22.940660
+0.175115,0.013428,0.975860,4.409925,-2.868740,0.900296,33.136509,-165.682541,-22.940660
+0.173345,0.011902,0.969146,4.318369,-2.853481,0.885037,32.986568,-164.932846,-24.290112
+0.172308,0.013489,0.973418,4.409925,-2.914518,0.930815,32.986568,-164.932846,-24.290112
+0.174627,0.013245,0.976165,4.470962,-2.914518,0.930815,34.635899,-164.183151,-23.240538
+0.174444,0.012818,0.971648,4.379406,-2.853481,0.885037,34.635899,-164.183151,-23.240538
+0.173833,0.014039,0.971892,4.394665,-2.822962,0.900296,34.635899,-164.183151,-23.240538
+0.174810,0.012574,0.974395,4.333628,-2.883999,0.885037,33.586327,-164.932846,-23.090599
+0.172246,0.013794,0.974029,4.303110,-2.899258,0.839259,33.586327,-164.932846,-23.090599
+0.173894,0.012940,0.972869,4.409925,-2.792444,0.854518,32.986568,-164.333099,-23.690355
+0.174139,0.014954,0.974029,4.409925,-2.960295,0.869778,32.986568,-164.333099,-23.690355
+0.174200,0.011658,0.971526,4.379406,-2.822962,0.900296,32.986568,-164.333099,-23.690355
+0.174871,0.013611,0.974273,4.409925,-2.853481,1.007111,33.286449,-164.632980,-23.390476
+0.176275,0.013977,0.973235,4.318369,-2.853481,0.900296,33.286449,-164.632980,-23.390476
+0.173711,0.014222,0.969634,4.394665,-2.899258,0.900296,32.986568,-164.632980,-23.090599
+0.174078,0.011292,0.972991,4.379406,-2.960295,0.793481,32.986568,-164.632980,-23.090599
+0.174200,0.014466,0.971160,4.425184,-2.914518,0.915555,32.986568,-164.632980,-23.090599
+0.172918,0.014710,0.974029,4.348887,-2.822962,0.854518,33.136509,-163.583405,-22.940660
+0.172735,0.013672,0.972320,4.425184,-2.883999,0.854518,33.136509,-163.583405,-22.940660
+0.174993,0.014405,0.973968,4.470962,-2.853481,0.915555,33.886204,-164.333099,-23.090599
+0.173711,0.013062,0.973113,4.409925,-2.868740,0.946074,33.886204,-164.333099,-23.090599
+0.174505,0.014771,0.976043,4.364147,-2.792444,0.839259,33.886204,-164.333099,-23.090599
+0.174444,0.012452,0.970611,4.455702,-2.929777,0.869778,33.586327,-164.333099,-22.790722
+0.175115,0.011658,0.974029,4.379406,-2.822962,0.869778,33.586327,-164.333099,-22.790722
+0.173467,0.012574,0.969024,4.364147,-2.792444,0.946074,33.286449,-165.232727,-22.490843
+0.172552,0.012818,0.974761,4.364147,-2.807703,0.869778,33.286449,-165.232727,-22.490843
+0.174383,0.013855,0.975921,4.394665,-2.792444,0.869778,33.286449,-165.232727,-22.490843
+0.175604,0.012818,0.974639,4.486221,-2.868740,0.885037,33.286449,-164.033218,-23.690355
+0.173284,0.014344,0.972259,4.394665,-2.899258,0.869778,33.286449,-164.033218,-23.690355
+0.174871,0.012329,0.969817,4.364147,-2.822962,0.900296,33.286449,-164.033218,-23.390476
+0.175848,0.014100,0.969512,4.287850,-2.838221,0.808740,33.286449,-164.033218,-23.390476
+0.175359,0.015137,0.976836,4.348887,-2.838221,0.930815,33.736263,-165.082794,-23.540417
+0.173467,0.014954,0.976104,4.318369,-2.792444,0.885037,33.736263,-165.082794,-23.540417
+0.174139,0.012268,0.972137,4.379406,-2.899258,0.869778,33.736263,-165.082794,-23.540417
+0.176336,0.013916,0.973418,4.379406,-2.914518,0.915555,32.686691,-165.232727,-24.889866
+0.174444,0.012879,0.972564,4.425184,-2.761925,0.915555,32.686691,-165.232727,-24.889866
+0.174017,0.013123,0.973479,4.348887,-2.807703,0.885037,32.836632,-164.483032,-23.840294
+0.173894,0.013794,0.969512,4.379406,-2.746666,0.854518,32.836632,-164.483032,-23.840294
+0.174078,0.012513,0.973907,4.455702,-2.838221,0.946074,32.836632,-164.483032,-23.840294
+0.173650,0.012635,0.973662,4.348887,-2.883999,0.885037,33.736263,-164.483032,-23.840294
+0.174383,0.011780,0.971099,4.409925,-2.883999,0.930815,33.736263,-164.483032,-23.840294
+0.174261,0.014954,0.972503,4.379406,-2.853481,0.900296,34.036144,-165.382675,-22.340904
+0.172552,0.013489,0.971770,4.394665,-2.838221,0.854518,34.036144,-165.382675,-22.340904
+0.176580,0.014832,0.971892,4.425184,-2.807703,0.946074,34.036144,-165.382675,-22.340904
+0.174871,0.012391,0.970367,4.272591,-2.929777,0.885037,33.436386,-165.082794,-21.441271
+0.173345,0.013184,0.972625,4.409925,-2.868740,0.854518,33.436386,-165.082794,-21.441271
+0.172979,0.015503,0.969939,4.425184,-2.807703,0.900296,32.386814,-165.232727,-22.190966
+0.173650,0.015198,0.972869,4.440443,-2.914518,0.915555,32.386814,-165.232727,-22.190966
+0.173162,0.011048,0.974822,4.287850,-2.822962,0.854518,32.386814,-165.232727,-22.190966
+0.174261,0.013916,0.972076,4.409925,-2.883999,0.854518,33.286449,-165.832489,-23.090599
+0.173711,0.014527,0.973113,4.348887,-2.777184,0.839259,33.286449,-165.832489,-23.090599
+0.175481,0.014527,0.972442,4.455702,-2.807703,0.869778,33.136509,-165.682541,-23.840294
+0.174627,0.013001,0.973785,4.470962,-2.899258,0.961333,33.136509,-165.682541,-23.840294
+0.174627,0.012207,0.975433,4.379406,-2.822962,0.839259,33.136509,-165.682541,-23.840294
+0.174383,0.014649,0.973113,4.287850,-2.914518,0.808740,32.836632,-165.382675,-23.840294
+0.174688,0.013611,0.975127,4.425184,-2.716147,0.946074,32.836632,-165.382675,-23.840294
+0.174749,0.014039,0.970611,4.470962,-2.960295,0.930815,32.086937,-164.632980,-22.790722
+0.173589,0.013672,0.975188,4.440443,-2.883999,0.915555,32.086937,-164.632980,-22.790722
+0.172979,0.013184,0.973846,4.379406,-2.899258,0.808740,32.086937,-164.632980,-22.790722
+0.174444,0.012696,0.971038,4.303110,-2.822962,0.824000,33.286449,-164.632980,-23.090599
+0.173345,0.013001,0.971221,4.364147,-2.883999,0.839259,33.286449,-164.632980,-23.090599
+0.174322,0.014588,0.970306,4.379406,-2.838221,0.900296,33.136509,-165.382675,-22.340904
+0.175787,0.012391,0.971648,4.394665,-2.822962,0.885037,33.136509,-165.382675,-22.340904
+0.173406,0.012879,0.975188,4.409925,-2.883999,0.915555,33.136509,-165.382675,-22.340904
+0.175604,0.011475,0.973540,4.455702,-2.883999,0.839259,32.236874,-165.382675,-22.940660
+0.174505,0.015503,0.973113,4.409925,-2.899258,0.930815,32.236874,-165.382675,-22.940660
+0.174566,0.014344,0.973846,4.364147,-2.853481,0.915555,32.686691,-165.232727,-22.490843
+0.175237,0.014649,0.970977,4.348887,-2.914518,0.778222,32.686691,-165.232727,-22.490843
+0.174017,0.013794,0.973907,4.364147,-2.838221,0.808740,33.286449,-164.632980,-22.190966
+0.174139,0.014893,0.972503,4.394665,-2.838221,0.824000,33.286449,-164.632980,-22.190966
+0.172918,0.012085,0.972381,4.394665,-2.822962,0.778222,33.286449,-164.632980,-22.190966
+0.174078,0.012391,0.971221,4.501480,-2.914518,0.885037,33.286449,-165.232727,-23.090599
+0.176397,0.014222,0.973601,4.440443,-2.899258,0.885037,33.286449,-165.232727,-23.090599
+0.173894,0.013855,0.972564,4.425184,-2.899258,0.869778,32.386814,-164.632980,-23.990232
+0.175787,0.013245,0.971465,4.364147,-2.792444,0.915555,32.386814,-164.632980,-23.990232
+0.174627,0.013855,0.974151,4.379406,-2.838221,0.854518,32.386814,-164.632980,-23.990232
+0.175359,0.014039,0.970428,4.379406,-2.838221,0.885037,32.986568,-164.632980,-24.290112
+0.173162,0.012879,0.974883,4.333628,-2.883999,0.854518,32.986568,-164.632980,-24.290112
+0.175665,0.012757,0.973846,4.440443,-2.822962,0.854518,33.586327,-164.932846,-23.390476
+0.174993,0.015320,0.971587,4.379406,-2.960295,0.930815,33.586327,-164.932846,-23.390476
+0.173345,0.013611,0.975372,4.425184,-2.883999,0.930815,33.586327,-164.932846,-23.390476
+0.174322,0.013489,0.974090,4.425184,-2.929777,0.839259,34.336021,-164.483032,-22.940660
+0.173650,0.014283,0.975188,4.333628,-2.838221,0.839259,34.336021,-164.483032,-22.940660
+0.174810,0.014466,0.969451,4.394665,-2.883999,0.961333,33.586327,-164.033218,-22.490843
+0.174932,0.014222,0.976470,4.394665,-2.822962,0.885037,33.586327,-164.033218,-22.490843
+0.174139,0.014405,0.973846,4.440443,-2.883999,0.885037,33.586327,-164.033218,-22.490843
+0.175237,0.013489,0.973907,4.333628,-2.868740,0.900296,34.635899,-165.082794,-23.540417
+0.174627,0.014954,0.975005,4.455702,-2.838221,0.869778,34.635899,-165.082794,-23.540417
+0.175298,0.014039,0.974395,4.348887,-2.838221,0.869778,32.386814,-165.532608,-23.690355
+0.175176,0.015320,0.971282,4.409925,-2.792444,0.854518,32.386814,-165.532608,-23.690355
+0.174688,0.012513,0.974212,4.409925,-2.960295,0.885037,32.386814,-165.532608,-23.690355
+0.174810,0.012940,0.972930,4.425184,-2.868740,0.915555,33.736263,-164.782913,-23.840294
+0.173711,0.015198,0.971465,4.440443,-2.853481,0.869778,33.736263,-164.782913,-23.840294
+0.174505,0.014100,0.969634,4.409925,-2.868740,0.930815,33.736263,-165.382675,-23.540417
+0.173467,0.013367,0.974456,4.379406,-2.853481,0.961333,33.736263,-165.382675,-23.540417
+0.174749,0.014344,0.969390,4.425184,-2.853481,0.946074,33.736263,-165.382675,-23.540417
+0.173406,0.012574,0.974517,4.409925,-2.868740,0.930815,34.036144,-165.982422,-22.340904
+0.174017,0.014039,0.973296,4.440443,-2.990814,0.930815,34.036144,-165.982422,-22.340904
+0.173589,0.014832,0.970122,4.440443,-2.914518,0.854518,33.586327,-163.733337,-22.790722
+0.174627,0.013489,0.973601,4.379406,-2.853481,0.915555,33.586327,-163.733337,-22.790722
+0.173772,0.014283,0.974334,4.440443,-2.807703,0.915555,33.586327,-163.733337,-22.790722
+0.173650,0.013672,0.971099,4.394665,-2.838221,0.839259,33.736263,-165.082794,-22.940660
+0.173711,0.014832,0.974273,4.409925,-2.914518,0.930815,33.736263,-165.082794,-22.940660
+0.172613,0.012268,0.971221,4.425184,-2.822962,0.900296,32.986568,-165.832489,-22.790722
+0.174871,0.012574,0.974822,4.348887,-2.883999,0.900296,32.986568,-165.832489,-22.790722
+0.173406,0.014039,0.971282,4.318369,-2.807703,0.885037,32.986568,-165.832489,-22.790722
+0.173528,0.013794,0.974883,4.455702,-2.853481,0.869778,34.036144,-164.183151,-24.140171