From 034d0a6a610c8838d6c43b51a4401ce818b77624 Mon Sep 17 00:00:00 2001 From: Luis Rodrigues Date: Wed, 23 Jul 2014 18:19:49 +0200 Subject: [PATCH 001/216] driver for the TeraRangerOne I2C ranger finder --- makefiles/config_px4fmu-v1_default.mk | 1 + makefiles/config_px4fmu-v2_default.mk | 1 + src/drivers/trone/module.mk | 40 ++ src/drivers/trone/trone.cpp | 913 ++++++++++++++++++++++++++ 4 files changed, 955 insertions(+) create mode 100644 src/drivers/trone/module.mk create mode 100644 src/drivers/trone/trone.cpp diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index cc0958c29a..70c6f7df5c 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -26,6 +26,7 @@ MODULES += drivers/hmc5883 MODULES += drivers/ms5611 MODULES += drivers/mb12xx MODULES += drivers/ll40ls +MODULES += drivers/trone MODULES += drivers/gps MODULES += drivers/hil MODULES += drivers/hott/hott_telemetry diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index a498a1b402..2fcdd6b39c 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -29,6 +29,7 @@ MODULES += drivers/ms5611 MODULES += drivers/mb12xx MODULES += drivers/sf0x MODULES += drivers/ll40ls +MODULES += drivers/trone MODULES += drivers/gps MODULES += drivers/hil MODULES += drivers/hott/hott_telemetry diff --git a/src/drivers/trone/module.mk b/src/drivers/trone/module.mk new file mode 100644 index 0000000000..d8edf17cce --- /dev/null +++ b/src/drivers/trone/module.mk @@ -0,0 +1,40 @@ +############################################################################ +# +# Copyright (c) 2013 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# Makefile to build the TeraRanger One range finder driver +# + +MODULE_COMMAND = trone + +SRCS = trone.cpp diff --git a/src/drivers/trone/trone.cpp b/src/drivers/trone/trone.cpp new file mode 100644 index 0000000000..2f2f692a10 --- /dev/null +++ b/src/drivers/trone/trone.cpp @@ -0,0 +1,913 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file trone.cpp + * @author Luis Rodrigues + * + * Driver for the TeraRanger One range finders connected via I2C. + */ + +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include + +#include +#include +#include + +#include +#include + +#include + +/* Configuration Constants */ +#define TRONE_BUS PX4_I2C_BUS_EXPANSION +#define TRONE_BASEADDR 0x30 /* 7-bit address */ +#define TRONE_DEVICE_PATH "/dev/trone" + +/* TRONE Registers addresses */ + +#define TRONE_MEASURE_REG 0x00 /* Measure range register */ + +/* Device limits */ +#define TRONE_MIN_DISTANCE (0.20f) +#define TRONE_MAX_DISTANCE (14.00f) + +#define TRONE_CONVERSION_INTERVAL 50000 /* 50ms */ + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +#ifndef CONFIG_SCHED_WORKQUEUE +# error This requires CONFIG_SCHED_WORKQUEUE. +#endif + +class TRONE : public device::I2C +{ +public: + TRONE(int bus = TRONE_BUS, int address = TRONE_BASEADDR); + virtual ~TRONE(); + + virtual int init(); + + virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + + /** + * Diagnostics - print some basic information about the driver. + */ + void print_info(); + +protected: + virtual int probe(); + +private: + float _min_distance; + float _max_distance; + work_s _work; + RingBuffer *_reports; + bool _sensor_ok; + int _measure_ticks; + bool _collect_phase; + int _class_instance; + + orb_advert_t _range_finder_topic; + + perf_counter_t _sample_perf; + perf_counter_t _comms_errors; + perf_counter_t _buffer_overflows; + + /** + * Test whether the device supported by the driver is present at a + * specific address. + * + * @param address The I2C bus address to probe. + * @return True if the device is present. + */ + int probe_address(uint8_t address); + + /** + * Initialise the automatic measurement state machine and start it. + * + * @note This function is called at open and error time. It might make sense + * to make it more aggressive about resetting the bus in case of errors. + */ + void start(); + + /** + * Stop the automatic measurement state machine. + */ + void stop(); + + /** + * Set the min and max distance thresholds if you want the end points of the sensors + * range to be brought in at all, otherwise it will use the defaults TRONE_MIN_DISTANCE + * and TRONE_MAX_DISTANCE + */ + void set_minimum_distance(float min); + void set_maximum_distance(float max); + float get_minimum_distance(); + float get_maximum_distance(); + + /** + * Perform a poll cycle; collect from the previous measurement + * and start a new one. + */ + void cycle(); + int measure(); + int collect(); + /** + * Static trampoline from the workq context; because we don't have a + * generic workq wrapper yet. + * + * @param arg Instance pointer for the driver that is polling. + */ + static void cycle_trampoline(void *arg); + + +}; + +static const uint8_t crc_table[] = { + 0x00, 0x07, 0x0e, 0x09, 0x1c, 0x1b, 0x12, 0x15, 0x38, 0x3f, 0x36, 0x31, + 0x24, 0x23, 0x2a, 0x2d, 0x70, 0x77, 0x7e, 0x79, 0x6c, 0x6b, 0x62, 0x65, + 0x48, 0x4f, 0x46, 0x41, 0x54, 0x53, 0x5a, 0x5d, 0xe0, 0xe7, 0xee, 0xe9, + 0xfc, 0xfb, 0xf2, 0xf5, 0xd8, 0xdf, 0xd6, 0xd1, 0xc4, 0xc3, 0xca, 0xcd, + 0x90, 0x97, 0x9e, 0x99, 0x8c, 0x8b, 0x82, 0x85, 0xa8, 0xaf, 0xa6, 0xa1, + 0xb4, 0xb3, 0xba, 0xbd, 0xc7, 0xc0, 0xc9, 0xce, 0xdb, 0xdc, 0xd5, 0xd2, + 0xff, 0xf8, 0xf1, 0xf6, 0xe3, 0xe4, 0xed, 0xea, 0xb7, 0xb0, 0xb9, 0xbe, + 0xab, 0xac, 0xa5, 0xa2, 0x8f, 0x88, 0x81, 0x86, 0x93, 0x94, 0x9d, 0x9a, + 0x27, 0x20, 0x29, 0x2e, 0x3b, 0x3c, 0x35, 0x32, 0x1f, 0x18, 0x11, 0x16, + 0x03, 0x04, 0x0d, 0x0a, 0x57, 0x50, 0x59, 0x5e, 0x4b, 0x4c, 0x45, 0x42, + 0x6f, 0x68, 0x61, 0x66, 0x73, 0x74, 0x7d, 0x7a, 0x89, 0x8e, 0x87, 0x80, + 0x95, 0x92, 0x9b, 0x9c, 0xb1, 0xb6, 0xbf, 0xb8, 0xad, 0xaa, 0xa3, 0xa4, + 0xf9, 0xfe, 0xf7, 0xf0, 0xe5, 0xe2, 0xeb, 0xec, 0xc1, 0xc6, 0xcf, 0xc8, + 0xdd, 0xda, 0xd3, 0xd4, 0x69, 0x6e, 0x67, 0x60, 0x75, 0x72, 0x7b, 0x7c, + 0x51, 0x56, 0x5f, 0x58, 0x4d, 0x4a, 0x43, 0x44, 0x19, 0x1e, 0x17, 0x10, + 0x05, 0x02, 0x0b, 0x0c, 0x21, 0x26, 0x2f, 0x28, 0x3d, 0x3a, 0x33, 0x34, + 0x4e, 0x49, 0x40, 0x47, 0x52, 0x55, 0x5c, 0x5b, 0x76, 0x71, 0x78, 0x7f, + 0x6a, 0x6d, 0x64, 0x63, 0x3e, 0x39, 0x30, 0x37, 0x22, 0x25, 0x2c, 0x2b, + 0x06, 0x01, 0x08, 0x0f, 0x1a, 0x1d, 0x14, 0x13, 0xae, 0xa9, 0xa0, 0xa7, + 0xb2, 0xb5, 0xbc, 0xbb, 0x96, 0x91, 0x98, 0x9f, 0x8a, 0x8d, 0x84, 0x83, + 0xde, 0xd9, 0xd0, 0xd7, 0xc2, 0xc5, 0xcc, 0xcb, 0xe6, 0xe1, 0xe8, 0xef, + 0xfa, 0xfd, 0xf4, 0xf3 +}; + +uint8_t crc8(uint8_t *p, uint8_t len){ + uint16_t i; + uint16_t crc = 0x0; + + while (len--) { + i = (crc ^ *p++) & 0xFF; + crc = (crc_table[i] ^ (crc << 8)) & 0xFF; + } + + return crc & 0xFF; +} + +/* + * Driver 'main' command. + */ +extern "C" __EXPORT int trone_main(int argc, char *argv[]); + +TRONE::TRONE(int bus, int address) : + I2C("TRONE", TRONE_DEVICE_PATH, bus, address, 100000), + _min_distance(TRONE_MIN_DISTANCE), + _max_distance(TRONE_MAX_DISTANCE), + _reports(nullptr), + _sensor_ok(false), + _measure_ticks(0), + _collect_phase(false), + _class_instance(-1), + _range_finder_topic(-1), + _sample_perf(perf_alloc(PC_ELAPSED, "trone_read")), + _comms_errors(perf_alloc(PC_COUNT, "trone_comms_errors")), + _buffer_overflows(perf_alloc(PC_COUNT, "trone_buffer_overflows")) +{ + // up the retries since the device misses the first measure attempts + I2C::_retries = 3; + + // enable debug() calls + _debug_enabled = false; + + // work_cancel in the dtor will explode if we don't do this... + memset(&_work, 0, sizeof(_work)); +} + +TRONE::~TRONE() +{ + /* make sure we are truly inactive */ + stop(); + + /* free any existing reports */ + if (_reports != nullptr) { + delete _reports; + } + + if (_class_instance != -1) { + unregister_class_devname(RANGE_FINDER_DEVICE_PATH, _class_instance); + } + + // free perf counters + perf_free(_sample_perf); + perf_free(_comms_errors); + perf_free(_buffer_overflows); +} + +int +TRONE::init() +{ + int ret = ERROR; + + /* do I2C init (and probe) first */ + if (I2C::init() != OK) { + goto out; + } + + /* allocate basic report buffers */ + _reports = new RingBuffer(2, sizeof(range_finder_report)); + + if (_reports == nullptr) { + goto out; + } + + _class_instance = register_class_devname(RANGE_FINDER_DEVICE_PATH); + + if (_class_instance == CLASS_DEVICE_PRIMARY) { + /* get a publish handle on the range finder topic */ + struct range_finder_report rf_report; + measure(); + _reports->get(&rf_report); + _range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &rf_report); + + if (_range_finder_topic < 0) { + debug("failed to create sensor_range_finder object. Did you start uOrb?"); + } + } + + ret = OK; + /* sensor is ok, but we don't really know if it is within range */ + _sensor_ok = true; +out: + return ret; +} + +int +TRONE::probe() +{ + return measure(); +} + +void +TRONE::set_minimum_distance(float min) +{ + _min_distance = min; +} + +void +TRONE::set_maximum_distance(float max) +{ + _max_distance = max; +} + +float +TRONE::get_minimum_distance() +{ + return _min_distance; +} + +float +TRONE::get_maximum_distance() +{ + return _max_distance; +} + +int +TRONE::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + switch (cmd) { + + case SENSORIOCSPOLLRATE: { + switch (arg) { + + /* switching to manual polling */ + case SENSOR_POLLRATE_MANUAL: + stop(); + _measure_ticks = 0; + return OK; + + /* external signalling (DRDY) not supported */ + case SENSOR_POLLRATE_EXTERNAL: + + /* zero would be bad */ + case 0: + return -EINVAL; + + /* set default/max polling rate */ + case SENSOR_POLLRATE_MAX: + case SENSOR_POLLRATE_DEFAULT: { + /* do we need to start internal polling? */ + bool want_start = (_measure_ticks == 0); + + /* set interval for next measurement to minimum legal value */ + _measure_ticks = USEC2TICK(TRONE_CONVERSION_INTERVAL); + + /* if we need to start the poll state machine, do it */ + if (want_start) { + start(); + } + + return OK; + } + + /* adjust to a legal polling interval in Hz */ + default: { + /* do we need to start internal polling? */ + bool want_start = (_measure_ticks == 0); + + /* convert hz to tick interval via microseconds */ + unsigned ticks = USEC2TICK(1000000 / arg); + + /* check against maximum rate */ + if (ticks < USEC2TICK(TRONE_CONVERSION_INTERVAL)) { + return -EINVAL; + } + + /* update interval for next measurement */ + _measure_ticks = ticks; + + /* if we need to start the poll state machine, do it */ + if (want_start) { + start(); + } + + return OK; + } + } + } + + case SENSORIOCGPOLLRATE: + if (_measure_ticks == 0) { + return SENSOR_POLLRATE_MANUAL; + } + + return (1000 / _measure_ticks); + + case SENSORIOCSQUEUEDEPTH: { + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 1) || (arg > 100)) { + return -EINVAL; + } + + irqstate_t flags = irqsave(); + + if (!_reports->resize(arg)) { + irqrestore(flags); + return -ENOMEM; + } + + irqrestore(flags); + + return OK; + } + + case SENSORIOCGQUEUEDEPTH: + return _reports->size(); + + case SENSORIOCRESET: + /* XXX implement this */ + return -EINVAL; + + case RANGEFINDERIOCSETMINIUMDISTANCE: { + set_minimum_distance(*(float *)arg); + return 0; + } + break; + + case RANGEFINDERIOCSETMAXIUMDISTANCE: { + set_maximum_distance(*(float *)arg); + return 0; + } + break; + + default: + /* give it to the superclass */ + return I2C::ioctl(filp, cmd, arg); + } +} + +ssize_t +TRONE::read(struct file *filp, char *buffer, size_t buflen) +{ + unsigned count = buflen / sizeof(struct range_finder_report); + struct range_finder_report *rbuf = reinterpret_cast(buffer); + int ret = 0; + + /* buffer must be large enough */ + if (count < 1) { + return -ENOSPC; + } + + /* if automatic measurement is enabled */ + if (_measure_ticks > 0) { + + /* + * While there is space in the caller's buffer, and reports, copy them. + * Note that we may be pre-empted by the workq thread while we are doing this; + * we are careful to avoid racing with them. + */ + while (count--) { + if (_reports->get(rbuf)) { + ret += sizeof(*rbuf); + rbuf++; + } + } + + /* if there was no data, warn the caller */ + return ret ? ret : -EAGAIN; + } + + /* manual measurement - run one conversion */ + do { + _reports->flush(); + + /* trigger a measurement */ + if (OK != measure()) { + ret = -EIO; + break; + } + + /* wait for it to complete */ + usleep(TRONE_CONVERSION_INTERVAL); + + /* run the collection phase */ + if (OK != collect()) { + ret = -EIO; + break; + } + + /* state machine will have generated a report, copy it out */ + if (_reports->get(rbuf)) { + ret = sizeof(*rbuf); + } + + } while (0); + + return ret; +} + +int +TRONE::measure() +{ + int ret; + + /* + * Send the command to begin a measurement. + */ + const uint8_t cmd = TRONE_MEASURE_REG; + ret = transfer(&cmd, sizeof(cmd), nullptr, 0); + + if (OK != ret) { + perf_count(_comms_errors); + log("i2c::transfer returned %d", ret); + return ret; + } + + ret = OK; + + return ret; +} + +int +TRONE::collect() +{ + int ret = -EIO; + + /* read from the sensor */ + uint8_t val[3] = {0, 0, 0}; + + perf_begin(_sample_perf); + + ret = transfer(nullptr, 0, &val[0], 3); + + if (ret < 0) { + log("error reading from sensor: %d", ret); + perf_count(_comms_errors); + perf_end(_sample_perf); + return ret; + } + + uint16_t distance = (val[0] << 8) | val[1]; + float si_units = distance * 0.001f; /* mm to m */ + struct range_finder_report report; + + /* this should be fairly close to the end of the measurement, so the best approximation of the time */ + report.timestamp = hrt_absolute_time(); + report.error_count = perf_event_count(_comms_errors); + report.distance = si_units; + report.valid = crc8(val, 2) == val[2] && si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0; + + + /* publish it, if we are the primary */ + if (_range_finder_topic >= 0) { + orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &report); + } + + if (_reports->force(&report)) { + perf_count(_buffer_overflows); + } + + /* notify anyone waiting for data */ + poll_notify(POLLIN); + + ret = OK; + + perf_end(_sample_perf); + return ret; +} + +void +TRONE::start() +{ + /* reset the report ring and state machine */ + _collect_phase = false; + _reports->flush(); + + /* schedule a cycle to start things */ + work_queue(HPWORK, &_work, (worker_t)&TRONE::cycle_trampoline, this, 1); + + /* notify about state change */ + struct subsystem_info_s info = { + true, + true, + true, + SUBSYSTEM_TYPE_RANGEFINDER + }; + static orb_advert_t pub = -1; + + if (pub > 0) { + orb_publish(ORB_ID(subsystem_info), pub, &info); + + } else { + pub = orb_advertise(ORB_ID(subsystem_info), &info); + } +} + +void +TRONE::stop() +{ + work_cancel(HPWORK, &_work); +} + +void +TRONE::cycle_trampoline(void *arg) +{ + TRONE *dev = (TRONE *)arg; + + dev->cycle(); +} + +void +TRONE::cycle() +{ + /* collection phase? */ + if (_collect_phase) { + + /* perform collection */ + if (OK != collect()) { + log("collection error"); + /* restart the measurement state machine */ + start(); + return; + } + + /* next phase is measurement */ + _collect_phase = false; + + /* + * Is there a collect->measure gap? + */ + if (_measure_ticks > USEC2TICK(TRONE_CONVERSION_INTERVAL)) { + + /* schedule a fresh cycle call when we are ready to measure again */ + work_queue(HPWORK, + &_work, + (worker_t)&TRONE::cycle_trampoline, + this, + _measure_ticks - USEC2TICK(TRONE_CONVERSION_INTERVAL)); + + return; + } + } + + /* measurement phase */ + if (OK != measure()) { + log("measure error"); + } + + /* next phase is collection */ + _collect_phase = true; + + /* schedule a fresh cycle call when the measurement is done */ + work_queue(HPWORK, + &_work, + (worker_t)&TRONE::cycle_trampoline, + this, + USEC2TICK(TRONE_CONVERSION_INTERVAL)); +} + +void +TRONE::print_info() +{ + perf_print_counter(_sample_perf); + perf_print_counter(_comms_errors); + perf_print_counter(_buffer_overflows); + printf("poll interval: %u ticks\n", _measure_ticks); + _reports->print_info("report queue"); +} + +/** + * Local functions in support of the shell command. + */ +namespace trone +{ + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +const int ERROR = -1; + +TRONE *g_dev; + +void start(); +void stop(); +void test(); +void reset(); +void info(); + +/** + * Start the driver. + */ +void +start() +{ + int fd; + + if (g_dev != nullptr) { + errx(1, "already started"); + } + + /* create the driver */ + g_dev = new TRONE(TRONE_BUS); + + + if (g_dev == nullptr) { + goto fail; + } + + if (OK != g_dev->init()) { + goto fail; + } + + /* set the poll rate to default, starts automatic data collection */ + fd = open(TRONE_DEVICE_PATH, O_RDONLY); + + if (fd < 0) { + goto fail; + } + + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { + goto fail; + } + + exit(0); + +fail: + + if (g_dev != nullptr) { + delete g_dev; + g_dev = nullptr; + } + + errx(1, "driver start failed"); +} + +/** + * Stop the driver + */ +void stop() +{ + if (g_dev != nullptr) { + delete g_dev; + g_dev = nullptr; + + } else { + errx(1, "driver not running"); + } + + exit(0); +} + +/** + * Perform some basic functional tests on the driver; + * make sure we can collect data from the sensor in polled + * and automatic modes. + */ +void +test() +{ + struct range_finder_report report; + ssize_t sz; + int ret; + + int fd = open(TRONE_DEVICE_PATH, O_RDONLY); + + if (fd < 0) { + err(1, "%s open failed (try 'trone start' if the driver is not running", TRONE_DEVICE_PATH); + } + + /* do a simple demand read */ + sz = read(fd, &report, sizeof(report)); + + if (sz != sizeof(report)) { + err(1, "immediate read failed"); + } + + warnx("single read"); + warnx("measurement: %0.2f m", (double)report.distance); + warnx("time: %lld", report.timestamp); + + /* start the sensor polling at 2Hz */ + if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) { + errx(1, "failed to set 2Hz poll rate"); + } + + /* read the sensor 50x and report each value */ + for (unsigned i = 0; i < 50; i++) { + struct pollfd fds; + + /* wait for data to be ready */ + fds.fd = fd; + fds.events = POLLIN; + ret = poll(&fds, 1, 2000); + + if (ret != 1) { + errx(1, "timed out waiting for sensor data"); + } + + /* now go get it */ + sz = read(fd, &report, sizeof(report)); + + if (sz != sizeof(report)) { + err(1, "periodic read failed"); + } + + warnx("periodic read %u", i); + warnx("valid %u", report.valid); + warnx("measurement: %0.3f", (double)report.distance); + warnx("time: %lld", report.timestamp); + } + + /* reset the sensor polling to default rate */ + if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) { + errx(1, "failed to set default poll rate"); + } + + errx(0, "PASS"); +} + +/** + * Reset the driver. + */ +void +reset() +{ + int fd = open(TRONE_DEVICE_PATH, O_RDONLY); + + if (fd < 0) { + err(1, "failed "); + } + + if (ioctl(fd, SENSORIOCRESET, 0) < 0) { + err(1, "driver reset failed"); + } + + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { + err(1, "driver poll restart failed"); + } + + exit(0); +} + +/** + * Print a little info about the driver. + */ +void +info() +{ + if (g_dev == nullptr) { + errx(1, "driver not running"); + } + + printf("state @ %p\n", g_dev); + g_dev->print_info(); + + exit(0); +} + +} // namespace + +int +trone_main(int argc, char *argv[]) +{ + /* + * Start/load the driver. + */ + if (!strcmp(argv[1], "start")) { + trone::start(); + } + + /* + * Stop the driver + */ + if (!strcmp(argv[1], "stop")) { + trone::stop(); + } + + /* + * Test the driver/device. + */ + if (!strcmp(argv[1], "test")) { + trone::test(); + } + + /* + * Reset the driver. + */ + if (!strcmp(argv[1], "reset")) { + trone::reset(); + } + + /* + * Print driver information. + */ + if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) { + trone::info(); + } + + errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'"); +} From a30a5d2665d3d0f68262d7de68aa5d4086a42321 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 25 Jul 2014 14:27:49 +0200 Subject: [PATCH 002/216] update attitude_estimator_ekf, includes matlab This adds the latest c implementation (matlab coder) of attitude_estimator_ekf, the .m matlab script and the .prj file with the settings to export the matlab code to c --- .../attitude_estimator_ekf/AttitudeEKF.m | 286 +++ .../attitudeKalmanfilter.prj | 503 ++++++ .../attitude_estimator_ekf_main.cpp | 33 +- .../attitude_estimator_ekf_params.c | 41 +- .../attitude_estimator_ekf_params.h | 10 +- .../codegen/AttitudeEKF.c | 1573 +++++++++++++++++ .../codegen/AttitudeEKF.h | 26 + .../codegen/AttitudeEKF_types.h | 17 + .../codegen/attitudeKalmanfilter.c | 1148 ------------ .../codegen/attitudeKalmanfilter.h | 34 - .../codegen/attitudeKalmanfilter_initialize.c | 31 - .../codegen/attitudeKalmanfilter_initialize.h | 34 - .../codegen/attitudeKalmanfilter_terminate.c | 31 - .../codegen/attitudeKalmanfilter_terminate.h | 34 - .../codegen/attitudeKalmanfilter_types.h | 16 - .../attitude_estimator_ekf/codegen/cross.c | 37 - .../attitude_estimator_ekf/codegen/cross.h | 34 - .../attitude_estimator_ekf/codegen/eye.c | 51 - .../attitude_estimator_ekf/codegen/eye.h | 35 - .../attitude_estimator_ekf/codegen/mrdivide.c | 357 ---- .../attitude_estimator_ekf/codegen/mrdivide.h | 36 - .../attitude_estimator_ekf/codegen/norm.c | 54 - .../attitude_estimator_ekf/codegen/norm.h | 34 - .../attitude_estimator_ekf/codegen/rdivide.c | 38 - .../attitude_estimator_ekf/codegen/rdivide.h | 34 - .../attitude_estimator_ekf/codegen/rtGetInf.c | 139 -- .../attitude_estimator_ekf/codegen/rtGetInf.h | 23 - .../attitude_estimator_ekf/codegen/rtGetNaN.c | 96 - .../attitude_estimator_ekf/codegen/rtGetNaN.h | 21 - .../codegen/rt_defines.h | 24 - .../codegen/rt_nonfinite.c | 87 - .../codegen/rt_nonfinite.h | 53 - .../attitude_estimator_ekf/codegen/rtwtypes.h | 319 ++-- src/modules/attitude_estimator_ekf/module.mk | 12 +- 34 files changed, 2632 insertions(+), 2669 deletions(-) create mode 100644 src/modules/attitude_estimator_ekf/AttitudeEKF.m create mode 100644 src/modules/attitude_estimator_ekf/attitudeKalmanfilter.prj create mode 100644 src/modules/attitude_estimator_ekf/codegen/AttitudeEKF.c create mode 100644 src/modules/attitude_estimator_ekf/codegen/AttitudeEKF.h create mode 100644 src/modules/attitude_estimator_ekf/codegen/AttitudeEKF_types.h delete mode 100755 src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c delete mode 100755 src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h delete mode 100755 src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c delete mode 100755 src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h delete mode 100755 src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c delete mode 100755 src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h delete mode 100755 src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h delete mode 100755 src/modules/attitude_estimator_ekf/codegen/cross.c delete mode 100755 src/modules/attitude_estimator_ekf/codegen/cross.h delete mode 100755 src/modules/attitude_estimator_ekf/codegen/eye.c delete mode 100755 src/modules/attitude_estimator_ekf/codegen/eye.h delete mode 100755 src/modules/attitude_estimator_ekf/codegen/mrdivide.c delete mode 100755 src/modules/attitude_estimator_ekf/codegen/mrdivide.h delete mode 100755 src/modules/attitude_estimator_ekf/codegen/norm.c delete mode 100755 src/modules/attitude_estimator_ekf/codegen/norm.h delete mode 100755 src/modules/attitude_estimator_ekf/codegen/rdivide.c delete mode 100755 src/modules/attitude_estimator_ekf/codegen/rdivide.h delete mode 100755 src/modules/attitude_estimator_ekf/codegen/rtGetInf.c delete mode 100755 src/modules/attitude_estimator_ekf/codegen/rtGetInf.h delete mode 100755 src/modules/attitude_estimator_ekf/codegen/rtGetNaN.c delete mode 100755 src/modules/attitude_estimator_ekf/codegen/rtGetNaN.h delete mode 100755 src/modules/attitude_estimator_ekf/codegen/rt_defines.h delete mode 100755 src/modules/attitude_estimator_ekf/codegen/rt_nonfinite.c delete mode 100755 src/modules/attitude_estimator_ekf/codegen/rt_nonfinite.h mode change 100755 => 100644 src/modules/attitude_estimator_ekf/codegen/rtwtypes.h diff --git a/src/modules/attitude_estimator_ekf/AttitudeEKF.m b/src/modules/attitude_estimator_ekf/AttitudeEKF.m new file mode 100644 index 0000000000..1218cb65da --- /dev/null +++ b/src/modules/attitude_estimator_ekf/AttitudeEKF.m @@ -0,0 +1,286 @@ +function [xa_apo,Pa_apo,Rot_matrix,eulerAngles,debugOutput]... + = AttitudeEKF(approx_prediction,use_inertia_matrix,zFlag,dt,z,q_rotSpeed,q_rotAcc,q_acc,q_mag,r_gyro,r_accel,r_mag,J) + + +%LQG Postion Estimator and Controller +% Observer: +% x[n|n] = x[n|n-1] + M(y[n] - Cx[n|n-1] - Du[n]) +% x[n+1|n] = Ax[n|n] + Bu[n] +% +% $Author: Tobias Naegeli $ $Date: 2014 $ $Revision: 3 $ +% +% +% Arguments: +% approx_prediction: if 1 then the exponential map is approximated with a +% first order taylor approximation. has at the moment not a big influence +% (just 1st or 2nd order approximation) we should change it to rodriquez +% approximation. +% use_inertia_matrix: set to true if you have the inertia matrix J for your +% quadrotor +% xa_apo_k: old state vectotr +% zFlag: if sensor measurement is available [gyro, acc, mag] +% dt: dt in s +% z: measurements [gyro, acc, mag] +% q_rotSpeed: process noise gyro +% q_rotAcc: process noise gyro acceleration +% q_acc: process noise acceleration +% q_mag: process noise magnetometer +% r_gyro: measurement noise gyro +% r_accel: measurement noise accel +% r_mag: measurement noise mag +% J: moment of inertia matrix + + +% Output: +% xa_apo: updated state vectotr +% Pa_apo: updated state covariance matrix +% Rot_matrix: rotation matrix +% eulerAngles: euler angles +% debugOutput: not used + + +%% model specific parameters + + + +% compute once the inverse of the Inertia +persistent Ji; +if isempty(Ji) + Ji=single(inv(J)); +end + +%% init +persistent x_apo +if(isempty(x_apo)) + gyro_init=single([0;0;0]); + gyro_acc_init=single([0;0;0]); + acc_init=single([0;0;-9.81]); + mag_init=single([1;0;0]); + x_apo=single([gyro_init;gyro_acc_init;acc_init;mag_init]); + +end + +persistent P_apo +if(isempty(P_apo)) + % P_apo = single(eye(NSTATES) * 1000); + P_apo = single(200*ones(12)); +end + +debugOutput = single(zeros(4,1)); + +%% copy the states +wx= x_apo(1); % x body angular rate +wy= x_apo(2); % y body angular rate +wz= x_apo(3); % z body angular rate + +wax= x_apo(4); % x body angular acceleration +way= x_apo(5); % y body angular acceleration +waz= x_apo(6); % z body angular acceleration + +zex= x_apo(7); % x component gravity vector +zey= x_apo(8); % y component gravity vector +zez= x_apo(9); % z component gravity vector + +mux= x_apo(10); % x component magnetic field vector +muy= x_apo(11); % y component magnetic field vector +muz= x_apo(12); % z component magnetic field vector + + + + +%% prediction section +% compute the apriori state estimate from the previous aposteriori estimate +%body angular accelerations +if (use_inertia_matrix==1) + wak =[wax;way;waz]+Ji*(-cross([wax;way;waz],J*[wax;way;waz]))*dt; +else + wak =[wax;way;waz]; +end + +%body angular rates +wk =[wx; wy; wz] + dt*wak; + +%derivative of the prediction rotation matrix +O=[0,-wz,wy;wz,0,-wx;-wy,wx,0]'; + +%prediction of the earth z vector +if (approx_prediction==1) + %e^(Odt)=I+dt*O+dt^2/2!O^2 + % so we do a first order approximation of the exponential map + zek =(O*dt+single(eye(3)))*[zex;zey;zez]; + +else + zek =(single(eye(3))+O*dt+dt^2/2*O^2)*[zex;zey;zez]; + %zek =expm2(O*dt)*[zex;zey;zez]; not working because use double + %precision +end + + + +%prediction of the magnetic vector +if (approx_prediction==1) + %e^(Odt)=I+dt*O+dt^2/2!O^2 + % so we do a first order approximation of the exponential map + muk =(O*dt+single(eye(3)))*[mux;muy;muz]; +else + muk =(single(eye(3))+O*dt+dt^2/2*O^2)*[mux;muy;muz]; + %muk =expm2(O*dt)*[mux;muy;muz]; not working because use double + %precision +end + +x_apr=[wk;wak;zek;muk]; + +% compute the apriori error covariance estimate from the previous +%aposteriori estimate + +EZ=[0,zez,-zey; + -zez,0,zex; + zey,-zex,0]'; +MA=[0,muz,-muy; + -muz,0,mux; + muy,-mux,0]'; + +E=single(eye(3)); +Z=single(zeros(3)); + +A_lin=[ Z, E, Z, Z + Z, Z, Z, Z + EZ, Z, O, Z + MA, Z, Z, O]; + +A_lin=eye(12)+A_lin*dt; + +%process covariance matrix + +persistent Q +if (isempty(Q)) + Q=diag([ q_rotSpeed,q_rotSpeed,q_rotSpeed,... + q_rotAcc,q_rotAcc,q_rotAcc,... + q_acc,q_acc,q_acc,... + q_mag,q_mag,q_mag]); +end + +P_apr=A_lin*P_apo*A_lin'+Q; + + +%% update +if zFlag(1)==1&&zFlag(2)==1&&zFlag(3)==1 + + R=[r_gyro,0,0,0,0,0,0,0,0; + 0,r_gyro,0,0,0,0,0,0,0; + 0,0,r_gyro,0,0,0,0,0,0; + 0,0,0,r_accel,0,0,0,0,0; + 0,0,0,0,r_accel,0,0,0,0; + 0,0,0,0,0,r_accel,0,0,0; + 0,0,0,0,0,0,r_mag,0,0; + 0,0,0,0,0,0,0,r_mag,0; + 0,0,0,0,0,0,0,0,r_mag]; + %observation matrix + %[zw;ze;zmk]; + H_k=[ E, Z, Z, Z; + Z, Z, E, Z; + Z, Z, Z, E]; + + y_k=z(1:9)-H_k*x_apr; + + + S_k=H_k*P_apr*H_k'+R; + K_k=(P_apr*H_k'/(S_k)); + + + x_apo=x_apr+K_k*y_k; + P_apo=(eye(12)-K_k*H_k)*P_apr; +else + if zFlag(1)==1&&zFlag(2)==0&&zFlag(3)==0 + + R=[r_gyro,0,0; + 0,r_gyro,0; + 0,0,r_gyro]; + %observation matrix + + H_k=[ E, Z, Z, Z]; + + y_k=z(1:3)-H_k(1:3,1:12)*x_apr; + + S_k=H_k(1:3,1:12)*P_apr*H_k(1:3,1:12)'+R(1:3,1:3); + K_k=(P_apr*H_k(1:3,1:12)'/(S_k)); + + + x_apo=x_apr+K_k*y_k; + P_apo=(eye(12)-K_k*H_k(1:3,1:12))*P_apr; + else + if zFlag(1)==1&&zFlag(2)==1&&zFlag(3)==0 + + R=[r_gyro,0,0,0,0,0; + 0,r_gyro,0,0,0,0; + 0,0,r_gyro,0,0,0; + 0,0,0,r_accel,0,0; + 0,0,0,0,r_accel,0; + 0,0,0,0,0,r_accel]; + + %observation matrix + + H_k=[ E, Z, Z, Z; + Z, Z, E, Z]; + + y_k=z(1:6)-H_k(1:6,1:12)*x_apr; + + S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)'+R(1:6,1:6); + K_k=(P_apr*H_k(1:6,1:12)'/(S_k)); + + + x_apo=x_apr+K_k*y_k; + P_apo=(eye(12)-K_k*H_k(1:6,1:12))*P_apr; + else + if zFlag(1)==1&&zFlag(2)==0&&zFlag(3)==1 + R=[r_gyro,0,0,0,0,0; + 0,r_gyro,0,0,0,0; + 0,0,r_gyro,0,0,0; + 0,0,0,r_mag,0,0; + 0,0,0,0,r_mag,0; + 0,0,0,0,0,r_mag]; + %observation matrix + + H_k=[ E, Z, Z, Z; + Z, Z, Z, E]; + + y_k=[z(1:3);z(7:9)]-H_k(1:6,1:12)*x_apr; + + S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)'+R(1:6,1:6); + K_k=(P_apr*H_k(1:6,1:12)'/(S_k)); + + + x_apo=x_apr+K_k*y_k; + P_apo=(eye(12)-K_k*H_k(1:6,1:12))*P_apr; + else + x_apo=x_apr; + P_apo=P_apr; + end + end + end +end + + + +%% euler anglels extraction +z_n_b = -x_apo(7:9)./norm(x_apo(7:9)); +m_n_b = x_apo(10:12)./norm(x_apo(10:12)); + +y_n_b=cross(z_n_b,m_n_b); +y_n_b=y_n_b./norm(y_n_b); + +x_n_b=(cross(y_n_b,z_n_b)); +x_n_b=x_n_b./norm(x_n_b); + + +xa_apo=x_apo; +Pa_apo=P_apo; +% rotation matrix from earth to body system +Rot_matrix=[x_n_b,y_n_b,z_n_b]; + + +phi=atan2(Rot_matrix(2,3),Rot_matrix(3,3)); +theta=-asin(Rot_matrix(1,3)); +psi=atan2(Rot_matrix(1,2),Rot_matrix(1,1)); +eulerAngles=[phi;theta;psi]; + diff --git a/src/modules/attitude_estimator_ekf/attitudeKalmanfilter.prj b/src/modules/attitude_estimator_ekf/attitudeKalmanfilter.prj new file mode 100644 index 0000000000..a8315bf571 --- /dev/null +++ b/src/modules/attitude_estimator_ekf/attitudeKalmanfilter.prj @@ -0,0 +1,503 @@ + + + + + false + false + false + option.WorkingFolder.Project + + option.BuildFolder.Project + + + true + true + true + true + option.GlobalDataSyncMethod.SyncAlways + true + option.DynamicMemoryAllocation.Threshold + 65536 + 200000 + option.FilePartitionMethod.MapMFileToCFile + true + false + + true + false + true + false + + + + + + + + + true + false + 40000 + 100 + option.TargetLang.C + true + 10 + 200 + 4000 + true + 64 + true + true + option.ConstantInputs.CheckValues + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + true + option.WorkingFolder.Project + + option.BuildFolder.Specified + codegen + + true + false + false + false + option.DynamicMemoryAllocation.Threshold + 65536 + 200000 + false + option.FilePartitionMethod.SingleFile + true + true + false + option.DataTypeReplacement.CBuiltIn + false + true + option.ParenthesesLevel.Nominal + 31 + $M$N + $M$N + $M$N + $M$N + $M$N + $M$N + emxArray_$M$N + emx$M$N + + true + false + true + true + false + true + + + + + + + + + C89/C90 (ANSI) + true + ARM Compatible + ARM Cortex + true + 8 + 16 + 32 + 32 + 64 + 32 + 64 + 32 + 32 + option.HardwareEndianness.Little + true + false + option.HardwareAtomicIntegerSize.Long + option.HardwareAtomicFloatSize.Double + option.HardwareDivisionRounding.Undefined + Generic + MATLAB Host Computer + false + 8 + 16 + 32 + 64 + 64 + 32 + 64 + 64 + 64 + option.HardwareEndianness.Little + true + true + option.HardwareAtomicIntegerSize.Char + option.HardwareAtomicFloatSize.None + option.HardwareDivisionRounding.Zero + Automatically locate an installed toolchain + Faster Builds + + 40000 + 100 + true + option.TargetLang.C + option.CCompilerOptimization.On + + true + false + make_rtw + default_tmf + + 10 + 200 + 4000 + true + 64 + true + true + false + true + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + /opt/matlab/r2013b/bin/codegen/codegen/lib/AttitudeEKF/AttitudeEKF.a + R2012a + true + t:\private\desktop-dinfk-xp\Attitude_Kalmanfilter\codegen\mex\attitudeKalmanfilter\html\index.html + /home/thomasgubler/src/Firmware/src/modules/attitude_estimator_ekf/codegen/html/index.html + true + option.VerificationMode.None + false + ${PROJECT_ROOT}/AttitudeEKF_Test.m + ${PROJECT_ROOT}/AttitudeEKF_Test.m + false + false + 1024 + 2048 + AttitudeEKF_mex + AttitudeEKF + option.target.artifact.lib + option.FixedPointTypeProposalMode.ProposeFractionLengths + numerictype([],16,12) + 0 + true + false + false + false + true + false + + option.FixedPointMode.None + false + + + 16 + 4 + 0 + fimath('RoundingMethod', 'Floor', 'OverflowAction', 'Wrap', 'ProductMode', 'FullPrecision', 'MaxProductWordLength', 128, 'SumMode', 'FullPrecision', 'MaxSumWordLength', 128) + option.FixedPointTypeSource.SimAndDerived + + false + false + false + true + + + + + + + + + true + _fixpt + option.DefaultFixedPointSignedness.Automatic + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + uint8 + false + 1 x 1 + false + + + uint8 + false + 1 x 1 + false + + + uint8 + false + 3 x 1 + false + + + single + false + 1 x 1 + false + + + single + false + 9 x 1 + false + + + single + false + 1 x 1 + false + + + single + false + 1 x 1 + false + + + single + false + 1 x 1 + false + + + single + false + 1 x 1 + false + + + single + false + 1 x 1 + false + + + single + false + 1 x 1 + false + + + single + false + 1 x 1 + false + + + single + false + 3 x 3 + false + + + + + + ${PROJECT_ROOT}/AttitudeEKF_Test.m + + + + /opt/matlab/r2013b/bin/codegen/codegen/lib/AttitudeEKF/AttitudeEKF.a + + + + /opt/matlab/r2013b + + + + + + true + false + false + false + false + false + true + false + 3.15.5-1-ARCH + false + true + glnxa64 + true + + + + diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index 35dc39ec6a..e1bbf5bc7a 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -38,6 +38,7 @@ * * @author Tobias Naegeli * @author Lorenz Meier + * @author Thomas Gubler */ #include @@ -74,8 +75,7 @@ #ifdef __cplusplus extern "C" { #endif -#include "codegen/attitudeKalmanfilter_initialize.h" -#include "codegen/attitudeKalmanfilter.h" +#include "codegen/AttitudeEKF.h" #include "attitude_estimator_ekf_params.h" #ifdef __cplusplus } @@ -206,6 +206,8 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds 0, 0, 1.f }; /**< init: identity matrix */ + float debugOutput[4] = { 0.0f }; + // print text printf("Extended Kalman Filter Attitude Estimator initialized..\n\n"); fflush(stdout); @@ -213,7 +215,7 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds int overloadcounter = 19; /* Initialize filter */ - attitudeKalmanfilter_initialize(); + AttitudeEKF_initialize(); /* store start time to guard against too slow update rates */ uint64_t last_run = hrt_absolute_time(); @@ -277,9 +279,9 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds /* keep track of sensor updates */ uint64_t sensor_last_timestamp[3] = {0, 0, 0}; - struct attitude_estimator_ekf_params ekf_params; + struct attitude_estimator_ekf_params ekf_params = { 0 }; - struct attitude_estimator_ekf_param_handles ekf_param_handles; + struct attitude_estimator_ekf_param_handles ekf_param_handles = { 0 }; /* initialize parameter handles */ parameters_init(&ekf_param_handles); @@ -508,8 +510,25 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds continue; } - attitudeKalmanfilter(update_vect, dt, z_k, x_aposteriori_k, P_aposteriori_k, ekf_params.q, ekf_params.r, - euler, Rot_matrix, x_aposteriori, P_aposteriori); + /* Call the estimator */ + AttitudeEKF(false, // approx_prediction + false, // use_inertia_matrix + update_vect, + dt, + z_k, + ekf_params.q[0], // q_rotSpeed, + ekf_params.q[1], // q_rotAcc + ekf_params.q[2], // q_acc + ekf_params.q[3], // q_mag + ekf_params.r[0], // r_gyro + ekf_params.r[1], // r_accel + ekf_params.r[2], // r_mag + ekf_params.moment_inertia_J, + x_aposteriori, + P_aposteriori, + Rot_matrix, + euler, + debugOutput); /* swap values for next iteration, check for fatal inputs */ if (isfinite(euler[0]) && isfinite(euler[1]) && isfinite(euler[2])) { diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c index bc0e3b93a2..5b57bfb4db 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c @@ -50,7 +50,6 @@ PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q1, 0.08f); PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q2, 0.009f); /* gyro offsets process noise */ PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q3, 0.005f); -PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q4, 0.0f); /* gyro measurement noise */ PARAM_DEFINE_FLOAT(EKF_ATT_V4_R0, 0.0008f); @@ -58,14 +57,38 @@ PARAM_DEFINE_FLOAT(EKF_ATT_V4_R0, 0.0008f); PARAM_DEFINE_FLOAT(EKF_ATT_V4_R1, 10000.0f); /* mag measurement noise */ PARAM_DEFINE_FLOAT(EKF_ATT_V4_R2, 100.0f); -/* offset estimation - UNUSED */ -PARAM_DEFINE_FLOAT(EKF_ATT_V4_R3, 0.0f); /* magnetic declination, in degrees */ PARAM_DEFINE_FLOAT(ATT_MAG_DECL, 0.0f); PARAM_DEFINE_INT32(ATT_ACC_COMP, 2); +/* + * Moment of inertia matrix diagonal entry (1, 1) + * + * @group attitude_ekf + * @unit kg*m^2 + */ +PARAM_DEFINE_FLOAT(ATT_J11, 0.0018); + +/* + * Moment of inertia matrix diagonal entry (2, 2) + * + * @group attitude_ekf + * @unit kg*m^2 + */ +PARAM_DEFINE_FLOAT(ATT_J22, 0.0018); + + +/* + * Moment of inertia matrix diagonal entry (3, 3) + * + * @group attitude_ekf + * @unit kg*m^2 + */ +PARAM_DEFINE_FLOAT(ATT_J33, 0.0037); + + int parameters_init(struct attitude_estimator_ekf_param_handles *h) { /* PID parameters */ @@ -73,17 +96,19 @@ int parameters_init(struct attitude_estimator_ekf_param_handles *h) h->q1 = param_find("EKF_ATT_V3_Q1"); h->q2 = param_find("EKF_ATT_V3_Q2"); h->q3 = param_find("EKF_ATT_V3_Q3"); - h->q4 = param_find("EKF_ATT_V3_Q4"); h->r0 = param_find("EKF_ATT_V4_R0"); h->r1 = param_find("EKF_ATT_V4_R1"); h->r2 = param_find("EKF_ATT_V4_R2"); - h->r3 = param_find("EKF_ATT_V4_R3"); h->mag_decl = param_find("ATT_MAG_DECL"); h->acc_comp = param_find("ATT_ACC_COMP"); + h->moment_inertia_J[0] = param_find("ATT_J11"); + h->moment_inertia_J[1] = param_find("ATT_J22"); + h->moment_inertia_J[2] = param_find("ATT_J33"); + return OK; } @@ -93,17 +118,19 @@ int parameters_update(const struct attitude_estimator_ekf_param_handles *h, stru param_get(h->q1, &(p->q[1])); param_get(h->q2, &(p->q[2])); param_get(h->q3, &(p->q[3])); - param_get(h->q4, &(p->q[4])); param_get(h->r0, &(p->r[0])); param_get(h->r1, &(p->r[1])); param_get(h->r2, &(p->r[2])); - param_get(h->r3, &(p->r[3])); param_get(h->mag_decl, &(p->mag_decl)); p->mag_decl *= M_PI_F / 180.0f; param_get(h->acc_comp, &(p->acc_comp)); + for (int i = 0; i < 3; i++) { + param_get(h->moment_inertia_J[i], &(p->moment_inertia_J[3 * i + i])); + } + return OK; } diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h index 5985541cac..fbb6a18ff4 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h @@ -42,8 +42,9 @@ #include struct attitude_estimator_ekf_params { - float r[9]; - float q[12]; + float r[3]; + float q[4]; + float moment_inertia_J[9]; float roll_off; float pitch_off; float yaw_off; @@ -52,8 +53,9 @@ struct attitude_estimator_ekf_params { }; struct attitude_estimator_ekf_param_handles { - param_t r0, r1, r2, r3; - param_t q0, q1, q2, q3, q4; + param_t r0, r1, r2; + param_t q0, q1, q2, q3; + param_t moment_inertia_J[3]; /**< diagonal entries of the matrix */ param_t mag_decl; param_t acc_comp; }; diff --git a/src/modules/attitude_estimator_ekf/codegen/AttitudeEKF.c b/src/modules/attitude_estimator_ekf/codegen/AttitudeEKF.c new file mode 100644 index 0000000000..d568e57376 --- /dev/null +++ b/src/modules/attitude_estimator_ekf/codegen/AttitudeEKF.c @@ -0,0 +1,1573 @@ +/* + * AttitudeEKF.c + * + * Code generation for function 'AttitudeEKF' + * + * C source code generated on: Fri Jul 25 14:06:41 2014 + * + */ + +/* Include files */ +#include "AttitudeEKF.h" + +/* Variable Definitions */ +static float Ji[9]; +static boolean_T Ji_not_empty; +static float x_apo[12]; +static float P_apo[144]; +static float Q[144]; +static boolean_T Q_not_empty; + +/* Function Declarations */ +static void AttitudeEKF_init(void); +static void b_mrdivide(const float A[72], const float B[36], float y[72]); +static void inv(const float x[9], float y[9]); +static void mrdivide(const float A[108], const float B[81], float y[108]); +static float norm(const float x[3]); + +/* Function Definitions */ +static void AttitudeEKF_init(void) +{ + int i; + static const float fv5[12] = { 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, + -9.81F, 1.0F, 0.0F, 0.0F }; + + for (i = 0; i < 12; i++) { + x_apo[i] = fv5[i]; + } + + for (i = 0; i < 144; i++) { + P_apo[i] = 200.0F; + } +} + +/* + * + */ +static void b_mrdivide(const float A[72], const float B[36], float y[72]) +{ + float b_A[36]; + signed char ipiv[6]; + int i1; + int iy; + int j; + int c; + int ix; + float temp; + int k; + float s; + int jy; + int ijA; + float Y[72]; + for (i1 = 0; i1 < 6; i1++) { + for (iy = 0; iy < 6; iy++) { + b_A[iy + 6 * i1] = B[i1 + 6 * iy]; + } + + ipiv[i1] = (signed char)(1 + i1); + } + + for (j = 0; j < 5; j++) { + c = j * 7; + iy = 0; + ix = c; + temp = (real32_T)fabs(b_A[c]); + for (k = 2; k <= 6 - j; k++) { + ix++; + s = (real32_T)fabs(b_A[ix]); + if (s > temp) { + iy = k - 1; + temp = s; + } + } + + if (b_A[c + iy] != 0.0F) { + if (iy != 0) { + ipiv[j] = (signed char)((j + iy) + 1); + ix = j; + iy += j; + for (k = 0; k < 6; k++) { + temp = b_A[ix]; + b_A[ix] = b_A[iy]; + b_A[iy] = temp; + ix += 6; + iy += 6; + } + } + + i1 = (c - j) + 6; + for (jy = c + 1; jy + 1 <= i1; jy++) { + b_A[jy] /= b_A[c]; + } + } + + iy = c; + jy = c + 6; + for (k = 1; k <= 5 - j; k++) { + temp = b_A[jy]; + if (b_A[jy] != 0.0F) { + ix = c + 1; + i1 = (iy - j) + 12; + for (ijA = 7 + iy; ijA + 1 <= i1; ijA++) { + b_A[ijA] += b_A[ix] * -temp; + ix++; + } + } + + jy += 6; + iy += 6; + } + } + + for (i1 = 0; i1 < 12; i1++) { + for (iy = 0; iy < 6; iy++) { + Y[iy + 6 * i1] = A[i1 + 12 * iy]; + } + } + + for (jy = 0; jy < 6; jy++) { + if (ipiv[jy] != jy + 1) { + for (j = 0; j < 12; j++) { + temp = Y[jy + 6 * j]; + Y[jy + 6 * j] = Y[(ipiv[jy] + 6 * j) - 1]; + Y[(ipiv[jy] + 6 * j) - 1] = temp; + } + } + } + + for (j = 0; j < 12; j++) { + c = 6 * j; + for (k = 0; k < 6; k++) { + iy = 6 * k; + if (Y[k + c] != 0.0F) { + for (jy = k + 2; jy < 7; jy++) { + Y[(jy + c) - 1] -= Y[k + c] * b_A[(jy + iy) - 1]; + } + } + } + } + + for (j = 0; j < 12; j++) { + c = 6 * j; + for (k = 5; k > -1; k += -1) { + iy = 6 * k; + if (Y[k + c] != 0.0F) { + Y[k + c] /= b_A[k + iy]; + for (jy = 0; jy + 1 <= k; jy++) { + Y[jy + c] -= Y[k + c] * b_A[jy + iy]; + } + } + } + } + + for (i1 = 0; i1 < 6; i1++) { + for (iy = 0; iy < 12; iy++) { + y[iy + 12 * i1] = Y[i1 + 6 * iy]; + } + } +} + +/* + * + */ +static void inv(const float x[9], float y[9]) +{ + float b_x[9]; + int p1; + int p2; + int p3; + float absx11; + float absx21; + float absx31; + int itmp; + for (p1 = 0; p1 < 9; p1++) { + b_x[p1] = x[p1]; + } + + p1 = 0; + p2 = 3; + p3 = 6; + absx11 = (real32_T)fabs(x[0]); + absx21 = (real32_T)fabs(x[1]); + absx31 = (real32_T)fabs(x[2]); + if ((absx21 > absx11) && (absx21 > absx31)) { + p1 = 3; + p2 = 0; + b_x[0] = x[1]; + b_x[1] = x[0]; + b_x[3] = x[4]; + b_x[4] = x[3]; + b_x[6] = x[7]; + b_x[7] = x[6]; + } else { + if (absx31 > absx11) { + p1 = 6; + p3 = 0; + b_x[0] = x[2]; + b_x[2] = x[0]; + b_x[3] = x[5]; + b_x[5] = x[3]; + b_x[6] = x[8]; + b_x[8] = x[6]; + } + } + + absx11 = b_x[1] / b_x[0]; + b_x[1] /= b_x[0]; + absx21 = b_x[2] / b_x[0]; + b_x[2] /= b_x[0]; + b_x[4] -= absx11 * b_x[3]; + b_x[5] -= absx21 * b_x[3]; + b_x[7] -= absx11 * b_x[6]; + b_x[8] -= absx21 * b_x[6]; + if ((real32_T)fabs(b_x[5]) > (real32_T)fabs(b_x[4])) { + itmp = p2; + p2 = p3; + p3 = itmp; + b_x[1] = absx21; + b_x[2] = absx11; + absx11 = b_x[4]; + b_x[4] = b_x[5]; + b_x[5] = absx11; + absx11 = b_x[7]; + b_x[7] = b_x[8]; + b_x[8] = absx11; + } + + absx11 = b_x[5] / b_x[4]; + b_x[5] /= b_x[4]; + b_x[8] -= absx11 * b_x[7]; + absx11 = (b_x[5] * b_x[1] - b_x[2]) / b_x[8]; + absx21 = -(b_x[1] + b_x[7] * absx11) / b_x[4]; + y[p1] = ((1.0F - b_x[3] * absx21) - b_x[6] * absx11) / b_x[0]; + y[p1 + 1] = absx21; + y[p1 + 2] = absx11; + absx11 = -b_x[5] / b_x[8]; + absx21 = (1.0F - b_x[7] * absx11) / b_x[4]; + y[p2] = -(b_x[3] * absx21 + b_x[6] * absx11) / b_x[0]; + y[p2 + 1] = absx21; + y[p2 + 2] = absx11; + absx11 = 1.0F / b_x[8]; + absx21 = -b_x[7] * absx11 / b_x[4]; + y[p3] = -(b_x[3] * absx21 + b_x[6] * absx11) / b_x[0]; + y[p3 + 1] = absx21; + y[p3 + 2] = absx11; +} + +/* + * + */ +static void mrdivide(const float A[108], const float B[81], float y[108]) +{ + float b_A[81]; + signed char ipiv[9]; + int i0; + int iy; + int j; + int c; + int ix; + float temp; + int k; + float s; + int jy; + int ijA; + float Y[108]; + for (i0 = 0; i0 < 9; i0++) { + for (iy = 0; iy < 9; iy++) { + b_A[iy + 9 * i0] = B[i0 + 9 * iy]; + } + + ipiv[i0] = (signed char)(1 + i0); + } + + for (j = 0; j < 8; j++) { + c = j * 10; + iy = 0; + ix = c; + temp = (real32_T)fabs(b_A[c]); + for (k = 2; k <= 9 - j; k++) { + ix++; + s = (real32_T)fabs(b_A[ix]); + if (s > temp) { + iy = k - 1; + temp = s; + } + } + + if (b_A[c + iy] != 0.0F) { + if (iy != 0) { + ipiv[j] = (signed char)((j + iy) + 1); + ix = j; + iy += j; + for (k = 0; k < 9; k++) { + temp = b_A[ix]; + b_A[ix] = b_A[iy]; + b_A[iy] = temp; + ix += 9; + iy += 9; + } + } + + i0 = (c - j) + 9; + for (jy = c + 1; jy + 1 <= i0; jy++) { + b_A[jy] /= b_A[c]; + } + } + + iy = c; + jy = c + 9; + for (k = 1; k <= 8 - j; k++) { + temp = b_A[jy]; + if (b_A[jy] != 0.0F) { + ix = c + 1; + i0 = (iy - j) + 18; + for (ijA = 10 + iy; ijA + 1 <= i0; ijA++) { + b_A[ijA] += b_A[ix] * -temp; + ix++; + } + } + + jy += 9; + iy += 9; + } + } + + for (i0 = 0; i0 < 12; i0++) { + for (iy = 0; iy < 9; iy++) { + Y[iy + 9 * i0] = A[i0 + 12 * iy]; + } + } + + for (jy = 0; jy < 9; jy++) { + if (ipiv[jy] != jy + 1) { + for (j = 0; j < 12; j++) { + temp = Y[jy + 9 * j]; + Y[jy + 9 * j] = Y[(ipiv[jy] + 9 * j) - 1]; + Y[(ipiv[jy] + 9 * j) - 1] = temp; + } + } + } + + for (j = 0; j < 12; j++) { + c = 9 * j; + for (k = 0; k < 9; k++) { + iy = 9 * k; + if (Y[k + c] != 0.0F) { + for (jy = k + 2; jy < 10; jy++) { + Y[(jy + c) - 1] -= Y[k + c] * b_A[(jy + iy) - 1]; + } + } + } + } + + for (j = 0; j < 12; j++) { + c = 9 * j; + for (k = 8; k > -1; k += -1) { + iy = 9 * k; + if (Y[k + c] != 0.0F) { + Y[k + c] /= b_A[k + iy]; + for (jy = 0; jy + 1 <= k; jy++) { + Y[jy + c] -= Y[k + c] * b_A[jy + iy]; + } + } + } + } + + for (i0 = 0; i0 < 9; i0++) { + for (iy = 0; iy < 12; iy++) { + y[iy + 12 * i0] = Y[i0 + 9 * iy]; + } + } +} + +/* + * + */ +static float norm(const float x[3]) +{ + float y; + float scale; + int k; + float absxk; + float t; + y = 0.0F; + scale = 1.17549435E-38F; + for (k = 0; k < 3; k++) { + absxk = (real32_T)fabs(x[k]); + if (absxk > scale) { + t = scale / absxk; + y = 1.0F + y * t * t; + scale = absxk; + } else { + t = absxk / scale; + y += t * t; + } + } + + return scale * (real32_T)sqrt(y); +} + +/* + * function [xa_apo,Pa_apo,Rot_matrix,eulerAngles,debugOutput]... + * = AttitudeEKF(approx_prediction,use_inertia_matrix,zFlag,dt,z,q_rotSpeed,q_rotAcc,q_acc,q_mag,r_gyro,r_accel,r_mag,J) + */ +void AttitudeEKF(unsigned char approx_prediction, unsigned char + use_inertia_matrix, const unsigned char zFlag[3], float dt, + const float z[9], float q_rotSpeed, float q_rotAcc, float q_acc, + float q_mag, float r_gyro, float r_accel, float r_mag, const + float J[9], float xa_apo[12], float Pa_apo[144], float + Rot_matrix[9], float eulerAngles[3], float debugOutput[4]) +{ + int i; + float fv0[3]; + int r2; + float zek[3]; + float muk[3]; + float b_muk[3]; + float c_muk[3]; + float fv1[3]; + float wak[3]; + float O[9]; + float b_O[9]; + static const signed char iv0[9] = { 1, 0, 0, 0, 1, 0, 0, 0, 1 }; + + float fv2[3]; + float maxval; + int r1; + float fv3[9]; + float fv4[3]; + float x_apr[12]; + signed char I[144]; + float A_lin[144]; + static const signed char iv1[36] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 1, + 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; + + float b_A_lin[144]; + float v[12]; + float P_apr[144]; + float b_P_apr[108]; + static const signed char b[108] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 }; + + float K_k[108]; + float a[81]; + static const signed char b_a[108] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 }; + + float b_r_gyro[81]; + float c_a[81]; + float d_a[36]; + static const signed char e_a[36] = { 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; + + static const signed char b_b[36] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; + + float c_r_gyro[9]; + float b_K_k[36]; + int r3; + float a21; + float f_a[36]; + float c_P_apr[72]; + static const signed char c_b[72] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 1, 0, 0, 0 }; + + float c_K_k[72]; + static const signed char g_a[72] = { 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, + 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, + 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0 }; + + float b_z[6]; + static const signed char d_b[72] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 1 }; + + static const signed char h_a[72] = { 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, + 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, + 0, 0, 0, 0, 0, 1 }; + + float i_a[6]; + float c_z[6]; + + /* LQG Postion Estimator and Controller */ + /* Observer: */ + /* x[n|n] = x[n|n-1] + M(y[n] - Cx[n|n-1] - Du[n]) */ + /* x[n+1|n] = Ax[n|n] + Bu[n] */ + /* */ + /* $Author: Tobias Naegeli $ $Date: 2014 $ $Revision: 3 $ */ + /* */ + /* */ + /* Arguments: */ + /* approx_prediction: if 1 then the exponential map is approximated with a */ + /* first order taylor approximation. has at the moment not a big influence */ + /* (just 1st or 2nd order approximation) we should change it to rodriquez */ + /* approximation. */ + /* use_inertia_matrix: set to true if you have the inertia matrix J for your */ + /* quadrotor */ + /* xa_apo_k: old state vectotr */ + /* zFlag: if sensor measurement is available [gyro, acc, mag] */ + /* dt: dt in s */ + /* z: measurements [gyro, acc, mag] */ + /* q_rotSpeed: process noise gyro */ + /* q_rotAcc: process noise gyro acceleration */ + /* q_acc: process noise acceleration */ + /* q_mag: process noise magnetometer */ + /* r_gyro: measurement noise gyro */ + /* r_accel: measurement noise accel */ + /* r_mag: measurement noise mag */ + /* J: moment of inertia matrix */ + /* Output: */ + /* xa_apo: updated state vectotr */ + /* Pa_apo: updated state covariance matrix */ + /* Rot_matrix: rotation matrix */ + /* eulerAngles: euler angles */ + /* debugOutput: not used */ + /* % model specific parameters */ + /* compute once the inverse of the Inertia */ + /* 'AttitudeEKF:48' if isempty(Ji) */ + if (!Ji_not_empty) { + /* 'AttitudeEKF:49' Ji=single(inv(J)); */ + inv(J, Ji); + Ji_not_empty = TRUE; + } + + /* % init */ + /* 'AttitudeEKF:54' if(isempty(x_apo)) */ + /* 'AttitudeEKF:64' if(isempty(P_apo)) */ + /* 'AttitudeEKF:69' debugOutput = single(zeros(4,1)); */ + for (i = 0; i < 4; i++) { + debugOutput[i] = 0.0F; + } + + /* % copy the states */ + /* 'AttitudeEKF:72' wx= x_apo(1); */ + /* x body angular rate */ + /* 'AttitudeEKF:73' wy= x_apo(2); */ + /* y body angular rate */ + /* 'AttitudeEKF:74' wz= x_apo(3); */ + /* z body angular rate */ + /* 'AttitudeEKF:76' wax= x_apo(4); */ + /* x body angular acceleration */ + /* 'AttitudeEKF:77' way= x_apo(5); */ + /* y body angular acceleration */ + /* 'AttitudeEKF:78' waz= x_apo(6); */ + /* z body angular acceleration */ + /* 'AttitudeEKF:80' zex= x_apo(7); */ + /* x component gravity vector */ + /* 'AttitudeEKF:81' zey= x_apo(8); */ + /* y component gravity vector */ + /* 'AttitudeEKF:82' zez= x_apo(9); */ + /* z component gravity vector */ + /* 'AttitudeEKF:84' mux= x_apo(10); */ + /* x component magnetic field vector */ + /* 'AttitudeEKF:85' muy= x_apo(11); */ + /* y component magnetic field vector */ + /* 'AttitudeEKF:86' muz= x_apo(12); */ + /* z component magnetic field vector */ + /* % prediction section */ + /* compute the apriori state estimate from the previous aposteriori estimate */ + /* body angular accelerations */ + /* 'AttitudeEKF:94' if (use_inertia_matrix==1) */ + if (use_inertia_matrix == 1) { + /* 'AttitudeEKF:95' wak =[wax;way;waz]+Ji*(-cross([wax;way;waz],J*[wax;way;waz]))*dt; */ + fv0[0] = x_apo[3]; + fv0[1] = x_apo[4]; + fv0[2] = x_apo[5]; + for (r2 = 0; r2 < 3; r2++) { + zek[r2] = 0.0F; + for (i = 0; i < 3; i++) { + zek[r2] += J[r2 + 3 * i] * fv0[i]; + } + } + + muk[0] = x_apo[3]; + muk[1] = x_apo[4]; + muk[2] = x_apo[5]; + b_muk[0] = x_apo[4] * zek[2] - x_apo[5] * zek[1]; + b_muk[1] = x_apo[5] * zek[0] - x_apo[3] * zek[2]; + b_muk[2] = x_apo[3] * zek[1] - x_apo[4] * zek[0]; + for (r2 = 0; r2 < 3; r2++) { + c_muk[r2] = -b_muk[r2]; + } + + fv1[0] = x_apo[3]; + fv1[1] = x_apo[4]; + fv1[2] = x_apo[5]; + for (r2 = 0; r2 < 3; r2++) { + fv0[r2] = 0.0F; + for (i = 0; i < 3; i++) { + fv0[r2] += Ji[r2 + 3 * i] * c_muk[i]; + } + + wak[r2] = fv1[r2] + fv0[r2] * dt; + } + } else { + /* 'AttitudeEKF:96' else */ + /* 'AttitudeEKF:97' wak =[wax;way;waz]; */ + wak[0] = x_apo[3]; + wak[1] = x_apo[4]; + wak[2] = x_apo[5]; + } + + /* body angular rates */ + /* 'AttitudeEKF:101' wk =[wx; wy; wz] + dt*wak; */ + /* derivative of the prediction rotation matrix */ + /* 'AttitudeEKF:104' O=[0,-wz,wy;wz,0,-wx;-wy,wx,0]'; */ + O[0] = 0.0F; + O[1] = -x_apo[2]; + O[2] = x_apo[1]; + O[3] = x_apo[2]; + O[4] = 0.0F; + O[5] = -x_apo[0]; + O[6] = -x_apo[1]; + O[7] = x_apo[0]; + O[8] = 0.0F; + + /* prediction of the earth z vector */ + /* 'AttitudeEKF:107' if (approx_prediction==1) */ + if (approx_prediction == 1) { + /* e^(Odt)=I+dt*O+dt^2/2!O^2 */ + /* so we do a first order approximation of the exponential map */ + /* 'AttitudeEKF:110' zek =(O*dt+single(eye(3)))*[zex;zey;zez]; */ + for (r2 = 0; r2 < 3; r2++) { + for (i = 0; i < 3; i++) { + b_O[i + 3 * r2] = O[i + 3 * r2] * dt + (float)iv0[i + 3 * r2]; + } + } + + fv2[0] = x_apo[6]; + fv2[1] = x_apo[7]; + fv2[2] = x_apo[8]; + for (r2 = 0; r2 < 3; r2++) { + zek[r2] = 0.0F; + for (i = 0; i < 3; i++) { + zek[r2] += b_O[r2 + 3 * i] * fv2[i]; + } + } + } else { + /* 'AttitudeEKF:112' else */ + /* 'AttitudeEKF:113' zek =(single(eye(3))+O*dt+dt^2/2*O^2)*[zex;zey;zez]; */ + maxval = dt * dt / 2.0F; + for (r2 = 0; r2 < 3; r2++) { + for (i = 0; i < 3; i++) { + b_O[r2 + 3 * i] = 0.0F; + for (r1 = 0; r1 < 3; r1++) { + b_O[r2 + 3 * i] += O[r2 + 3 * r1] * O[r1 + 3 * i]; + } + } + } + + for (r2 = 0; r2 < 3; r2++) { + for (i = 0; i < 3; i++) { + fv3[i + 3 * r2] = ((float)iv0[i + 3 * r2] + O[i + 3 * r2] * dt) + maxval + * b_O[i + 3 * r2]; + } + } + + fv2[0] = x_apo[6]; + fv2[1] = x_apo[7]; + fv2[2] = x_apo[8]; + for (r2 = 0; r2 < 3; r2++) { + zek[r2] = 0.0F; + for (i = 0; i < 3; i++) { + zek[r2] += fv3[r2 + 3 * i] * fv2[i]; + } + } + + /* zek =expm2(O*dt)*[zex;zey;zez]; not working because use double */ + /* precision */ + } + + /* prediction of the magnetic vector */ + /* 'AttitudeEKF:121' if (approx_prediction==1) */ + if (approx_prediction == 1) { + /* e^(Odt)=I+dt*O+dt^2/2!O^2 */ + /* so we do a first order approximation of the exponential map */ + /* 'AttitudeEKF:124' muk =(O*dt+single(eye(3)))*[mux;muy;muz]; */ + for (r2 = 0; r2 < 3; r2++) { + for (i = 0; i < 3; i++) { + b_O[i + 3 * r2] = O[i + 3 * r2] * dt + (float)iv0[i + 3 * r2]; + } + } + + fv4[0] = x_apo[9]; + fv4[1] = x_apo[10]; + fv4[2] = x_apo[11]; + for (r2 = 0; r2 < 3; r2++) { + muk[r2] = 0.0F; + for (i = 0; i < 3; i++) { + muk[r2] += b_O[r2 + 3 * i] * fv4[i]; + } + } + } else { + /* 'AttitudeEKF:125' else */ + /* 'AttitudeEKF:126' muk =(single(eye(3))+O*dt+dt^2/2*O^2)*[mux;muy;muz]; */ + maxval = dt * dt / 2.0F; + for (r2 = 0; r2 < 3; r2++) { + for (i = 0; i < 3; i++) { + b_O[r2 + 3 * i] = 0.0F; + for (r1 = 0; r1 < 3; r1++) { + b_O[r2 + 3 * i] += O[r2 + 3 * r1] * O[r1 + 3 * i]; + } + } + } + + for (r2 = 0; r2 < 3; r2++) { + for (i = 0; i < 3; i++) { + fv3[i + 3 * r2] = ((float)iv0[i + 3 * r2] + O[i + 3 * r2] * dt) + maxval + * b_O[i + 3 * r2]; + } + } + + fv4[0] = x_apo[9]; + fv4[1] = x_apo[10]; + fv4[2] = x_apo[11]; + for (r2 = 0; r2 < 3; r2++) { + muk[r2] = 0.0F; + for (i = 0; i < 3; i++) { + muk[r2] += fv3[r2 + 3 * i] * fv4[i]; + } + } + + /* muk =expm2(O*dt)*[mux;muy;muz]; not working because use double */ + /* precision */ + } + + /* 'AttitudeEKF:131' x_apr=[wk;wak;zek;muk]; */ + x_apr[0] = x_apo[0] + dt * wak[0]; + x_apr[1] = x_apo[1] + dt * wak[1]; + x_apr[2] = x_apo[2] + dt * wak[2]; + for (i = 0; i < 3; i++) { + x_apr[i + 3] = wak[i]; + } + + for (i = 0; i < 3; i++) { + x_apr[i + 6] = zek[i]; + } + + for (i = 0; i < 3; i++) { + x_apr[i + 9] = muk[i]; + } + + /* compute the apriori error covariance estimate from the previous */ + /* aposteriori estimate */ + /* 'AttitudeEKF:136' EZ=[0,zez,-zey; */ + /* 'AttitudeEKF:137' -zez,0,zex; */ + /* 'AttitudeEKF:138' zey,-zex,0]'; */ + /* 'AttitudeEKF:139' MA=[0,muz,-muy; */ + /* 'AttitudeEKF:140' -muz,0,mux; */ + /* 'AttitudeEKF:141' muy,-mux,0]'; */ + /* 'AttitudeEKF:143' E=single(eye(3)); */ + /* 'AttitudeEKF:144' Z=single(zeros(3)); */ + /* 'AttitudeEKF:146' A_lin=[ Z, E, Z, Z */ + /* 'AttitudeEKF:147' Z, Z, Z, Z */ + /* 'AttitudeEKF:148' EZ, Z, O, Z */ + /* 'AttitudeEKF:149' MA, Z, Z, O]; */ + /* 'AttitudeEKF:151' A_lin=eye(12)+A_lin*dt; */ + memset(&I[0], 0, 144U * sizeof(signed char)); + for (i = 0; i < 12; i++) { + I[i + 12 * i] = 1; + for (r2 = 0; r2 < 3; r2++) { + A_lin[r2 + 12 * i] = iv1[r2 + 3 * i]; + } + + for (r2 = 0; r2 < 3; r2++) { + A_lin[(r2 + 12 * i) + 3] = 0.0F; + } + } + + A_lin[6] = 0.0F; + A_lin[7] = x_apo[8]; + A_lin[8] = -x_apo[7]; + A_lin[18] = -x_apo[8]; + A_lin[19] = 0.0F; + A_lin[20] = x_apo[6]; + A_lin[30] = x_apo[7]; + A_lin[31] = -x_apo[6]; + A_lin[32] = 0.0F; + for (r2 = 0; r2 < 3; r2++) { + for (i = 0; i < 3; i++) { + A_lin[(i + 12 * (r2 + 3)) + 6] = 0.0F; + } + } + + for (r2 = 0; r2 < 3; r2++) { + for (i = 0; i < 3; i++) { + A_lin[(i + 12 * (r2 + 6)) + 6] = O[i + 3 * r2]; + } + } + + for (r2 = 0; r2 < 3; r2++) { + for (i = 0; i < 3; i++) { + A_lin[(i + 12 * (r2 + 9)) + 6] = 0.0F; + } + } + + A_lin[9] = 0.0F; + A_lin[10] = x_apo[11]; + A_lin[11] = -x_apo[10]; + A_lin[21] = -x_apo[11]; + A_lin[22] = 0.0F; + A_lin[23] = x_apo[9]; + A_lin[33] = x_apo[10]; + A_lin[34] = -x_apo[9]; + A_lin[35] = 0.0F; + for (r2 = 0; r2 < 3; r2++) { + for (i = 0; i < 3; i++) { + A_lin[(i + 12 * (r2 + 3)) + 9] = 0.0F; + } + } + + for (r2 = 0; r2 < 3; r2++) { + for (i = 0; i < 3; i++) { + A_lin[(i + 12 * (r2 + 6)) + 9] = 0.0F; + } + } + + for (r2 = 0; r2 < 3; r2++) { + for (i = 0; i < 3; i++) { + A_lin[(i + 12 * (r2 + 9)) + 9] = O[i + 3 * r2]; + } + } + + for (r2 = 0; r2 < 12; r2++) { + for (i = 0; i < 12; i++) { + b_A_lin[i + 12 * r2] = (float)I[i + 12 * r2] + A_lin[i + 12 * r2] * dt; + } + } + + /* process covariance matrix */ + /* 'AttitudeEKF:156' if (isempty(Q)) */ + if (!Q_not_empty) { + /* 'AttitudeEKF:157' Q=diag([ q_rotSpeed,q_rotSpeed,q_rotSpeed,... */ + /* 'AttitudeEKF:158' q_rotAcc,q_rotAcc,q_rotAcc,... */ + /* 'AttitudeEKF:159' q_acc,q_acc,q_acc,... */ + /* 'AttitudeEKF:160' q_mag,q_mag,q_mag]); */ + v[0] = q_rotSpeed; + v[1] = q_rotSpeed; + v[2] = q_rotSpeed; + v[3] = q_rotAcc; + v[4] = q_rotAcc; + v[5] = q_rotAcc; + v[6] = q_acc; + v[7] = q_acc; + v[8] = q_acc; + v[9] = q_mag; + v[10] = q_mag; + v[11] = q_mag; + memset(&Q[0], 0, 144U * sizeof(float)); + for (i = 0; i < 12; i++) { + Q[i + 12 * i] = v[i]; + } + + Q_not_empty = TRUE; + } + + /* 'AttitudeEKF:163' P_apr=A_lin*P_apo*A_lin'+Q; */ + for (r2 = 0; r2 < 12; r2++) { + for (i = 0; i < 12; i++) { + A_lin[r2 + 12 * i] = 0.0F; + for (r1 = 0; r1 < 12; r1++) { + A_lin[r2 + 12 * i] += b_A_lin[r2 + 12 * r1] * P_apo[r1 + 12 * i]; + } + } + } + + for (r2 = 0; r2 < 12; r2++) { + for (i = 0; i < 12; i++) { + maxval = 0.0F; + for (r1 = 0; r1 < 12; r1++) { + maxval += A_lin[r2 + 12 * r1] * b_A_lin[i + 12 * r1]; + } + + P_apr[r2 + 12 * i] = maxval + Q[r2 + 12 * i]; + } + } + + /* % update */ + /* 'AttitudeEKF:167' if zFlag(1)==1&&zFlag(2)==1&&zFlag(3)==1 */ + if ((zFlag[0] == 1) && (zFlag[1] == 1) && (zFlag[2] == 1)) { + /* 'AttitudeEKF:169' R=[r_gyro,0,0,0,0,0,0,0,0; */ + /* 'AttitudeEKF:170' 0,r_gyro,0,0,0,0,0,0,0; */ + /* 'AttitudeEKF:171' 0,0,r_gyro,0,0,0,0,0,0; */ + /* 'AttitudeEKF:172' 0,0,0,r_accel,0,0,0,0,0; */ + /* 'AttitudeEKF:173' 0,0,0,0,r_accel,0,0,0,0; */ + /* 'AttitudeEKF:174' 0,0,0,0,0,r_accel,0,0,0; */ + /* 'AttitudeEKF:175' 0,0,0,0,0,0,r_mag,0,0; */ + /* 'AttitudeEKF:176' 0,0,0,0,0,0,0,r_mag,0; */ + /* 'AttitudeEKF:177' 0,0,0,0,0,0,0,0,r_mag]; */ + /* observation matrix */ + /* [zw;ze;zmk]; */ + /* 'AttitudeEKF:180' H_k=[ E, Z, Z, Z; */ + /* 'AttitudeEKF:181' Z, Z, E, Z; */ + /* 'AttitudeEKF:182' Z, Z, Z, E]; */ + /* 'AttitudeEKF:184' y_k=z(1:9)-H_k*x_apr; */ + /* 'AttitudeEKF:187' S_k=H_k*P_apr*H_k'+R; */ + /* 'AttitudeEKF:188' K_k=(P_apr*H_k'/(S_k)); */ + for (r2 = 0; r2 < 12; r2++) { + for (i = 0; i < 9; i++) { + b_P_apr[r2 + 12 * i] = 0.0F; + for (r1 = 0; r1 < 12; r1++) { + b_P_apr[r2 + 12 * i] += P_apr[r2 + 12 * r1] * (float)b[r1 + 12 * i]; + } + } + } + + for (r2 = 0; r2 < 9; r2++) { + for (i = 0; i < 12; i++) { + K_k[r2 + 9 * i] = 0.0F; + for (r1 = 0; r1 < 12; r1++) { + K_k[r2 + 9 * i] += (float)b_a[r2 + 9 * r1] * P_apr[r1 + 12 * i]; + } + } + + for (i = 0; i < 9; i++) { + a[r2 + 9 * i] = 0.0F; + for (r1 = 0; r1 < 12; r1++) { + a[r2 + 9 * i] += K_k[r2 + 9 * r1] * (float)b[r1 + 12 * i]; + } + } + } + + b_r_gyro[0] = r_gyro; + b_r_gyro[9] = 0.0F; + b_r_gyro[18] = 0.0F; + b_r_gyro[27] = 0.0F; + b_r_gyro[36] = 0.0F; + b_r_gyro[45] = 0.0F; + b_r_gyro[54] = 0.0F; + b_r_gyro[63] = 0.0F; + b_r_gyro[72] = 0.0F; + b_r_gyro[1] = 0.0F; + b_r_gyro[10] = r_gyro; + b_r_gyro[19] = 0.0F; + b_r_gyro[28] = 0.0F; + b_r_gyro[37] = 0.0F; + b_r_gyro[46] = 0.0F; + b_r_gyro[55] = 0.0F; + b_r_gyro[64] = 0.0F; + b_r_gyro[73] = 0.0F; + b_r_gyro[2] = 0.0F; + b_r_gyro[11] = 0.0F; + b_r_gyro[20] = r_gyro; + b_r_gyro[29] = 0.0F; + b_r_gyro[38] = 0.0F; + b_r_gyro[47] = 0.0F; + b_r_gyro[56] = 0.0F; + b_r_gyro[65] = 0.0F; + b_r_gyro[74] = 0.0F; + b_r_gyro[3] = 0.0F; + b_r_gyro[12] = 0.0F; + b_r_gyro[21] = 0.0F; + b_r_gyro[30] = r_accel; + b_r_gyro[39] = 0.0F; + b_r_gyro[48] = 0.0F; + b_r_gyro[57] = 0.0F; + b_r_gyro[66] = 0.0F; + b_r_gyro[75] = 0.0F; + b_r_gyro[4] = 0.0F; + b_r_gyro[13] = 0.0F; + b_r_gyro[22] = 0.0F; + b_r_gyro[31] = 0.0F; + b_r_gyro[40] = r_accel; + b_r_gyro[49] = 0.0F; + b_r_gyro[58] = 0.0F; + b_r_gyro[67] = 0.0F; + b_r_gyro[76] = 0.0F; + b_r_gyro[5] = 0.0F; + b_r_gyro[14] = 0.0F; + b_r_gyro[23] = 0.0F; + b_r_gyro[32] = 0.0F; + b_r_gyro[41] = 0.0F; + b_r_gyro[50] = r_accel; + b_r_gyro[59] = 0.0F; + b_r_gyro[68] = 0.0F; + b_r_gyro[77] = 0.0F; + b_r_gyro[6] = 0.0F; + b_r_gyro[15] = 0.0F; + b_r_gyro[24] = 0.0F; + b_r_gyro[33] = 0.0F; + b_r_gyro[42] = 0.0F; + b_r_gyro[51] = 0.0F; + b_r_gyro[60] = r_mag; + b_r_gyro[69] = 0.0F; + b_r_gyro[78] = 0.0F; + b_r_gyro[7] = 0.0F; + b_r_gyro[16] = 0.0F; + b_r_gyro[25] = 0.0F; + b_r_gyro[34] = 0.0F; + b_r_gyro[43] = 0.0F; + b_r_gyro[52] = 0.0F; + b_r_gyro[61] = 0.0F; + b_r_gyro[70] = r_mag; + b_r_gyro[79] = 0.0F; + b_r_gyro[8] = 0.0F; + b_r_gyro[17] = 0.0F; + b_r_gyro[26] = 0.0F; + b_r_gyro[35] = 0.0F; + b_r_gyro[44] = 0.0F; + b_r_gyro[53] = 0.0F; + b_r_gyro[62] = 0.0F; + b_r_gyro[71] = 0.0F; + b_r_gyro[80] = r_mag; + for (r2 = 0; r2 < 9; r2++) { + for (i = 0; i < 9; i++) { + c_a[i + 9 * r2] = a[i + 9 * r2] + b_r_gyro[i + 9 * r2]; + } + } + + mrdivide(b_P_apr, c_a, K_k); + + /* 'AttitudeEKF:191' x_apo=x_apr+K_k*y_k; */ + for (r2 = 0; r2 < 9; r2++) { + maxval = 0.0F; + for (i = 0; i < 12; i++) { + maxval += (float)b_a[r2 + 9 * i] * x_apr[i]; + } + + b_O[r2] = z[r2] - maxval; + } + + for (r2 = 0; r2 < 12; r2++) { + maxval = 0.0F; + for (i = 0; i < 9; i++) { + maxval += K_k[r2 + 12 * i] * b_O[i]; + } + + x_apo[r2] = x_apr[r2] + maxval; + } + + /* 'AttitudeEKF:192' P_apo=(eye(12)-K_k*H_k)*P_apr; */ + memset(&I[0], 0, 144U * sizeof(signed char)); + for (i = 0; i < 12; i++) { + I[i + 12 * i] = 1; + } + + for (r2 = 0; r2 < 12; r2++) { + for (i = 0; i < 12; i++) { + maxval = 0.0F; + for (r1 = 0; r1 < 9; r1++) { + maxval += K_k[r2 + 12 * r1] * (float)b_a[r1 + 9 * i]; + } + + A_lin[r2 + 12 * i] = (float)I[r2 + 12 * i] - maxval; + } + } + + for (r2 = 0; r2 < 12; r2++) { + for (i = 0; i < 12; i++) { + P_apo[r2 + 12 * i] = 0.0F; + for (r1 = 0; r1 < 12; r1++) { + P_apo[r2 + 12 * i] += A_lin[r2 + 12 * r1] * P_apr[r1 + 12 * i]; + } + } + } + } else { + /* 'AttitudeEKF:193' else */ + /* 'AttitudeEKF:194' if zFlag(1)==1&&zFlag(2)==0&&zFlag(3)==0 */ + if ((zFlag[0] == 1) && (zFlag[1] == 0) && (zFlag[2] == 0)) { + /* 'AttitudeEKF:196' R=[r_gyro,0,0; */ + /* 'AttitudeEKF:197' 0,r_gyro,0; */ + /* 'AttitudeEKF:198' 0,0,r_gyro]; */ + /* observation matrix */ + /* 'AttitudeEKF:201' H_k=[ E, Z, Z, Z]; */ + /* 'AttitudeEKF:203' y_k=z(1:3)-H_k(1:3,1:12)*x_apr; */ + /* 'AttitudeEKF:205' S_k=H_k(1:3,1:12)*P_apr*H_k(1:3,1:12)'+R(1:3,1:3); */ + /* 'AttitudeEKF:206' K_k=(P_apr*H_k(1:3,1:12)'/(S_k)); */ + for (r2 = 0; r2 < 3; r2++) { + for (i = 0; i < 12; i++) { + d_a[r2 + 3 * i] = 0.0F; + for (r1 = 0; r1 < 12; r1++) { + d_a[r2 + 3 * i] += (float)e_a[r2 + 3 * r1] * P_apr[r1 + 12 * i]; + } + } + } + + for (r2 = 0; r2 < 3; r2++) { + for (i = 0; i < 3; i++) { + b_O[r2 + 3 * i] = 0.0F; + for (r1 = 0; r1 < 12; r1++) { + b_O[r2 + 3 * i] += d_a[i + 3 * r1] * (float)b_b[r1 + 12 * r2]; + } + } + } + + c_r_gyro[0] = r_gyro; + c_r_gyro[1] = 0.0F; + c_r_gyro[2] = 0.0F; + c_r_gyro[3] = 0.0F; + c_r_gyro[4] = r_gyro; + c_r_gyro[5] = 0.0F; + c_r_gyro[6] = 0.0F; + c_r_gyro[7] = 0.0F; + c_r_gyro[8] = r_gyro; + for (r2 = 0; r2 < 3; r2++) { + for (i = 0; i < 3; i++) { + O[i + 3 * r2] = b_O[i + 3 * r2] + c_r_gyro[i + 3 * r2]; + } + + for (i = 0; i < 12; i++) { + b_K_k[r2 + 3 * i] = 0.0F; + for (r1 = 0; r1 < 12; r1++) { + b_K_k[r2 + 3 * i] += P_apr[i + 12 * r1] * (float)b_b[r1 + 12 * r2]; + } + } + } + + r1 = 0; + r2 = 1; + r3 = 2; + maxval = (real32_T)fabs(O[0]); + a21 = (real32_T)fabs(O[1]); + if (a21 > maxval) { + maxval = a21; + r1 = 1; + r2 = 0; + } + + if ((real32_T)fabs(O[2]) > maxval) { + r1 = 2; + r2 = 1; + r3 = 0; + } + + O[r2] /= O[r1]; + O[r3] /= O[r1]; + O[3 + r2] -= O[r2] * O[3 + r1]; + O[3 + r3] -= O[r3] * O[3 + r1]; + O[6 + r2] -= O[r2] * O[6 + r1]; + O[6 + r3] -= O[r3] * O[6 + r1]; + if ((real32_T)fabs(O[3 + r3]) > (real32_T)fabs(O[3 + r2])) { + i = r2; + r2 = r3; + r3 = i; + } + + O[3 + r3] /= O[3 + r2]; + O[6 + r3] -= O[3 + r3] * O[6 + r2]; + for (i = 0; i < 12; i++) { + f_a[3 * i] = b_K_k[r1 + 3 * i]; + f_a[1 + 3 * i] = b_K_k[r2 + 3 * i] - f_a[3 * i] * O[r2]; + f_a[2 + 3 * i] = (b_K_k[r3 + 3 * i] - f_a[3 * i] * O[r3]) - f_a[1 + 3 * + i] * O[3 + r3]; + f_a[2 + 3 * i] /= O[6 + r3]; + f_a[3 * i] -= f_a[2 + 3 * i] * O[6 + r1]; + f_a[1 + 3 * i] -= f_a[2 + 3 * i] * O[6 + r2]; + f_a[1 + 3 * i] /= O[3 + r2]; + f_a[3 * i] -= f_a[1 + 3 * i] * O[3 + r1]; + f_a[3 * i] /= O[r1]; + } + + for (r2 = 0; r2 < 3; r2++) { + for (i = 0; i < 12; i++) { + b_K_k[i + 12 * r2] = f_a[r2 + 3 * i]; + } + } + + /* 'AttitudeEKF:209' x_apo=x_apr+K_k*y_k; */ + for (r2 = 0; r2 < 3; r2++) { + maxval = 0.0F; + for (i = 0; i < 12; i++) { + maxval += (float)e_a[r2 + 3 * i] * x_apr[i]; + } + + b_muk[r2] = z[r2] - maxval; + } + + for (r2 = 0; r2 < 12; r2++) { + maxval = 0.0F; + for (i = 0; i < 3; i++) { + maxval += b_K_k[r2 + 12 * i] * b_muk[i]; + } + + x_apo[r2] = x_apr[r2] + maxval; + } + + /* 'AttitudeEKF:210' P_apo=(eye(12)-K_k*H_k(1:3,1:12))*P_apr; */ + memset(&I[0], 0, 144U * sizeof(signed char)); + for (i = 0; i < 12; i++) { + I[i + 12 * i] = 1; + } + + for (r2 = 0; r2 < 12; r2++) { + for (i = 0; i < 12; i++) { + maxval = 0.0F; + for (r1 = 0; r1 < 3; r1++) { + maxval += b_K_k[r2 + 12 * r1] * (float)e_a[r1 + 3 * i]; + } + + A_lin[r2 + 12 * i] = (float)I[r2 + 12 * i] - maxval; + } + } + + for (r2 = 0; r2 < 12; r2++) { + for (i = 0; i < 12; i++) { + P_apo[r2 + 12 * i] = 0.0F; + for (r1 = 0; r1 < 12; r1++) { + P_apo[r2 + 12 * i] += A_lin[r2 + 12 * r1] * P_apr[r1 + 12 * i]; + } + } + } + } else { + /* 'AttitudeEKF:211' else */ + /* 'AttitudeEKF:212' if zFlag(1)==1&&zFlag(2)==1&&zFlag(3)==0 */ + if ((zFlag[0] == 1) && (zFlag[1] == 1) && (zFlag[2] == 0)) { + /* 'AttitudeEKF:214' R=[r_gyro,0,0,0,0,0; */ + /* 'AttitudeEKF:215' 0,r_gyro,0,0,0,0; */ + /* 'AttitudeEKF:216' 0,0,r_gyro,0,0,0; */ + /* 'AttitudeEKF:217' 0,0,0,r_accel,0,0; */ + /* 'AttitudeEKF:218' 0,0,0,0,r_accel,0; */ + /* 'AttitudeEKF:219' 0,0,0,0,0,r_accel]; */ + /* observation matrix */ + /* 'AttitudeEKF:223' H_k=[ E, Z, Z, Z; */ + /* 'AttitudeEKF:224' Z, Z, E, Z]; */ + /* 'AttitudeEKF:226' y_k=z(1:6)-H_k(1:6,1:12)*x_apr; */ + /* 'AttitudeEKF:228' S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)'+R(1:6,1:6); */ + /* 'AttitudeEKF:229' K_k=(P_apr*H_k(1:6,1:12)'/(S_k)); */ + for (r2 = 0; r2 < 12; r2++) { + for (i = 0; i < 6; i++) { + c_P_apr[r2 + 12 * i] = 0.0F; + for (r1 = 0; r1 < 12; r1++) { + c_P_apr[r2 + 12 * i] += P_apr[r2 + 12 * r1] * (float)c_b[r1 + 12 * + i]; + } + } + } + + for (r2 = 0; r2 < 6; r2++) { + for (i = 0; i < 12; i++) { + c_K_k[r2 + 6 * i] = 0.0F; + for (r1 = 0; r1 < 12; r1++) { + c_K_k[r2 + 6 * i] += (float)g_a[r2 + 6 * r1] * P_apr[r1 + 12 * i]; + } + } + + for (i = 0; i < 6; i++) { + d_a[r2 + 6 * i] = 0.0F; + for (r1 = 0; r1 < 12; r1++) { + d_a[r2 + 6 * i] += c_K_k[r2 + 6 * r1] * (float)c_b[r1 + 12 * i]; + } + } + } + + b_K_k[0] = r_gyro; + b_K_k[6] = 0.0F; + b_K_k[12] = 0.0F; + b_K_k[18] = 0.0F; + b_K_k[24] = 0.0F; + b_K_k[30] = 0.0F; + b_K_k[1] = 0.0F; + b_K_k[7] = r_gyro; + b_K_k[13] = 0.0F; + b_K_k[19] = 0.0F; + b_K_k[25] = 0.0F; + b_K_k[31] = 0.0F; + b_K_k[2] = 0.0F; + b_K_k[8] = 0.0F; + b_K_k[14] = r_gyro; + b_K_k[20] = 0.0F; + b_K_k[26] = 0.0F; + b_K_k[32] = 0.0F; + b_K_k[3] = 0.0F; + b_K_k[9] = 0.0F; + b_K_k[15] = 0.0F; + b_K_k[21] = r_accel; + b_K_k[27] = 0.0F; + b_K_k[33] = 0.0F; + b_K_k[4] = 0.0F; + b_K_k[10] = 0.0F; + b_K_k[16] = 0.0F; + b_K_k[22] = 0.0F; + b_K_k[28] = r_accel; + b_K_k[34] = 0.0F; + b_K_k[5] = 0.0F; + b_K_k[11] = 0.0F; + b_K_k[17] = 0.0F; + b_K_k[23] = 0.0F; + b_K_k[29] = 0.0F; + b_K_k[35] = r_accel; + for (r2 = 0; r2 < 6; r2++) { + for (i = 0; i < 6; i++) { + f_a[i + 6 * r2] = d_a[i + 6 * r2] + b_K_k[i + 6 * r2]; + } + } + + b_mrdivide(c_P_apr, f_a, c_K_k); + + /* 'AttitudeEKF:232' x_apo=x_apr+K_k*y_k; */ + for (r2 = 0; r2 < 6; r2++) { + maxval = 0.0F; + for (i = 0; i < 12; i++) { + maxval += (float)g_a[r2 + 6 * i] * x_apr[i]; + } + + b_z[r2] = z[r2] - maxval; + } + + for (r2 = 0; r2 < 12; r2++) { + maxval = 0.0F; + for (i = 0; i < 6; i++) { + maxval += c_K_k[r2 + 12 * i] * b_z[i]; + } + + x_apo[r2] = x_apr[r2] + maxval; + } + + /* 'AttitudeEKF:233' P_apo=(eye(12)-K_k*H_k(1:6,1:12))*P_apr; */ + memset(&I[0], 0, 144U * sizeof(signed char)); + for (i = 0; i < 12; i++) { + I[i + 12 * i] = 1; + } + + for (r2 = 0; r2 < 12; r2++) { + for (i = 0; i < 12; i++) { + maxval = 0.0F; + for (r1 = 0; r1 < 6; r1++) { + maxval += c_K_k[r2 + 12 * r1] * (float)g_a[r1 + 6 * i]; + } + + A_lin[r2 + 12 * i] = (float)I[r2 + 12 * i] - maxval; + } + } + + for (r2 = 0; r2 < 12; r2++) { + for (i = 0; i < 12; i++) { + P_apo[r2 + 12 * i] = 0.0F; + for (r1 = 0; r1 < 12; r1++) { + P_apo[r2 + 12 * i] += A_lin[r2 + 12 * r1] * P_apr[r1 + 12 * i]; + } + } + } + } else { + /* 'AttitudeEKF:234' else */ + /* 'AttitudeEKF:235' if zFlag(1)==1&&zFlag(2)==0&&zFlag(3)==1 */ + if ((zFlag[0] == 1) && (zFlag[1] == 0) && (zFlag[2] == 1)) { + /* 'AttitudeEKF:236' R=[r_gyro,0,0,0,0,0; */ + /* 'AttitudeEKF:237' 0,r_gyro,0,0,0,0; */ + /* 'AttitudeEKF:238' 0,0,r_gyro,0,0,0; */ + /* 'AttitudeEKF:239' 0,0,0,r_mag,0,0; */ + /* 'AttitudeEKF:240' 0,0,0,0,r_mag,0; */ + /* 'AttitudeEKF:241' 0,0,0,0,0,r_mag]; */ + /* observation matrix */ + /* 'AttitudeEKF:244' H_k=[ E, Z, Z, Z; */ + /* 'AttitudeEKF:245' Z, Z, Z, E]; */ + /* 'AttitudeEKF:247' y_k=[z(1:3);z(7:9)]-H_k(1:6,1:12)*x_apr; */ + /* 'AttitudeEKF:249' S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)'+R(1:6,1:6); */ + /* 'AttitudeEKF:250' K_k=(P_apr*H_k(1:6,1:12)'/(S_k)); */ + for (r2 = 0; r2 < 12; r2++) { + for (i = 0; i < 6; i++) { + c_P_apr[r2 + 12 * i] = 0.0F; + for (r1 = 0; r1 < 12; r1++) { + c_P_apr[r2 + 12 * i] += P_apr[r2 + 12 * r1] * (float)d_b[r1 + 12 + * i]; + } + } + } + + for (r2 = 0; r2 < 6; r2++) { + for (i = 0; i < 12; i++) { + c_K_k[r2 + 6 * i] = 0.0F; + for (r1 = 0; r1 < 12; r1++) { + c_K_k[r2 + 6 * i] += (float)h_a[r2 + 6 * r1] * P_apr[r1 + 12 * i]; + } + } + + for (i = 0; i < 6; i++) { + d_a[r2 + 6 * i] = 0.0F; + for (r1 = 0; r1 < 12; r1++) { + d_a[r2 + 6 * i] += c_K_k[r2 + 6 * r1] * (float)d_b[r1 + 12 * i]; + } + } + } + + b_K_k[0] = r_gyro; + b_K_k[6] = 0.0F; + b_K_k[12] = 0.0F; + b_K_k[18] = 0.0F; + b_K_k[24] = 0.0F; + b_K_k[30] = 0.0F; + b_K_k[1] = 0.0F; + b_K_k[7] = r_gyro; + b_K_k[13] = 0.0F; + b_K_k[19] = 0.0F; + b_K_k[25] = 0.0F; + b_K_k[31] = 0.0F; + b_K_k[2] = 0.0F; + b_K_k[8] = 0.0F; + b_K_k[14] = r_gyro; + b_K_k[20] = 0.0F; + b_K_k[26] = 0.0F; + b_K_k[32] = 0.0F; + b_K_k[3] = 0.0F; + b_K_k[9] = 0.0F; + b_K_k[15] = 0.0F; + b_K_k[21] = r_mag; + b_K_k[27] = 0.0F; + b_K_k[33] = 0.0F; + b_K_k[4] = 0.0F; + b_K_k[10] = 0.0F; + b_K_k[16] = 0.0F; + b_K_k[22] = 0.0F; + b_K_k[28] = r_mag; + b_K_k[34] = 0.0F; + b_K_k[5] = 0.0F; + b_K_k[11] = 0.0F; + b_K_k[17] = 0.0F; + b_K_k[23] = 0.0F; + b_K_k[29] = 0.0F; + b_K_k[35] = r_mag; + for (r2 = 0; r2 < 6; r2++) { + for (i = 0; i < 6; i++) { + f_a[i + 6 * r2] = d_a[i + 6 * r2] + b_K_k[i + 6 * r2]; + } + } + + b_mrdivide(c_P_apr, f_a, c_K_k); + + /* 'AttitudeEKF:253' x_apo=x_apr+K_k*y_k; */ + for (r2 = 0; r2 < 3; r2++) { + b_z[r2] = z[r2]; + } + + for (r2 = 0; r2 < 3; r2++) { + b_z[r2 + 3] = z[6 + r2]; + } + + for (r2 = 0; r2 < 6; r2++) { + i_a[r2] = 0.0F; + for (i = 0; i < 12; i++) { + i_a[r2] += (float)h_a[r2 + 6 * i] * x_apr[i]; + } + + c_z[r2] = b_z[r2] - i_a[r2]; + } + + for (r2 = 0; r2 < 12; r2++) { + maxval = 0.0F; + for (i = 0; i < 6; i++) { + maxval += c_K_k[r2 + 12 * i] * c_z[i]; + } + + x_apo[r2] = x_apr[r2] + maxval; + } + + /* 'AttitudeEKF:254' P_apo=(eye(12)-K_k*H_k(1:6,1:12))*P_apr; */ + memset(&I[0], 0, 144U * sizeof(signed char)); + for (i = 0; i < 12; i++) { + I[i + 12 * i] = 1; + } + + for (r2 = 0; r2 < 12; r2++) { + for (i = 0; i < 12; i++) { + maxval = 0.0F; + for (r1 = 0; r1 < 6; r1++) { + maxval += c_K_k[r2 + 12 * r1] * (float)h_a[r1 + 6 * i]; + } + + A_lin[r2 + 12 * i] = (float)I[r2 + 12 * i] - maxval; + } + } + + for (r2 = 0; r2 < 12; r2++) { + for (i = 0; i < 12; i++) { + P_apo[r2 + 12 * i] = 0.0F; + for (r1 = 0; r1 < 12; r1++) { + P_apo[r2 + 12 * i] += A_lin[r2 + 12 * r1] * P_apr[r1 + 12 * i]; + } + } + } + } else { + /* 'AttitudeEKF:255' else */ + /* 'AttitudeEKF:256' x_apo=x_apr; */ + for (i = 0; i < 12; i++) { + x_apo[i] = x_apr[i]; + } + + /* 'AttitudeEKF:257' P_apo=P_apr; */ + memcpy(&P_apo[0], &P_apr[0], 144U * sizeof(float)); + } + } + } + } + + /* % euler anglels extraction */ + /* 'AttitudeEKF:266' z_n_b = -x_apo(7:9)./norm(x_apo(7:9)); */ + maxval = norm(*(float (*)[3])&x_apo[6]); + a21 = norm(*(float (*)[3])&x_apo[9]); + for (i = 0; i < 3; i++) { + /* 'AttitudeEKF:267' m_n_b = x_apo(10:12)./norm(x_apo(10:12)); */ + muk[i] = -x_apo[i + 6] / maxval; + zek[i] = x_apo[i + 9] / a21; + } + + /* 'AttitudeEKF:269' y_n_b=cross(z_n_b,m_n_b); */ + wak[0] = muk[1] * zek[2] - muk[2] * zek[1]; + wak[1] = muk[2] * zek[0] - muk[0] * zek[2]; + wak[2] = muk[0] * zek[1] - muk[1] * zek[0]; + + /* 'AttitudeEKF:270' y_n_b=y_n_b./norm(y_n_b); */ + maxval = norm(wak); + for (r2 = 0; r2 < 3; r2++) { + wak[r2] /= maxval; + } + + /* 'AttitudeEKF:272' x_n_b=(cross(y_n_b,z_n_b)); */ + zek[0] = wak[1] * muk[2] - wak[2] * muk[1]; + zek[1] = wak[2] * muk[0] - wak[0] * muk[2]; + zek[2] = wak[0] * muk[1] - wak[1] * muk[0]; + + /* 'AttitudeEKF:273' x_n_b=x_n_b./norm(x_n_b); */ + maxval = norm(zek); + for (r2 = 0; r2 < 3; r2++) { + zek[r2] /= maxval; + } + + /* 'AttitudeEKF:276' xa_apo=x_apo; */ + for (i = 0; i < 12; i++) { + xa_apo[i] = x_apo[i]; + } + + /* 'AttitudeEKF:277' Pa_apo=P_apo; */ + memcpy(&Pa_apo[0], &P_apo[0], 144U * sizeof(float)); + + /* rotation matrix from earth to body system */ + /* 'AttitudeEKF:279' Rot_matrix=[x_n_b,y_n_b,z_n_b]; */ + for (r2 = 0; r2 < 3; r2++) { + Rot_matrix[r2] = zek[r2]; + Rot_matrix[3 + r2] = wak[r2]; + Rot_matrix[6 + r2] = muk[r2]; + } + + /* 'AttitudeEKF:282' phi=atan2(Rot_matrix(2,3),Rot_matrix(3,3)); */ + /* 'AttitudeEKF:283' theta=-asin(Rot_matrix(1,3)); */ + /* 'AttitudeEKF:284' psi=atan2(Rot_matrix(1,2),Rot_matrix(1,1)); */ + /* 'AttitudeEKF:285' eulerAngles=[phi;theta;psi]; */ + eulerAngles[0] = (real32_T)atan2(Rot_matrix[7], Rot_matrix[8]); + eulerAngles[1] = -(real32_T)asin(Rot_matrix[6]); + eulerAngles[2] = (real32_T)atan2(Rot_matrix[3], Rot_matrix[0]); +} + +void AttitudeEKF_initialize(void) +{ + Q_not_empty = FALSE; + Ji_not_empty = FALSE; + AttitudeEKF_init(); +} + +void AttitudeEKF_terminate(void) +{ + /* (no terminate code required) */ +} + +/* End of code generation (AttitudeEKF.c) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/AttitudeEKF.h b/src/modules/attitude_estimator_ekf/codegen/AttitudeEKF.h new file mode 100644 index 0000000000..fc02752109 --- /dev/null +++ b/src/modules/attitude_estimator_ekf/codegen/AttitudeEKF.h @@ -0,0 +1,26 @@ +/* + * AttitudeEKF.h + * + * Code generation for function 'AttitudeEKF' + * + * C source code generated on: Fri Jul 25 14:06:41 2014 + * + */ + +#ifndef __ATTITUDEEKF_H__ +#define __ATTITUDEEKF_H__ +/* Include files */ +#include +#include +#include +#include + +#include "rtwtypes.h" +#include "AttitudeEKF_types.h" + +/* Function Declarations */ +extern void AttitudeEKF(unsigned char approx_prediction, unsigned char use_inertia_matrix, const unsigned char zFlag[3], float dt, const float z[9], float q_rotSpeed, float q_rotAcc, float q_acc, float q_mag, float r_gyro, float r_accel, float r_mag, const float J[9], float xa_apo[12], float Pa_apo[144], float Rot_matrix[9], float eulerAngles[3], float debugOutput[4]); +extern void AttitudeEKF_initialize(void); +extern void AttitudeEKF_terminate(void); +#endif +/* End of code generation (AttitudeEKF.h) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/AttitudeEKF_types.h b/src/modules/attitude_estimator_ekf/codegen/AttitudeEKF_types.h new file mode 100644 index 0000000000..63eb7e5018 --- /dev/null +++ b/src/modules/attitude_estimator_ekf/codegen/AttitudeEKF_types.h @@ -0,0 +1,17 @@ +/* + * AttitudeEKF_types.h + * + * Code generation for function 'AttitudeEKF' + * + * C source code generated on: Fri Jul 25 14:06:41 2014 + * + */ + +#ifndef __ATTITUDEEKF_TYPES_H__ +#define __ATTITUDEEKF_TYPES_H__ + +/* Include files */ +#include "rtwtypes.h" + +#endif +/* End of code generation (AttitudeEKF_types.h) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c deleted file mode 100755 index 9e97ad11a8..0000000000 --- a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c +++ /dev/null @@ -1,1148 +0,0 @@ -/* - * attitudeKalmanfilter.c - * - * Code generation for function 'attitudeKalmanfilter' - * - * C source code generated on: Sat Jan 19 15:25:29 2013 - * - */ - -/* Include files */ -#include "rt_nonfinite.h" -#include "attitudeKalmanfilter.h" -#include "rdivide.h" -#include "norm.h" -#include "cross.h" -#include "eye.h" -#include "mrdivide.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ -static real32_T rt_atan2f_snf(real32_T u0, real32_T u1); - -/* Function Definitions */ -static real32_T rt_atan2f_snf(real32_T u0, real32_T u1) -{ - real32_T y; - int32_T b_u0; - int32_T b_u1; - if (rtIsNaNF(u0) || rtIsNaNF(u1)) { - y = ((real32_T)rtNaN); - } else if (rtIsInfF(u0) && rtIsInfF(u1)) { - if (u0 > 0.0F) { - b_u0 = 1; - } else { - b_u0 = -1; - } - - if (u1 > 0.0F) { - b_u1 = 1; - } else { - b_u1 = -1; - } - - y = (real32_T)atan2((real32_T)b_u0, (real32_T)b_u1); - } else if (u1 == 0.0F) { - if (u0 > 0.0F) { - y = RT_PIF / 2.0F; - } else if (u0 < 0.0F) { - y = -(RT_PIF / 2.0F); - } else { - y = 0.0F; - } - } else { - y = (real32_T)atan2(u0, u1); - } - - return y; -} - -/* - * function [eulerAngles,Rot_matrix,x_aposteriori,P_aposteriori] = attitudeKalmanfilter_wo(updateVect,dt,z,x_aposteriori_k,P_aposteriori_k,q,r) - */ -void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const - real32_T z[9], const real32_T x_aposteriori_k[12], const real32_T - P_aposteriori_k[144], const real32_T q[12], real32_T r[9], real32_T - eulerAngles[3], real32_T Rot_matrix[9], real32_T x_aposteriori[12], real32_T - P_aposteriori[144]) -{ - real32_T wak[3]; - real32_T O[9]; - real_T dv0[9]; - real32_T a[9]; - int32_T i; - real32_T b_a[9]; - real32_T x_n_b[3]; - real32_T b_x_aposteriori_k[3]; - real32_T z_n_b[3]; - real32_T c_a[3]; - real32_T d_a[3]; - int32_T i0; - real32_T x_apriori[12]; - real_T dv1[144]; - real32_T A_lin[144]; - static const int8_T iv0[36] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, - 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; - - real32_T b_A_lin[144]; - real32_T b_q[144]; - real32_T c_A_lin[144]; - real32_T d_A_lin[144]; - real32_T e_A_lin[144]; - int32_T i1; - real32_T P_apriori[144]; - real32_T b_P_apriori[108]; - static const int8_T iv1[108] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 }; - - real32_T K_k[108]; - real32_T fv0[81]; - static const int8_T iv2[108] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 }; - - real32_T b_r[81]; - real32_T fv1[81]; - real32_T f0; - real32_T c_P_apriori[36]; - static const int8_T iv3[36] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; - - real32_T fv2[36]; - static const int8_T iv4[36] = { 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; - - real32_T c_r[9]; - real32_T b_K_k[36]; - real32_T d_P_apriori[72]; - static const int8_T iv5[72] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 1, 0, 0, 0 }; - - real32_T c_K_k[72]; - static const int8_T iv6[72] = { 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, - 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0 }; - - real32_T b_z[6]; - static const int8_T iv7[72] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 1 }; - - static const int8_T iv8[72] = { 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, - 0, 0, 0, 1 }; - - real32_T fv3[6]; - real32_T c_z[6]; - - /* Extended Attitude Kalmanfilter */ - /* */ - /* state vector x has the following entries [ax,ay,az||mx,my,mz||wox,woy,woz||wx,wy,wz]' */ - /* measurement vector z has the following entries [ax,ay,az||mx,my,mz||wmx,wmy,wmz]' */ - /* knownConst has the following entries [PrvaA,PrvarM,PrvarWO,PrvarW||MsvarA,MsvarM,MsvarW] */ - /* */ - /* [x_aposteriori,P_aposteriori] = AttKalman(dt,z_k,x_aposteriori_k,P_aposteriori_k,knownConst) */ - /* */ - /* Example.... */ - /* */ - /* $Author: Tobias Naegeli $ $Date: 2012 $ $Revision: 1 $ */ - /* coder.varsize('udpIndVect', [9,1], [1,0]) */ - /* udpIndVect=find(updVect); */ - /* process and measurement noise covariance matrix */ - /* Q = diag(q.^2*dt); */ - /* observation matrix */ - /* 'attitudeKalmanfilter:33' wx= x_aposteriori_k(1); */ - /* 'attitudeKalmanfilter:34' wy= x_aposteriori_k(2); */ - /* 'attitudeKalmanfilter:35' wz= x_aposteriori_k(3); */ - /* 'attitudeKalmanfilter:37' wax= x_aposteriori_k(4); */ - /* 'attitudeKalmanfilter:38' way= x_aposteriori_k(5); */ - /* 'attitudeKalmanfilter:39' waz= x_aposteriori_k(6); */ - /* 'attitudeKalmanfilter:41' zex= x_aposteriori_k(7); */ - /* 'attitudeKalmanfilter:42' zey= x_aposteriori_k(8); */ - /* 'attitudeKalmanfilter:43' zez= x_aposteriori_k(9); */ - /* 'attitudeKalmanfilter:45' mux= x_aposteriori_k(10); */ - /* 'attitudeKalmanfilter:46' muy= x_aposteriori_k(11); */ - /* 'attitudeKalmanfilter:47' muz= x_aposteriori_k(12); */ - /* % prediction section */ - /* body angular accelerations */ - /* 'attitudeKalmanfilter:51' wak =[wax;way;waz]; */ - wak[0] = x_aposteriori_k[3]; - wak[1] = x_aposteriori_k[4]; - wak[2] = x_aposteriori_k[5]; - - /* body angular rates */ - /* 'attitudeKalmanfilter:54' wk =[wx; wy; wz] + dt*wak; */ - /* derivative of the prediction rotation matrix */ - /* 'attitudeKalmanfilter:57' O=[0,-wz,wy;wz,0,-wx;-wy,wx,0]'; */ - O[0] = 0.0F; - O[1] = -x_aposteriori_k[2]; - O[2] = x_aposteriori_k[1]; - O[3] = x_aposteriori_k[2]; - O[4] = 0.0F; - O[5] = -x_aposteriori_k[0]; - O[6] = -x_aposteriori_k[1]; - O[7] = x_aposteriori_k[0]; - O[8] = 0.0F; - - /* prediction of the earth z vector */ - /* 'attitudeKalmanfilter:60' zek =(eye(3)+O*dt)*[zex;zey;zez]; */ - eye(dv0); - for (i = 0; i < 9; i++) { - a[i] = (real32_T)dv0[i] + O[i] * dt; - } - - /* prediction of the magnetic vector */ - /* 'attitudeKalmanfilter:63' muk =(eye(3)+O*dt)*[mux;muy;muz]; */ - eye(dv0); - for (i = 0; i < 9; i++) { - b_a[i] = (real32_T)dv0[i] + O[i] * dt; - } - - /* 'attitudeKalmanfilter:65' EZ=[0,zez,-zey; */ - /* 'attitudeKalmanfilter:66' -zez,0,zex; */ - /* 'attitudeKalmanfilter:67' zey,-zex,0]'; */ - /* 'attitudeKalmanfilter:68' MA=[0,muz,-muy; */ - /* 'attitudeKalmanfilter:69' -muz,0,mux; */ - /* 'attitudeKalmanfilter:70' zey,-mux,0]'; */ - /* 'attitudeKalmanfilter:74' E=eye(3); */ - /* 'attitudeKalmanfilter:76' Z=zeros(3); */ - /* 'attitudeKalmanfilter:77' x_apriori=[wk;wak;zek;muk]; */ - x_n_b[0] = x_aposteriori_k[0]; - x_n_b[1] = x_aposteriori_k[1]; - x_n_b[2] = x_aposteriori_k[2]; - b_x_aposteriori_k[0] = x_aposteriori_k[6]; - b_x_aposteriori_k[1] = x_aposteriori_k[7]; - b_x_aposteriori_k[2] = x_aposteriori_k[8]; - z_n_b[0] = x_aposteriori_k[9]; - z_n_b[1] = x_aposteriori_k[10]; - z_n_b[2] = x_aposteriori_k[11]; - for (i = 0; i < 3; i++) { - c_a[i] = 0.0F; - for (i0 = 0; i0 < 3; i0++) { - c_a[i] += a[i + 3 * i0] * b_x_aposteriori_k[i0]; - } - - d_a[i] = 0.0F; - for (i0 = 0; i0 < 3; i0++) { - d_a[i] += b_a[i + 3 * i0] * z_n_b[i0]; - } - - x_apriori[i] = x_n_b[i] + dt * wak[i]; - } - - for (i = 0; i < 3; i++) { - x_apriori[i + 3] = wak[i]; - } - - for (i = 0; i < 3; i++) { - x_apriori[i + 6] = c_a[i]; - } - - for (i = 0; i < 3; i++) { - x_apriori[i + 9] = d_a[i]; - } - - /* 'attitudeKalmanfilter:81' A_lin=[ Z, E, Z, Z */ - /* 'attitudeKalmanfilter:82' Z, Z, Z, Z */ - /* 'attitudeKalmanfilter:83' EZ, Z, O, Z */ - /* 'attitudeKalmanfilter:84' MA, Z, Z, O]; */ - /* 'attitudeKalmanfilter:86' A_lin=eye(12)+A_lin*dt; */ - b_eye(dv1); - for (i = 0; i < 12; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_lin[i0 + 12 * i] = (real32_T)iv0[i0 + 3 * i]; - } - - for (i0 = 0; i0 < 3; i0++) { - A_lin[(i0 + 12 * i) + 3] = 0.0F; - } - } - - A_lin[6] = 0.0F; - A_lin[7] = x_aposteriori_k[8]; - A_lin[8] = -x_aposteriori_k[7]; - A_lin[18] = -x_aposteriori_k[8]; - A_lin[19] = 0.0F; - A_lin[20] = x_aposteriori_k[6]; - A_lin[30] = x_aposteriori_k[7]; - A_lin[31] = -x_aposteriori_k[6]; - A_lin[32] = 0.0F; - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_lin[(i0 + 12 * (i + 3)) + 6] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_lin[(i0 + 12 * (i + 6)) + 6] = O[i0 + 3 * i]; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_lin[(i0 + 12 * (i + 9)) + 6] = 0.0F; - } - } - - A_lin[9] = 0.0F; - A_lin[10] = x_aposteriori_k[11]; - A_lin[11] = -x_aposteriori_k[10]; - A_lin[21] = -x_aposteriori_k[11]; - A_lin[22] = 0.0F; - A_lin[23] = x_aposteriori_k[9]; - A_lin[33] = x_aposteriori_k[7]; - A_lin[34] = -x_aposteriori_k[9]; - A_lin[35] = 0.0F; - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_lin[(i0 + 12 * (i + 3)) + 9] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_lin[(i0 + 12 * (i + 6)) + 9] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_lin[(i0 + 12 * (i + 9)) + 9] = O[i0 + 3 * i]; - } - } - - for (i = 0; i < 12; i++) { - for (i0 = 0; i0 < 12; i0++) { - b_A_lin[i0 + 12 * i] = (real32_T)dv1[i0 + 12 * i] + A_lin[i0 + 12 * i] * - dt; - } - } - - /* 'attitudeKalmanfilter:88' Qtemp=[ q(1), 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; */ - /* 'attitudeKalmanfilter:89' 0, q(1), 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; */ - /* 'attitudeKalmanfilter:90' 0, 0, q(1), 0, 0, 0, 0, 0, 0, 0, 0, 0; */ - /* 'attitudeKalmanfilter:91' 0, 0, 0, q(2), 0, 0, 0, 0, 0, 0, 0, 0; */ - /* 'attitudeKalmanfilter:92' 0, 0, 0, 0, q(2), 0, 0, 0, 0, 0, 0, 0; */ - /* 'attitudeKalmanfilter:93' 0, 0, 0, 0, 0, q(2), 0, 0, 0, 0, 0, 0; */ - /* 'attitudeKalmanfilter:94' 0, 0, 0, 0, 0, 0, q(3), 0, 0, 0, 0, 0; */ - /* 'attitudeKalmanfilter:95' 0, 0, 0, 0, 0, 0, 0, q(3), 0, 0, 0, 0; */ - /* 'attitudeKalmanfilter:96' 0, 0, 0, 0, 0, 0, 0, 0, q(3), 0, 0, 0; */ - /* 'attitudeKalmanfilter:97' 0, 0, 0, 0, 0, 0, 0, 0, 0, q(4), 0, 0; */ - /* 'attitudeKalmanfilter:98' 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, q(4), 0; */ - /* 'attitudeKalmanfilter:99' 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, q(4)]; */ - /* 'attitudeKalmanfilter:103' Q=A_lin*Qtemp*A_lin'; */ - /* 'attitudeKalmanfilter:106' P_apriori=A_lin*P_aposteriori_k*A_lin'+Q; */ - b_q[0] = q[0]; - b_q[12] = 0.0F; - b_q[24] = 0.0F; - b_q[36] = 0.0F; - b_q[48] = 0.0F; - b_q[60] = 0.0F; - b_q[72] = 0.0F; - b_q[84] = 0.0F; - b_q[96] = 0.0F; - b_q[108] = 0.0F; - b_q[120] = 0.0F; - b_q[132] = 0.0F; - b_q[1] = 0.0F; - b_q[13] = q[0]; - b_q[25] = 0.0F; - b_q[37] = 0.0F; - b_q[49] = 0.0F; - b_q[61] = 0.0F; - b_q[73] = 0.0F; - b_q[85] = 0.0F; - b_q[97] = 0.0F; - b_q[109] = 0.0F; - b_q[121] = 0.0F; - b_q[133] = 0.0F; - b_q[2] = 0.0F; - b_q[14] = 0.0F; - b_q[26] = q[0]; - b_q[38] = 0.0F; - b_q[50] = 0.0F; - b_q[62] = 0.0F; - b_q[74] = 0.0F; - b_q[86] = 0.0F; - b_q[98] = 0.0F; - b_q[110] = 0.0F; - b_q[122] = 0.0F; - b_q[134] = 0.0F; - b_q[3] = 0.0F; - b_q[15] = 0.0F; - b_q[27] = 0.0F; - b_q[39] = q[1]; - b_q[51] = 0.0F; - b_q[63] = 0.0F; - b_q[75] = 0.0F; - b_q[87] = 0.0F; - b_q[99] = 0.0F; - b_q[111] = 0.0F; - b_q[123] = 0.0F; - b_q[135] = 0.0F; - b_q[4] = 0.0F; - b_q[16] = 0.0F; - b_q[28] = 0.0F; - b_q[40] = 0.0F; - b_q[52] = q[1]; - b_q[64] = 0.0F; - b_q[76] = 0.0F; - b_q[88] = 0.0F; - b_q[100] = 0.0F; - b_q[112] = 0.0F; - b_q[124] = 0.0F; - b_q[136] = 0.0F; - b_q[5] = 0.0F; - b_q[17] = 0.0F; - b_q[29] = 0.0F; - b_q[41] = 0.0F; - b_q[53] = 0.0F; - b_q[65] = q[1]; - b_q[77] = 0.0F; - b_q[89] = 0.0F; - b_q[101] = 0.0F; - b_q[113] = 0.0F; - b_q[125] = 0.0F; - b_q[137] = 0.0F; - b_q[6] = 0.0F; - b_q[18] = 0.0F; - b_q[30] = 0.0F; - b_q[42] = 0.0F; - b_q[54] = 0.0F; - b_q[66] = 0.0F; - b_q[78] = q[2]; - b_q[90] = 0.0F; - b_q[102] = 0.0F; - b_q[114] = 0.0F; - b_q[126] = 0.0F; - b_q[138] = 0.0F; - b_q[7] = 0.0F; - b_q[19] = 0.0F; - b_q[31] = 0.0F; - b_q[43] = 0.0F; - b_q[55] = 0.0F; - b_q[67] = 0.0F; - b_q[79] = 0.0F; - b_q[91] = q[2]; - b_q[103] = 0.0F; - b_q[115] = 0.0F; - b_q[127] = 0.0F; - b_q[139] = 0.0F; - b_q[8] = 0.0F; - b_q[20] = 0.0F; - b_q[32] = 0.0F; - b_q[44] = 0.0F; - b_q[56] = 0.0F; - b_q[68] = 0.0F; - b_q[80] = 0.0F; - b_q[92] = 0.0F; - b_q[104] = q[2]; - b_q[116] = 0.0F; - b_q[128] = 0.0F; - b_q[140] = 0.0F; - b_q[9] = 0.0F; - b_q[21] = 0.0F; - b_q[33] = 0.0F; - b_q[45] = 0.0F; - b_q[57] = 0.0F; - b_q[69] = 0.0F; - b_q[81] = 0.0F; - b_q[93] = 0.0F; - b_q[105] = 0.0F; - b_q[117] = q[3]; - b_q[129] = 0.0F; - b_q[141] = 0.0F; - b_q[10] = 0.0F; - b_q[22] = 0.0F; - b_q[34] = 0.0F; - b_q[46] = 0.0F; - b_q[58] = 0.0F; - b_q[70] = 0.0F; - b_q[82] = 0.0F; - b_q[94] = 0.0F; - b_q[106] = 0.0F; - b_q[118] = 0.0F; - b_q[130] = q[3]; - b_q[142] = 0.0F; - b_q[11] = 0.0F; - b_q[23] = 0.0F; - b_q[35] = 0.0F; - b_q[47] = 0.0F; - b_q[59] = 0.0F; - b_q[71] = 0.0F; - b_q[83] = 0.0F; - b_q[95] = 0.0F; - b_q[107] = 0.0F; - b_q[119] = 0.0F; - b_q[131] = 0.0F; - b_q[143] = q[3]; - for (i = 0; i < 12; i++) { - for (i0 = 0; i0 < 12; i0++) { - A_lin[i + 12 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - A_lin[i + 12 * i0] += b_A_lin[i + 12 * i1] * P_aposteriori_k[i1 + 12 * - i0]; - } - - c_A_lin[i + 12 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - c_A_lin[i + 12 * i0] += b_A_lin[i + 12 * i1] * b_q[i1 + 12 * i0]; - } - } - - for (i0 = 0; i0 < 12; i0++) { - d_A_lin[i + 12 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - d_A_lin[i + 12 * i0] += A_lin[i + 12 * i1] * b_A_lin[i0 + 12 * i1]; - } - - e_A_lin[i + 12 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - e_A_lin[i + 12 * i0] += c_A_lin[i + 12 * i1] * b_A_lin[i0 + 12 * i1]; - } - } - } - - for (i = 0; i < 12; i++) { - for (i0 = 0; i0 < 12; i0++) { - P_apriori[i0 + 12 * i] = d_A_lin[i0 + 12 * i] + e_A_lin[i0 + 12 * i]; - } - } - - /* % update */ - /* 'attitudeKalmanfilter:110' if updateVect(1)==1&&updateVect(2)==1&&updateVect(3)==1 */ - if ((updateVect[0] == 1) && (updateVect[1] == 1) && (updateVect[2] == 1)) { - /* 'attitudeKalmanfilter:111' if z(6)<4 || z(5)>15 */ - if ((z[5] < 4.0F) || (z[4] > 15.0F)) { - /* 'attitudeKalmanfilter:112' r(2)=10000; */ - r[1] = 10000.0F; - } - - /* 'attitudeKalmanfilter:114' R=[r(1),0,0,0,0,0,0,0,0; */ - /* 'attitudeKalmanfilter:115' 0,r(1),0,0,0,0,0,0,0; */ - /* 'attitudeKalmanfilter:116' 0,0,r(1),0,0,0,0,0,0; */ - /* 'attitudeKalmanfilter:117' 0,0,0,r(2),0,0,0,0,0; */ - /* 'attitudeKalmanfilter:118' 0,0,0,0,r(2),0,0,0,0; */ - /* 'attitudeKalmanfilter:119' 0,0,0,0,0,r(2),0,0,0; */ - /* 'attitudeKalmanfilter:120' 0,0,0,0,0,0,r(3),0,0; */ - /* 'attitudeKalmanfilter:121' 0,0,0,0,0,0,0,r(3),0; */ - /* 'attitudeKalmanfilter:122' 0,0,0,0,0,0,0,0,r(3)]; */ - /* observation matrix */ - /* [zw;ze;zmk]; */ - /* 'attitudeKalmanfilter:125' H_k=[ E, Z, Z, Z; */ - /* 'attitudeKalmanfilter:126' Z, Z, E, Z; */ - /* 'attitudeKalmanfilter:127' Z, Z, Z, E]; */ - /* 'attitudeKalmanfilter:129' y_k=z(1:9)-H_k*x_apriori; */ - /* 'attitudeKalmanfilter:132' S_k=H_k*P_apriori*H_k'+R; */ - /* 'attitudeKalmanfilter:133' K_k=(P_apriori*H_k'/(S_k)); */ - for (i = 0; i < 12; i++) { - for (i0 = 0; i0 < 9; i0++) { - b_P_apriori[i + 12 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - b_P_apriori[i + 12 * i0] += P_apriori[i + 12 * i1] * (real32_T)iv1[i1 - + 12 * i0]; - } - } - } - - for (i = 0; i < 9; i++) { - for (i0 = 0; i0 < 12; i0++) { - K_k[i + 9 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - K_k[i + 9 * i0] += (real32_T)iv2[i + 9 * i1] * P_apriori[i1 + 12 * i0]; - } - } - - for (i0 = 0; i0 < 9; i0++) { - fv0[i + 9 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - fv0[i + 9 * i0] += K_k[i + 9 * i1] * (real32_T)iv1[i1 + 12 * i0]; - } - } - } - - b_r[0] = r[0]; - b_r[9] = 0.0F; - b_r[18] = 0.0F; - b_r[27] = 0.0F; - b_r[36] = 0.0F; - b_r[45] = 0.0F; - b_r[54] = 0.0F; - b_r[63] = 0.0F; - b_r[72] = 0.0F; - b_r[1] = 0.0F; - b_r[10] = r[0]; - b_r[19] = 0.0F; - b_r[28] = 0.0F; - b_r[37] = 0.0F; - b_r[46] = 0.0F; - b_r[55] = 0.0F; - b_r[64] = 0.0F; - b_r[73] = 0.0F; - b_r[2] = 0.0F; - b_r[11] = 0.0F; - b_r[20] = r[0]; - b_r[29] = 0.0F; - b_r[38] = 0.0F; - b_r[47] = 0.0F; - b_r[56] = 0.0F; - b_r[65] = 0.0F; - b_r[74] = 0.0F; - b_r[3] = 0.0F; - b_r[12] = 0.0F; - b_r[21] = 0.0F; - b_r[30] = r[1]; - b_r[39] = 0.0F; - b_r[48] = 0.0F; - b_r[57] = 0.0F; - b_r[66] = 0.0F; - b_r[75] = 0.0F; - b_r[4] = 0.0F; - b_r[13] = 0.0F; - b_r[22] = 0.0F; - b_r[31] = 0.0F; - b_r[40] = r[1]; - b_r[49] = 0.0F; - b_r[58] = 0.0F; - b_r[67] = 0.0F; - b_r[76] = 0.0F; - b_r[5] = 0.0F; - b_r[14] = 0.0F; - b_r[23] = 0.0F; - b_r[32] = 0.0F; - b_r[41] = 0.0F; - b_r[50] = r[1]; - b_r[59] = 0.0F; - b_r[68] = 0.0F; - b_r[77] = 0.0F; - b_r[6] = 0.0F; - b_r[15] = 0.0F; - b_r[24] = 0.0F; - b_r[33] = 0.0F; - b_r[42] = 0.0F; - b_r[51] = 0.0F; - b_r[60] = r[2]; - b_r[69] = 0.0F; - b_r[78] = 0.0F; - b_r[7] = 0.0F; - b_r[16] = 0.0F; - b_r[25] = 0.0F; - b_r[34] = 0.0F; - b_r[43] = 0.0F; - b_r[52] = 0.0F; - b_r[61] = 0.0F; - b_r[70] = r[2]; - b_r[79] = 0.0F; - b_r[8] = 0.0F; - b_r[17] = 0.0F; - b_r[26] = 0.0F; - b_r[35] = 0.0F; - b_r[44] = 0.0F; - b_r[53] = 0.0F; - b_r[62] = 0.0F; - b_r[71] = 0.0F; - b_r[80] = r[2]; - for (i = 0; i < 9; i++) { - for (i0 = 0; i0 < 9; i0++) { - fv1[i0 + 9 * i] = fv0[i0 + 9 * i] + b_r[i0 + 9 * i]; - } - } - - mrdivide(b_P_apriori, fv1, K_k); - - /* 'attitudeKalmanfilter:136' x_aposteriori=x_apriori+K_k*y_k; */ - for (i = 0; i < 9; i++) { - f0 = 0.0F; - for (i0 = 0; i0 < 12; i0++) { - f0 += (real32_T)iv2[i + 9 * i0] * x_apriori[i0]; - } - - O[i] = z[i] - f0; - } - - for (i = 0; i < 12; i++) { - f0 = 0.0F; - for (i0 = 0; i0 < 9; i0++) { - f0 += K_k[i + 12 * i0] * O[i0]; - } - - x_aposteriori[i] = x_apriori[i] + f0; - } - - /* 'attitudeKalmanfilter:137' P_aposteriori=(eye(12)-K_k*H_k)*P_apriori; */ - b_eye(dv1); - for (i = 0; i < 12; i++) { - for (i0 = 0; i0 < 12; i0++) { - f0 = 0.0F; - for (i1 = 0; i1 < 9; i1++) { - f0 += K_k[i + 12 * i1] * (real32_T)iv2[i1 + 9 * i0]; - } - - b_A_lin[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0; - } - } - - for (i = 0; i < 12; i++) { - for (i0 = 0; i0 < 12; i0++) { - P_aposteriori[i + 12 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - P_aposteriori[i + 12 * i0] += b_A_lin[i + 12 * i1] * P_apriori[i1 + 12 - * i0]; - } - } - } - } else { - /* 'attitudeKalmanfilter:138' else */ - /* 'attitudeKalmanfilter:139' if updateVect(1)==1&&updateVect(2)==0&&updateVect(3)==0 */ - if ((updateVect[0] == 1) && (updateVect[1] == 0) && (updateVect[2] == 0)) { - /* 'attitudeKalmanfilter:141' R=[r(1),0,0; */ - /* 'attitudeKalmanfilter:142' 0,r(1),0; */ - /* 'attitudeKalmanfilter:143' 0,0,r(1)]; */ - /* observation matrix */ - /* 'attitudeKalmanfilter:146' H_k=[ E, Z, Z, Z]; */ - /* 'attitudeKalmanfilter:148' y_k=z(1:3)-H_k(1:3,1:12)*x_apriori; */ - /* 'attitudeKalmanfilter:150' S_k=H_k(1:3,1:12)*P_apriori*H_k(1:3,1:12)'+R(1:3,1:3); */ - /* 'attitudeKalmanfilter:151' K_k=(P_apriori*H_k(1:3,1:12)'/(S_k)); */ - for (i = 0; i < 12; i++) { - for (i0 = 0; i0 < 3; i0++) { - c_P_apriori[i + 12 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - c_P_apriori[i + 12 * i0] += P_apriori[i + 12 * i1] * (real32_T) - iv3[i1 + 12 * i0]; - } - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 12; i0++) { - fv2[i + 3 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - fv2[i + 3 * i0] += (real32_T)iv4[i + 3 * i1] * P_apriori[i1 + 12 * - i0]; - } - } - - for (i0 = 0; i0 < 3; i0++) { - O[i + 3 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - O[i + 3 * i0] += fv2[i + 3 * i1] * (real32_T)iv3[i1 + 12 * i0]; - } - } - } - - c_r[0] = r[0]; - c_r[3] = 0.0F; - c_r[6] = 0.0F; - c_r[1] = 0.0F; - c_r[4] = r[0]; - c_r[7] = 0.0F; - c_r[2] = 0.0F; - c_r[5] = 0.0F; - c_r[8] = r[0]; - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - a[i0 + 3 * i] = O[i0 + 3 * i] + c_r[i0 + 3 * i]; - } - } - - b_mrdivide(c_P_apriori, a, b_K_k); - - /* 'attitudeKalmanfilter:154' x_aposteriori=x_apriori+K_k*y_k; */ - for (i = 0; i < 3; i++) { - f0 = 0.0F; - for (i0 = 0; i0 < 12; i0++) { - f0 += (real32_T)iv4[i + 3 * i0] * x_apriori[i0]; - } - - x_n_b[i] = z[i] - f0; - } - - for (i = 0; i < 12; i++) { - f0 = 0.0F; - for (i0 = 0; i0 < 3; i0++) { - f0 += b_K_k[i + 12 * i0] * x_n_b[i0]; - } - - x_aposteriori[i] = x_apriori[i] + f0; - } - - /* 'attitudeKalmanfilter:155' P_aposteriori=(eye(12)-K_k*H_k(1:3,1:12))*P_apriori; */ - b_eye(dv1); - for (i = 0; i < 12; i++) { - for (i0 = 0; i0 < 12; i0++) { - f0 = 0.0F; - for (i1 = 0; i1 < 3; i1++) { - f0 += b_K_k[i + 12 * i1] * (real32_T)iv4[i1 + 3 * i0]; - } - - b_A_lin[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0; - } - } - - for (i = 0; i < 12; i++) { - for (i0 = 0; i0 < 12; i0++) { - P_aposteriori[i + 12 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - P_aposteriori[i + 12 * i0] += b_A_lin[i + 12 * i1] * P_apriori[i1 + - 12 * i0]; - } - } - } - } else { - /* 'attitudeKalmanfilter:156' else */ - /* 'attitudeKalmanfilter:157' if updateVect(1)==1&&updateVect(2)==1&&updateVect(3)==0 */ - if ((updateVect[0] == 1) && (updateVect[1] == 1) && (updateVect[2] == 0)) - { - /* 'attitudeKalmanfilter:158' if z(6)<4 || z(5)>15 */ - if ((z[5] < 4.0F) || (z[4] > 15.0F)) { - /* 'attitudeKalmanfilter:159' r(2)=10000; */ - r[1] = 10000.0F; - } - - /* 'attitudeKalmanfilter:162' R=[r(1),0,0,0,0,0; */ - /* 'attitudeKalmanfilter:163' 0,r(1),0,0,0,0; */ - /* 'attitudeKalmanfilter:164' 0,0,r(1),0,0,0; */ - /* 'attitudeKalmanfilter:165' 0,0,0,r(2),0,0; */ - /* 'attitudeKalmanfilter:166' 0,0,0,0,r(2),0; */ - /* 'attitudeKalmanfilter:167' 0,0,0,0,0,r(2)]; */ - /* observation matrix */ - /* 'attitudeKalmanfilter:170' H_k=[ E, Z, Z, Z; */ - /* 'attitudeKalmanfilter:171' Z, Z, E, Z]; */ - /* 'attitudeKalmanfilter:173' y_k=z(1:6)-H_k(1:6,1:12)*x_apriori; */ - /* 'attitudeKalmanfilter:175' S_k=H_k(1:6,1:12)*P_apriori*H_k(1:6,1:12)'+R(1:6,1:6); */ - /* 'attitudeKalmanfilter:176' K_k=(P_apriori*H_k(1:6,1:12)'/(S_k)); */ - for (i = 0; i < 12; i++) { - for (i0 = 0; i0 < 6; i0++) { - d_P_apriori[i + 12 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - d_P_apriori[i + 12 * i0] += P_apriori[i + 12 * i1] * (real32_T) - iv5[i1 + 12 * i0]; - } - } - } - - for (i = 0; i < 6; i++) { - for (i0 = 0; i0 < 12; i0++) { - c_K_k[i + 6 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - c_K_k[i + 6 * i0] += (real32_T)iv6[i + 6 * i1] * P_apriori[i1 + 12 - * i0]; - } - } - - for (i0 = 0; i0 < 6; i0++) { - fv2[i + 6 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - fv2[i + 6 * i0] += c_K_k[i + 6 * i1] * (real32_T)iv5[i1 + 12 * i0]; - } - } - } - - b_K_k[0] = r[0]; - b_K_k[6] = 0.0F; - b_K_k[12] = 0.0F; - b_K_k[18] = 0.0F; - b_K_k[24] = 0.0F; - b_K_k[30] = 0.0F; - b_K_k[1] = 0.0F; - b_K_k[7] = r[0]; - b_K_k[13] = 0.0F; - b_K_k[19] = 0.0F; - b_K_k[25] = 0.0F; - b_K_k[31] = 0.0F; - b_K_k[2] = 0.0F; - b_K_k[8] = 0.0F; - b_K_k[14] = r[0]; - b_K_k[20] = 0.0F; - b_K_k[26] = 0.0F; - b_K_k[32] = 0.0F; - b_K_k[3] = 0.0F; - b_K_k[9] = 0.0F; - b_K_k[15] = 0.0F; - b_K_k[21] = r[1]; - b_K_k[27] = 0.0F; - b_K_k[33] = 0.0F; - b_K_k[4] = 0.0F; - b_K_k[10] = 0.0F; - b_K_k[16] = 0.0F; - b_K_k[22] = 0.0F; - b_K_k[28] = r[1]; - b_K_k[34] = 0.0F; - b_K_k[5] = 0.0F; - b_K_k[11] = 0.0F; - b_K_k[17] = 0.0F; - b_K_k[23] = 0.0F; - b_K_k[29] = 0.0F; - b_K_k[35] = r[1]; - for (i = 0; i < 6; i++) { - for (i0 = 0; i0 < 6; i0++) { - c_P_apriori[i0 + 6 * i] = fv2[i0 + 6 * i] + b_K_k[i0 + 6 * i]; - } - } - - c_mrdivide(d_P_apriori, c_P_apriori, c_K_k); - - /* 'attitudeKalmanfilter:179' x_aposteriori=x_apriori+K_k*y_k; */ - for (i = 0; i < 6; i++) { - f0 = 0.0F; - for (i0 = 0; i0 < 12; i0++) { - f0 += (real32_T)iv6[i + 6 * i0] * x_apriori[i0]; - } - - b_z[i] = z[i] - f0; - } - - for (i = 0; i < 12; i++) { - f0 = 0.0F; - for (i0 = 0; i0 < 6; i0++) { - f0 += c_K_k[i + 12 * i0] * b_z[i0]; - } - - x_aposteriori[i] = x_apriori[i] + f0; - } - - /* 'attitudeKalmanfilter:180' P_aposteriori=(eye(12)-K_k*H_k(1:6,1:12))*P_apriori; */ - b_eye(dv1); - for (i = 0; i < 12; i++) { - for (i0 = 0; i0 < 12; i0++) { - f0 = 0.0F; - for (i1 = 0; i1 < 6; i1++) { - f0 += c_K_k[i + 12 * i1] * (real32_T)iv6[i1 + 6 * i0]; - } - - b_A_lin[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0; - } - } - - for (i = 0; i < 12; i++) { - for (i0 = 0; i0 < 12; i0++) { - P_aposteriori[i + 12 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - P_aposteriori[i + 12 * i0] += b_A_lin[i + 12 * i1] * P_apriori[i1 - + 12 * i0]; - } - } - } - } else { - /* 'attitudeKalmanfilter:181' else */ - /* 'attitudeKalmanfilter:182' if updateVect(1)==1&&updateVect(2)==0&&updateVect(3)==1 */ - if ((updateVect[0] == 1) && (updateVect[1] == 0) && (updateVect[2] == 1)) - { - /* 'attitudeKalmanfilter:183' R=[r(1),0,0,0,0,0; */ - /* 'attitudeKalmanfilter:184' 0,r(1),0,0,0,0; */ - /* 'attitudeKalmanfilter:185' 0,0,r(1),0,0,0; */ - /* 'attitudeKalmanfilter:186' 0,0,0,r(3),0,0; */ - /* 'attitudeKalmanfilter:187' 0,0,0,0,r(3),0; */ - /* 'attitudeKalmanfilter:188' 0,0,0,0,0,r(3)]; */ - /* observation matrix */ - /* 'attitudeKalmanfilter:191' H_k=[ E, Z, Z, Z; */ - /* 'attitudeKalmanfilter:192' Z, Z, Z, E]; */ - /* 'attitudeKalmanfilter:194' y_k=[z(1:3);z(7:9)]-H_k(1:6,1:12)*x_apriori; */ - /* 'attitudeKalmanfilter:196' S_k=H_k(1:6,1:12)*P_apriori*H_k(1:6,1:12)'+R(1:6,1:6); */ - /* 'attitudeKalmanfilter:197' K_k=(P_apriori*H_k(1:6,1:12)'/(S_k)); */ - for (i = 0; i < 12; i++) { - for (i0 = 0; i0 < 6; i0++) { - d_P_apriori[i + 12 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - d_P_apriori[i + 12 * i0] += P_apriori[i + 12 * i1] * (real32_T) - iv7[i1 + 12 * i0]; - } - } - } - - for (i = 0; i < 6; i++) { - for (i0 = 0; i0 < 12; i0++) { - c_K_k[i + 6 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - c_K_k[i + 6 * i0] += (real32_T)iv8[i + 6 * i1] * P_apriori[i1 + - 12 * i0]; - } - } - - for (i0 = 0; i0 < 6; i0++) { - fv2[i + 6 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - fv2[i + 6 * i0] += c_K_k[i + 6 * i1] * (real32_T)iv7[i1 + 12 * - i0]; - } - } - } - - b_K_k[0] = r[0]; - b_K_k[6] = 0.0F; - b_K_k[12] = 0.0F; - b_K_k[18] = 0.0F; - b_K_k[24] = 0.0F; - b_K_k[30] = 0.0F; - b_K_k[1] = 0.0F; - b_K_k[7] = r[0]; - b_K_k[13] = 0.0F; - b_K_k[19] = 0.0F; - b_K_k[25] = 0.0F; - b_K_k[31] = 0.0F; - b_K_k[2] = 0.0F; - b_K_k[8] = 0.0F; - b_K_k[14] = r[0]; - b_K_k[20] = 0.0F; - b_K_k[26] = 0.0F; - b_K_k[32] = 0.0F; - b_K_k[3] = 0.0F; - b_K_k[9] = 0.0F; - b_K_k[15] = 0.0F; - b_K_k[21] = r[2]; - b_K_k[27] = 0.0F; - b_K_k[33] = 0.0F; - b_K_k[4] = 0.0F; - b_K_k[10] = 0.0F; - b_K_k[16] = 0.0F; - b_K_k[22] = 0.0F; - b_K_k[28] = r[2]; - b_K_k[34] = 0.0F; - b_K_k[5] = 0.0F; - b_K_k[11] = 0.0F; - b_K_k[17] = 0.0F; - b_K_k[23] = 0.0F; - b_K_k[29] = 0.0F; - b_K_k[35] = r[2]; - for (i = 0; i < 6; i++) { - for (i0 = 0; i0 < 6; i0++) { - c_P_apriori[i0 + 6 * i] = fv2[i0 + 6 * i] + b_K_k[i0 + 6 * i]; - } - } - - c_mrdivide(d_P_apriori, c_P_apriori, c_K_k); - - /* 'attitudeKalmanfilter:200' x_aposteriori=x_apriori+K_k*y_k; */ - for (i = 0; i < 3; i++) { - b_z[i] = z[i]; - } - - for (i = 0; i < 3; i++) { - b_z[i + 3] = z[i + 6]; - } - - for (i = 0; i < 6; i++) { - fv3[i] = 0.0F; - for (i0 = 0; i0 < 12; i0++) { - fv3[i] += (real32_T)iv8[i + 6 * i0] * x_apriori[i0]; - } - - c_z[i] = b_z[i] - fv3[i]; - } - - for (i = 0; i < 12; i++) { - f0 = 0.0F; - for (i0 = 0; i0 < 6; i0++) { - f0 += c_K_k[i + 12 * i0] * c_z[i0]; - } - - x_aposteriori[i] = x_apriori[i] + f0; - } - - /* 'attitudeKalmanfilter:201' P_aposteriori=(eye(12)-K_k*H_k(1:6,1:12))*P_apriori; */ - b_eye(dv1); - for (i = 0; i < 12; i++) { - for (i0 = 0; i0 < 12; i0++) { - f0 = 0.0F; - for (i1 = 0; i1 < 6; i1++) { - f0 += c_K_k[i + 12 * i1] * (real32_T)iv8[i1 + 6 * i0]; - } - - b_A_lin[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0; - } - } - - for (i = 0; i < 12; i++) { - for (i0 = 0; i0 < 12; i0++) { - P_aposteriori[i + 12 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - P_aposteriori[i + 12 * i0] += b_A_lin[i + 12 * i1] * - P_apriori[i1 + 12 * i0]; - } - } - } - } else { - /* 'attitudeKalmanfilter:202' else */ - /* 'attitudeKalmanfilter:203' x_aposteriori=x_apriori; */ - for (i = 0; i < 12; i++) { - x_aposteriori[i] = x_apriori[i]; - } - - /* 'attitudeKalmanfilter:204' P_aposteriori=P_apriori; */ - memcpy(&P_aposteriori[0], &P_apriori[0], 144U * sizeof(real32_T)); - } - } - } - } - - /* % euler anglels extraction */ - /* 'attitudeKalmanfilter:213' z_n_b = -x_aposteriori(7:9)./norm(x_aposteriori(7:9)); */ - for (i = 0; i < 3; i++) { - x_n_b[i] = -x_aposteriori[i + 6]; - } - - rdivide(x_n_b, norm(*(real32_T (*)[3])&x_aposteriori[6]), z_n_b); - - /* 'attitudeKalmanfilter:214' m_n_b = x_aposteriori(10:12)./norm(x_aposteriori(10:12)); */ - rdivide(*(real32_T (*)[3])&x_aposteriori[9], norm(*(real32_T (*)[3])& - x_aposteriori[9]), wak); - - /* 'attitudeKalmanfilter:216' y_n_b=cross(z_n_b,m_n_b); */ - for (i = 0; i < 3; i++) { - x_n_b[i] = wak[i]; - } - - cross(z_n_b, x_n_b, wak); - - /* 'attitudeKalmanfilter:217' y_n_b=y_n_b./norm(y_n_b); */ - for (i = 0; i < 3; i++) { - x_n_b[i] = wak[i]; - } - - rdivide(x_n_b, norm(wak), wak); - - /* 'attitudeKalmanfilter:219' x_n_b=(cross(y_n_b,z_n_b)); */ - cross(wak, z_n_b, x_n_b); - - /* 'attitudeKalmanfilter:220' x_n_b=x_n_b./norm(x_n_b); */ - for (i = 0; i < 3; i++) { - b_x_aposteriori_k[i] = x_n_b[i]; - } - - rdivide(b_x_aposteriori_k, norm(x_n_b), x_n_b); - - /* 'attitudeKalmanfilter:226' Rot_matrix=[x_n_b,y_n_b,z_n_b]; */ - for (i = 0; i < 3; i++) { - Rot_matrix[i] = x_n_b[i]; - Rot_matrix[3 + i] = wak[i]; - Rot_matrix[6 + i] = z_n_b[i]; - } - - /* 'attitudeKalmanfilter:230' phi=atan2(Rot_matrix(2,3),Rot_matrix(3,3)); */ - /* 'attitudeKalmanfilter:231' theta=-asin(Rot_matrix(1,3)); */ - /* 'attitudeKalmanfilter:232' psi=atan2(Rot_matrix(1,2),Rot_matrix(1,1)); */ - /* 'attitudeKalmanfilter:233' eulerAngles=[phi;theta;psi]; */ - eulerAngles[0] = rt_atan2f_snf(Rot_matrix[7], Rot_matrix[8]); - eulerAngles[1] = -(real32_T)asin(Rot_matrix[6]); - eulerAngles[2] = rt_atan2f_snf(Rot_matrix[3], Rot_matrix[0]); -} - -/* End of code generation (attitudeKalmanfilter.c) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h deleted file mode 100755 index afa63c1a91..0000000000 --- a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h +++ /dev/null @@ -1,34 +0,0 @@ -/* - * attitudeKalmanfilter.h - * - * Code generation for function 'attitudeKalmanfilter' - * - * C source code generated on: Sat Jan 19 15:25:29 2013 - * - */ - -#ifndef __ATTITUDEKALMANFILTER_H__ -#define __ATTITUDEKALMANFILTER_H__ -/* Include files */ -#include -#include -#include -#include -#include "rt_defines.h" -#include "rt_nonfinite.h" - -#include "rtwtypes.h" -#include "attitudeKalmanfilter_types.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ -extern void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const real32_T z[9], const real32_T x_aposteriori_k[12], const real32_T P_aposteriori_k[144], const real32_T q[12], real32_T r[9], real32_T eulerAngles[3], real32_T Rot_matrix[9], real32_T x_aposteriori[12], real32_T P_aposteriori[144]); -#endif -/* End of code generation (attitudeKalmanfilter.h) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c deleted file mode 100755 index 7d620d7fa1..0000000000 --- a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c +++ /dev/null @@ -1,31 +0,0 @@ -/* - * attitudeKalmanfilter_initialize.c - * - * Code generation for function 'attitudeKalmanfilter_initialize' - * - * C source code generated on: Sat Jan 19 15:25:29 2013 - * - */ - -/* Include files */ -#include "rt_nonfinite.h" -#include "attitudeKalmanfilter.h" -#include "attitudeKalmanfilter_initialize.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ - -/* Function Definitions */ -void attitudeKalmanfilter_initialize(void) -{ - rt_InitInfAndNaN(8U); -} - -/* End of code generation (attitudeKalmanfilter_initialize.c) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h deleted file mode 100755 index 8b599be663..0000000000 --- a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h +++ /dev/null @@ -1,34 +0,0 @@ -/* - * attitudeKalmanfilter_initialize.h - * - * Code generation for function 'attitudeKalmanfilter_initialize' - * - * C source code generated on: Sat Jan 19 15:25:29 2013 - * - */ - -#ifndef __ATTITUDEKALMANFILTER_INITIALIZE_H__ -#define __ATTITUDEKALMANFILTER_INITIALIZE_H__ -/* Include files */ -#include -#include -#include -#include -#include "rt_defines.h" -#include "rt_nonfinite.h" - -#include "rtwtypes.h" -#include "attitudeKalmanfilter_types.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ -extern void attitudeKalmanfilter_initialize(void); -#endif -/* End of code generation (attitudeKalmanfilter_initialize.h) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c deleted file mode 100755 index 7f97274196..0000000000 --- a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c +++ /dev/null @@ -1,31 +0,0 @@ -/* - * attitudeKalmanfilter_terminate.c - * - * Code generation for function 'attitudeKalmanfilter_terminate' - * - * C source code generated on: Sat Jan 19 15:25:29 2013 - * - */ - -/* Include files */ -#include "rt_nonfinite.h" -#include "attitudeKalmanfilter.h" -#include "attitudeKalmanfilter_terminate.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ - -/* Function Definitions */ -void attitudeKalmanfilter_terminate(void) -{ - /* (no terminate code required) */ -} - -/* End of code generation (attitudeKalmanfilter_terminate.c) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h deleted file mode 100755 index da84a50244..0000000000 --- a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h +++ /dev/null @@ -1,34 +0,0 @@ -/* - * attitudeKalmanfilter_terminate.h - * - * Code generation for function 'attitudeKalmanfilter_terminate' - * - * C source code generated on: Sat Jan 19 15:25:29 2013 - * - */ - -#ifndef __ATTITUDEKALMANFILTER_TERMINATE_H__ -#define __ATTITUDEKALMANFILTER_TERMINATE_H__ -/* Include files */ -#include -#include -#include -#include -#include "rt_defines.h" -#include "rt_nonfinite.h" - -#include "rtwtypes.h" -#include "attitudeKalmanfilter_types.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ -extern void attitudeKalmanfilter_terminate(void); -#endif -/* End of code generation (attitudeKalmanfilter_terminate.h) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h deleted file mode 100755 index 30fd1e75e6..0000000000 --- a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h +++ /dev/null @@ -1,16 +0,0 @@ -/* - * attitudeKalmanfilter_types.h - * - * Code generation for function 'attitudeKalmanfilter' - * - * C source code generated on: Sat Jan 19 15:25:29 2013 - * - */ - -#ifndef __ATTITUDEKALMANFILTER_TYPES_H__ -#define __ATTITUDEKALMANFILTER_TYPES_H__ - -/* Type Definitions */ - -#endif -/* End of code generation (attitudeKalmanfilter_types.h) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/cross.c b/src/modules/attitude_estimator_ekf/codegen/cross.c deleted file mode 100755 index 84ada9002b..0000000000 --- a/src/modules/attitude_estimator_ekf/codegen/cross.c +++ /dev/null @@ -1,37 +0,0 @@ -/* - * cross.c - * - * Code generation for function 'cross' - * - * C source code generated on: Sat Jan 19 15:25:29 2013 - * - */ - -/* Include files */ -#include "rt_nonfinite.h" -#include "attitudeKalmanfilter.h" -#include "cross.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ - -/* Function Definitions */ - -/* - * - */ -void cross(const real32_T a[3], const real32_T b[3], real32_T c[3]) -{ - c[0] = a[1] * b[2] - a[2] * b[1]; - c[1] = a[2] * b[0] - a[0] * b[2]; - c[2] = a[0] * b[1] - a[1] * b[0]; -} - -/* End of code generation (cross.c) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/cross.h b/src/modules/attitude_estimator_ekf/codegen/cross.h deleted file mode 100755 index e727f0684a..0000000000 --- a/src/modules/attitude_estimator_ekf/codegen/cross.h +++ /dev/null @@ -1,34 +0,0 @@ -/* - * cross.h - * - * Code generation for function 'cross' - * - * C source code generated on: Sat Jan 19 15:25:29 2013 - * - */ - -#ifndef __CROSS_H__ -#define __CROSS_H__ -/* Include files */ -#include -#include -#include -#include -#include "rt_defines.h" -#include "rt_nonfinite.h" - -#include "rtwtypes.h" -#include "attitudeKalmanfilter_types.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ -extern void cross(const real32_T a[3], const real32_T b[3], real32_T c[3]); -#endif -/* End of code generation (cross.h) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/eye.c b/src/modules/attitude_estimator_ekf/codegen/eye.c deleted file mode 100755 index b89ab58ef6..0000000000 --- a/src/modules/attitude_estimator_ekf/codegen/eye.c +++ /dev/null @@ -1,51 +0,0 @@ -/* - * eye.c - * - * Code generation for function 'eye' - * - * C source code generated on: Sat Jan 19 15:25:29 2013 - * - */ - -/* Include files */ -#include "rt_nonfinite.h" -#include "attitudeKalmanfilter.h" -#include "eye.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ - -/* Function Definitions */ - -/* - * - */ -void b_eye(real_T I[144]) -{ - int32_T i; - memset(&I[0], 0, 144U * sizeof(real_T)); - for (i = 0; i < 12; i++) { - I[i + 12 * i] = 1.0; - } -} - -/* - * - */ -void eye(real_T I[9]) -{ - int32_T i; - memset(&I[0], 0, 9U * sizeof(real_T)); - for (i = 0; i < 3; i++) { - I[i + 3 * i] = 1.0; - } -} - -/* End of code generation (eye.c) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/eye.h b/src/modules/attitude_estimator_ekf/codegen/eye.h deleted file mode 100755 index 94fbe76717..0000000000 --- a/src/modules/attitude_estimator_ekf/codegen/eye.h +++ /dev/null @@ -1,35 +0,0 @@ -/* - * eye.h - * - * Code generation for function 'eye' - * - * C source code generated on: Sat Jan 19 15:25:29 2013 - * - */ - -#ifndef __EYE_H__ -#define __EYE_H__ -/* Include files */ -#include -#include -#include -#include -#include "rt_defines.h" -#include "rt_nonfinite.h" - -#include "rtwtypes.h" -#include "attitudeKalmanfilter_types.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ -extern void b_eye(real_T I[144]); -extern void eye(real_T I[9]); -#endif -/* End of code generation (eye.h) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/mrdivide.c b/src/modules/attitude_estimator_ekf/codegen/mrdivide.c deleted file mode 100755 index a810f22e4f..0000000000 --- a/src/modules/attitude_estimator_ekf/codegen/mrdivide.c +++ /dev/null @@ -1,357 +0,0 @@ -/* - * mrdivide.c - * - * Code generation for function 'mrdivide' - * - * C source code generated on: Sat Jan 19 15:25:29 2013 - * - */ - -/* Include files */ -#include "rt_nonfinite.h" -#include "attitudeKalmanfilter.h" -#include "mrdivide.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ - -/* Function Definitions */ - -/* - * - */ -void b_mrdivide(const real32_T A[36], const real32_T B[9], real32_T y[36]) -{ - real32_T b_A[9]; - int32_T rtemp; - int32_T k; - real32_T b_B[36]; - int32_T r1; - int32_T r2; - int32_T r3; - real32_T maxval; - real32_T a21; - real32_T Y[36]; - for (rtemp = 0; rtemp < 3; rtemp++) { - for (k = 0; k < 3; k++) { - b_A[k + 3 * rtemp] = B[rtemp + 3 * k]; - } - } - - for (rtemp = 0; rtemp < 12; rtemp++) { - for (k = 0; k < 3; k++) { - b_B[k + 3 * rtemp] = A[rtemp + 12 * k]; - } - } - - r1 = 0; - r2 = 1; - r3 = 2; - maxval = (real32_T)fabs(b_A[0]); - a21 = (real32_T)fabs(b_A[1]); - if (a21 > maxval) { - maxval = a21; - r1 = 1; - r2 = 0; - } - - if ((real32_T)fabs(b_A[2]) > maxval) { - r1 = 2; - r2 = 1; - r3 = 0; - } - - b_A[r2] /= b_A[r1]; - b_A[r3] /= b_A[r1]; - b_A[3 + r2] -= b_A[r2] * b_A[3 + r1]; - b_A[3 + r3] -= b_A[r3] * b_A[3 + r1]; - b_A[6 + r2] -= b_A[r2] * b_A[6 + r1]; - b_A[6 + r3] -= b_A[r3] * b_A[6 + r1]; - if ((real32_T)fabs(b_A[3 + r3]) > (real32_T)fabs(b_A[3 + r2])) { - rtemp = r2; - r2 = r3; - r3 = rtemp; - } - - b_A[3 + r3] /= b_A[3 + r2]; - b_A[6 + r3] -= b_A[3 + r3] * b_A[6 + r2]; - for (k = 0; k < 12; k++) { - Y[3 * k] = b_B[r1 + 3 * k]; - Y[1 + 3 * k] = b_B[r2 + 3 * k] - Y[3 * k] * b_A[r2]; - Y[2 + 3 * k] = (b_B[r3 + 3 * k] - Y[3 * k] * b_A[r3]) - Y[1 + 3 * k] * b_A[3 - + r3]; - Y[2 + 3 * k] /= b_A[6 + r3]; - Y[3 * k] -= Y[2 + 3 * k] * b_A[6 + r1]; - Y[1 + 3 * k] -= Y[2 + 3 * k] * b_A[6 + r2]; - Y[1 + 3 * k] /= b_A[3 + r2]; - Y[3 * k] -= Y[1 + 3 * k] * b_A[3 + r1]; - Y[3 * k] /= b_A[r1]; - } - - for (rtemp = 0; rtemp < 3; rtemp++) { - for (k = 0; k < 12; k++) { - y[k + 12 * rtemp] = Y[rtemp + 3 * k]; - } - } -} - -/* - * - */ -void c_mrdivide(const real32_T A[72], const real32_T B[36], real32_T y[72]) -{ - real32_T b_A[36]; - int8_T ipiv[6]; - int32_T i3; - int32_T iy; - int32_T j; - int32_T c; - int32_T ix; - real32_T temp; - int32_T k; - real32_T s; - int32_T jy; - int32_T ijA; - real32_T Y[72]; - for (i3 = 0; i3 < 6; i3++) { - for (iy = 0; iy < 6; iy++) { - b_A[iy + 6 * i3] = B[i3 + 6 * iy]; - } - - ipiv[i3] = (int8_T)(1 + i3); - } - - for (j = 0; j < 5; j++) { - c = j * 7; - iy = 0; - ix = c; - temp = (real32_T)fabs(b_A[c]); - for (k = 2; k <= 6 - j; k++) { - ix++; - s = (real32_T)fabs(b_A[ix]); - if (s > temp) { - iy = k - 1; - temp = s; - } - } - - if (b_A[c + iy] != 0.0F) { - if (iy != 0) { - ipiv[j] = (int8_T)((j + iy) + 1); - ix = j; - iy += j; - for (k = 0; k < 6; k++) { - temp = b_A[ix]; - b_A[ix] = b_A[iy]; - b_A[iy] = temp; - ix += 6; - iy += 6; - } - } - - i3 = (c - j) + 6; - for (jy = c + 1; jy + 1 <= i3; jy++) { - b_A[jy] /= b_A[c]; - } - } - - iy = c; - jy = c + 6; - for (k = 1; k <= 5 - j; k++) { - temp = b_A[jy]; - if (b_A[jy] != 0.0F) { - ix = c + 1; - i3 = (iy - j) + 12; - for (ijA = 7 + iy; ijA + 1 <= i3; ijA++) { - b_A[ijA] += b_A[ix] * -temp; - ix++; - } - } - - jy += 6; - iy += 6; - } - } - - for (i3 = 0; i3 < 12; i3++) { - for (iy = 0; iy < 6; iy++) { - Y[iy + 6 * i3] = A[i3 + 12 * iy]; - } - } - - for (jy = 0; jy < 6; jy++) { - if (ipiv[jy] != jy + 1) { - for (j = 0; j < 12; j++) { - temp = Y[jy + 6 * j]; - Y[jy + 6 * j] = Y[(ipiv[jy] + 6 * j) - 1]; - Y[(ipiv[jy] + 6 * j) - 1] = temp; - } - } - } - - for (j = 0; j < 12; j++) { - c = 6 * j; - for (k = 0; k < 6; k++) { - iy = 6 * k; - if (Y[k + c] != 0.0F) { - for (jy = k + 2; jy < 7; jy++) { - Y[(jy + c) - 1] -= Y[k + c] * b_A[(jy + iy) - 1]; - } - } - } - } - - for (j = 0; j < 12; j++) { - c = 6 * j; - for (k = 5; k > -1; k += -1) { - iy = 6 * k; - if (Y[k + c] != 0.0F) { - Y[k + c] /= b_A[k + iy]; - for (jy = 0; jy + 1 <= k; jy++) { - Y[jy + c] -= Y[k + c] * b_A[jy + iy]; - } - } - } - } - - for (i3 = 0; i3 < 6; i3++) { - for (iy = 0; iy < 12; iy++) { - y[iy + 12 * i3] = Y[i3 + 6 * iy]; - } - } -} - -/* - * - */ -void mrdivide(const real32_T A[108], const real32_T B[81], real32_T y[108]) -{ - real32_T b_A[81]; - int8_T ipiv[9]; - int32_T i2; - int32_T iy; - int32_T j; - int32_T c; - int32_T ix; - real32_T temp; - int32_T k; - real32_T s; - int32_T jy; - int32_T ijA; - real32_T Y[108]; - for (i2 = 0; i2 < 9; i2++) { - for (iy = 0; iy < 9; iy++) { - b_A[iy + 9 * i2] = B[i2 + 9 * iy]; - } - - ipiv[i2] = (int8_T)(1 + i2); - } - - for (j = 0; j < 8; j++) { - c = j * 10; - iy = 0; - ix = c; - temp = (real32_T)fabs(b_A[c]); - for (k = 2; k <= 9 - j; k++) { - ix++; - s = (real32_T)fabs(b_A[ix]); - if (s > temp) { - iy = k - 1; - temp = s; - } - } - - if (b_A[c + iy] != 0.0F) { - if (iy != 0) { - ipiv[j] = (int8_T)((j + iy) + 1); - ix = j; - iy += j; - for (k = 0; k < 9; k++) { - temp = b_A[ix]; - b_A[ix] = b_A[iy]; - b_A[iy] = temp; - ix += 9; - iy += 9; - } - } - - i2 = (c - j) + 9; - for (jy = c + 1; jy + 1 <= i2; jy++) { - b_A[jy] /= b_A[c]; - } - } - - iy = c; - jy = c + 9; - for (k = 1; k <= 8 - j; k++) { - temp = b_A[jy]; - if (b_A[jy] != 0.0F) { - ix = c + 1; - i2 = (iy - j) + 18; - for (ijA = 10 + iy; ijA + 1 <= i2; ijA++) { - b_A[ijA] += b_A[ix] * -temp; - ix++; - } - } - - jy += 9; - iy += 9; - } - } - - for (i2 = 0; i2 < 12; i2++) { - for (iy = 0; iy < 9; iy++) { - Y[iy + 9 * i2] = A[i2 + 12 * iy]; - } - } - - for (jy = 0; jy < 9; jy++) { - if (ipiv[jy] != jy + 1) { - for (j = 0; j < 12; j++) { - temp = Y[jy + 9 * j]; - Y[jy + 9 * j] = Y[(ipiv[jy] + 9 * j) - 1]; - Y[(ipiv[jy] + 9 * j) - 1] = temp; - } - } - } - - for (j = 0; j < 12; j++) { - c = 9 * j; - for (k = 0; k < 9; k++) { - iy = 9 * k; - if (Y[k + c] != 0.0F) { - for (jy = k + 2; jy < 10; jy++) { - Y[(jy + c) - 1] -= Y[k + c] * b_A[(jy + iy) - 1]; - } - } - } - } - - for (j = 0; j < 12; j++) { - c = 9 * j; - for (k = 8; k > -1; k += -1) { - iy = 9 * k; - if (Y[k + c] != 0.0F) { - Y[k + c] /= b_A[k + iy]; - for (jy = 0; jy + 1 <= k; jy++) { - Y[jy + c] -= Y[k + c] * b_A[jy + iy]; - } - } - } - } - - for (i2 = 0; i2 < 9; i2++) { - for (iy = 0; iy < 12; iy++) { - y[iy + 12 * i2] = Y[i2 + 9 * iy]; - } - } -} - -/* End of code generation (mrdivide.c) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/mrdivide.h b/src/modules/attitude_estimator_ekf/codegen/mrdivide.h deleted file mode 100755 index 2d3b0d51ff..0000000000 --- a/src/modules/attitude_estimator_ekf/codegen/mrdivide.h +++ /dev/null @@ -1,36 +0,0 @@ -/* - * mrdivide.h - * - * Code generation for function 'mrdivide' - * - * C source code generated on: Sat Jan 19 15:25:29 2013 - * - */ - -#ifndef __MRDIVIDE_H__ -#define __MRDIVIDE_H__ -/* Include files */ -#include -#include -#include -#include -#include "rt_defines.h" -#include "rt_nonfinite.h" - -#include "rtwtypes.h" -#include "attitudeKalmanfilter_types.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ -extern void b_mrdivide(const real32_T A[36], const real32_T B[9], real32_T y[36]); -extern void c_mrdivide(const real32_T A[72], const real32_T B[36], real32_T y[72]); -extern void mrdivide(const real32_T A[108], const real32_T B[81], real32_T y[108]); -#endif -/* End of code generation (mrdivide.h) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/norm.c b/src/modules/attitude_estimator_ekf/codegen/norm.c deleted file mode 100755 index 0c418cc7b6..0000000000 --- a/src/modules/attitude_estimator_ekf/codegen/norm.c +++ /dev/null @@ -1,54 +0,0 @@ -/* - * norm.c - * - * Code generation for function 'norm' - * - * C source code generated on: Sat Jan 19 15:25:29 2013 - * - */ - -/* Include files */ -#include "rt_nonfinite.h" -#include "attitudeKalmanfilter.h" -#include "norm.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ - -/* Function Definitions */ - -/* - * - */ -real32_T norm(const real32_T x[3]) -{ - real32_T y; - real32_T scale; - int32_T k; - real32_T absxk; - real32_T t; - y = 0.0F; - scale = 1.17549435E-38F; - for (k = 0; k < 3; k++) { - absxk = (real32_T)fabs(x[k]); - if (absxk > scale) { - t = scale / absxk; - y = 1.0F + y * t * t; - scale = absxk; - } else { - t = absxk / scale; - y += t * t; - } - } - - return scale * (real32_T)sqrt(y); -} - -/* End of code generation (norm.c) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/norm.h b/src/modules/attitude_estimator_ekf/codegen/norm.h deleted file mode 100755 index 60cf77b571..0000000000 --- a/src/modules/attitude_estimator_ekf/codegen/norm.h +++ /dev/null @@ -1,34 +0,0 @@ -/* - * norm.h - * - * Code generation for function 'norm' - * - * C source code generated on: Sat Jan 19 15:25:29 2013 - * - */ - -#ifndef __NORM_H__ -#define __NORM_H__ -/* Include files */ -#include -#include -#include -#include -#include "rt_defines.h" -#include "rt_nonfinite.h" - -#include "rtwtypes.h" -#include "attitudeKalmanfilter_types.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ -extern real32_T norm(const real32_T x[3]); -#endif -/* End of code generation (norm.h) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/rdivide.c b/src/modules/attitude_estimator_ekf/codegen/rdivide.c deleted file mode 100755 index d035dae5e1..0000000000 --- a/src/modules/attitude_estimator_ekf/codegen/rdivide.c +++ /dev/null @@ -1,38 +0,0 @@ -/* - * rdivide.c - * - * Code generation for function 'rdivide' - * - * C source code generated on: Sat Jan 19 15:25:29 2013 - * - */ - -/* Include files */ -#include "rt_nonfinite.h" -#include "attitudeKalmanfilter.h" -#include "rdivide.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ - -/* Function Definitions */ - -/* - * - */ -void rdivide(const real32_T x[3], real32_T y, real32_T z[3]) -{ - int32_T i; - for (i = 0; i < 3; i++) { - z[i] = x[i] / y; - } -} - -/* End of code generation (rdivide.c) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/rdivide.h b/src/modules/attitude_estimator_ekf/codegen/rdivide.h deleted file mode 100755 index 4bbebebe28..0000000000 --- a/src/modules/attitude_estimator_ekf/codegen/rdivide.h +++ /dev/null @@ -1,34 +0,0 @@ -/* - * rdivide.h - * - * Code generation for function 'rdivide' - * - * C source code generated on: Sat Jan 19 15:25:29 2013 - * - */ - -#ifndef __RDIVIDE_H__ -#define __RDIVIDE_H__ -/* Include files */ -#include -#include -#include -#include -#include "rt_defines.h" -#include "rt_nonfinite.h" - -#include "rtwtypes.h" -#include "attitudeKalmanfilter_types.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ -extern void rdivide(const real32_T x[3], real32_T y, real32_T z[3]); -#endif -/* End of code generation (rdivide.h) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/rtGetInf.c b/src/modules/attitude_estimator_ekf/codegen/rtGetInf.c deleted file mode 100755 index 34164d1044..0000000000 --- a/src/modules/attitude_estimator_ekf/codegen/rtGetInf.c +++ /dev/null @@ -1,139 +0,0 @@ -/* - * rtGetInf.c - * - * Code generation for function 'attitudeKalmanfilter' - * - * C source code generated on: Sat Jan 19 15:25:29 2013 - * - */ - -/* - * Abstract: - * MATLAB for code generation function to initialize non-finite, Inf and MinusInf - */ -#include "rtGetInf.h" -#define NumBitsPerChar 8U - -/* Function: rtGetInf ================================================== - * Abstract: - * Initialize rtInf needed by the generated code. - * Inf is initialized as non-signaling. Assumes IEEE. - */ -real_T rtGetInf(void) -{ - size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar); - real_T inf = 0.0; - if (bitsPerReal == 32U) { - inf = rtGetInfF(); - } else { - uint16_T one = 1U; - enum { - LittleEndian, - BigEndian - } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian; - switch (machByteOrder) { - case LittleEndian: - { - union { - LittleEndianIEEEDouble bitVal; - real_T fltVal; - } tmpVal; - - tmpVal.bitVal.words.wordH = 0x7FF00000U; - tmpVal.bitVal.words.wordL = 0x00000000U; - inf = tmpVal.fltVal; - break; - } - - case BigEndian: - { - union { - BigEndianIEEEDouble bitVal; - real_T fltVal; - } tmpVal; - - tmpVal.bitVal.words.wordH = 0x7FF00000U; - tmpVal.bitVal.words.wordL = 0x00000000U; - inf = tmpVal.fltVal; - break; - } - } - } - - return inf; -} - -/* Function: rtGetInfF ================================================== - * Abstract: - * Initialize rtInfF needed by the generated code. - * Inf is initialized as non-signaling. Assumes IEEE. - */ -real32_T rtGetInfF(void) -{ - IEEESingle infF; - infF.wordL.wordLuint = 0x7F800000U; - return infF.wordL.wordLreal; -} - -/* Function: rtGetMinusInf ================================================== - * Abstract: - * Initialize rtMinusInf needed by the generated code. - * Inf is initialized as non-signaling. Assumes IEEE. - */ -real_T rtGetMinusInf(void) -{ - size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar); - real_T minf = 0.0; - if (bitsPerReal == 32U) { - minf = rtGetMinusInfF(); - } else { - uint16_T one = 1U; - enum { - LittleEndian, - BigEndian - } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian; - switch (machByteOrder) { - case LittleEndian: - { - union { - LittleEndianIEEEDouble bitVal; - real_T fltVal; - } tmpVal; - - tmpVal.bitVal.words.wordH = 0xFFF00000U; - tmpVal.bitVal.words.wordL = 0x00000000U; - minf = tmpVal.fltVal; - break; - } - - case BigEndian: - { - union { - BigEndianIEEEDouble bitVal; - real_T fltVal; - } tmpVal; - - tmpVal.bitVal.words.wordH = 0xFFF00000U; - tmpVal.bitVal.words.wordL = 0x00000000U; - minf = tmpVal.fltVal; - break; - } - } - } - - return minf; -} - -/* Function: rtGetMinusInfF ================================================== - * Abstract: - * Initialize rtMinusInfF needed by the generated code. - * Inf is initialized as non-signaling. Assumes IEEE. - */ -real32_T rtGetMinusInfF(void) -{ - IEEESingle minfF; - minfF.wordL.wordLuint = 0xFF800000U; - return minfF.wordL.wordLreal; -} - -/* End of code generation (rtGetInf.c) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/rtGetInf.h b/src/modules/attitude_estimator_ekf/codegen/rtGetInf.h deleted file mode 100755 index 145373cd02..0000000000 --- a/src/modules/attitude_estimator_ekf/codegen/rtGetInf.h +++ /dev/null @@ -1,23 +0,0 @@ -/* - * rtGetInf.h - * - * Code generation for function 'attitudeKalmanfilter' - * - * C source code generated on: Sat Jan 19 15:25:29 2013 - * - */ - -#ifndef __RTGETINF_H__ -#define __RTGETINF_H__ - -#include -#include "rtwtypes.h" -#include "rt_nonfinite.h" - -extern real_T rtGetInf(void); -extern real32_T rtGetInfF(void); -extern real_T rtGetMinusInf(void); -extern real32_T rtGetMinusInfF(void); - -#endif -/* End of code generation (rtGetInf.h) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/rtGetNaN.c b/src/modules/attitude_estimator_ekf/codegen/rtGetNaN.c deleted file mode 100755 index d84ca9573e..0000000000 --- a/src/modules/attitude_estimator_ekf/codegen/rtGetNaN.c +++ /dev/null @@ -1,96 +0,0 @@ -/* - * rtGetNaN.c - * - * Code generation for function 'attitudeKalmanfilter' - * - * C source code generated on: Sat Jan 19 15:25:29 2013 - * - */ - -/* - * Abstract: - * MATLAB for code generation function to initialize non-finite, NaN - */ -#include "rtGetNaN.h" -#define NumBitsPerChar 8U - -/* Function: rtGetNaN ================================================== - * Abstract: - * Initialize rtNaN needed by the generated code. - * NaN is initialized as non-signaling. Assumes IEEE. - */ -real_T rtGetNaN(void) -{ - size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar); - real_T nan = 0.0; - if (bitsPerReal == 32U) { - nan = rtGetNaNF(); - } else { - uint16_T one = 1U; - enum { - LittleEndian, - BigEndian - } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian; - switch (machByteOrder) { - case LittleEndian: - { - union { - LittleEndianIEEEDouble bitVal; - real_T fltVal; - } tmpVal; - - tmpVal.bitVal.words.wordH = 0xFFF80000U; - tmpVal.bitVal.words.wordL = 0x00000000U; - nan = tmpVal.fltVal; - break; - } - - case BigEndian: - { - union { - BigEndianIEEEDouble bitVal; - real_T fltVal; - } tmpVal; - - tmpVal.bitVal.words.wordH = 0x7FFFFFFFU; - tmpVal.bitVal.words.wordL = 0xFFFFFFFFU; - nan = tmpVal.fltVal; - break; - } - } - } - - return nan; -} - -/* Function: rtGetNaNF ================================================== - * Abstract: - * Initialize rtNaNF needed by the generated code. - * NaN is initialized as non-signaling. Assumes IEEE. - */ -real32_T rtGetNaNF(void) -{ - IEEESingle nanF = { { 0 } }; - uint16_T one = 1U; - enum { - LittleEndian, - BigEndian - } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian; - switch (machByteOrder) { - case LittleEndian: - { - nanF.wordL.wordLuint = 0xFFC00000U; - break; - } - - case BigEndian: - { - nanF.wordL.wordLuint = 0x7FFFFFFFU; - break; - } - } - - return nanF.wordL.wordLreal; -} - -/* End of code generation (rtGetNaN.c) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/rtGetNaN.h b/src/modules/attitude_estimator_ekf/codegen/rtGetNaN.h deleted file mode 100755 index 65fdaa96f6..0000000000 --- a/src/modules/attitude_estimator_ekf/codegen/rtGetNaN.h +++ /dev/null @@ -1,21 +0,0 @@ -/* - * rtGetNaN.h - * - * Code generation for function 'attitudeKalmanfilter' - * - * C source code generated on: Sat Jan 19 15:25:29 2013 - * - */ - -#ifndef __RTGETNAN_H__ -#define __RTGETNAN_H__ - -#include -#include "rtwtypes.h" -#include "rt_nonfinite.h" - -extern real_T rtGetNaN(void); -extern real32_T rtGetNaNF(void); - -#endif -/* End of code generation (rtGetNaN.h) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/rt_defines.h b/src/modules/attitude_estimator_ekf/codegen/rt_defines.h deleted file mode 100755 index 3564983630..0000000000 --- a/src/modules/attitude_estimator_ekf/codegen/rt_defines.h +++ /dev/null @@ -1,24 +0,0 @@ -/* - * rt_defines.h - * - * Code generation for function 'attitudeKalmanfilter' - * - * C source code generated on: Sat Jan 19 15:25:29 2013 - * - */ - -#ifndef __RT_DEFINES_H__ -#define __RT_DEFINES_H__ - -#include - -#define RT_PI 3.14159265358979323846 -#define RT_PIF 3.1415927F -#define RT_LN_10 2.30258509299404568402 -#define RT_LN_10F 2.3025851F -#define RT_LOG10E 0.43429448190325182765 -#define RT_LOG10EF 0.43429449F -#define RT_E 2.7182818284590452354 -#define RT_EF 2.7182817F -#endif -/* End of code generation (rt_defines.h) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/rt_nonfinite.c b/src/modules/attitude_estimator_ekf/codegen/rt_nonfinite.c deleted file mode 100755 index 303d1d9d2e..0000000000 --- a/src/modules/attitude_estimator_ekf/codegen/rt_nonfinite.c +++ /dev/null @@ -1,87 +0,0 @@ -/* - * rt_nonfinite.c - * - * Code generation for function 'attitudeKalmanfilter' - * - * C source code generated on: Sat Jan 19 15:25:29 2013 - * - */ - -/* - * Abstract: - * MATLAB for code generation function to initialize non-finites, - * (Inf, NaN and -Inf). - */ -#include "rt_nonfinite.h" -#include "rtGetNaN.h" -#include "rtGetInf.h" - -real_T rtInf; -real_T rtMinusInf; -real_T rtNaN; -real32_T rtInfF; -real32_T rtMinusInfF; -real32_T rtNaNF; - -/* Function: rt_InitInfAndNaN ================================================== - * Abstract: - * Initialize the rtInf, rtMinusInf, and rtNaN needed by the - * generated code. NaN is initialized as non-signaling. Assumes IEEE. - */ -void rt_InitInfAndNaN(size_t realSize) -{ - (void) (realSize); - rtNaN = rtGetNaN(); - rtNaNF = rtGetNaNF(); - rtInf = rtGetInf(); - rtInfF = rtGetInfF(); - rtMinusInf = rtGetMinusInf(); - rtMinusInfF = rtGetMinusInfF(); -} - -/* Function: rtIsInf ================================================== - * Abstract: - * Test if value is infinite - */ -boolean_T rtIsInf(real_T value) -{ - return ((value==rtInf || value==rtMinusInf) ? 1U : 0U); -} - -/* Function: rtIsInfF ================================================= - * Abstract: - * Test if single-precision value is infinite - */ -boolean_T rtIsInfF(real32_T value) -{ - return(((value)==rtInfF || (value)==rtMinusInfF) ? 1U : 0U); -} - -/* Function: rtIsNaN ================================================== - * Abstract: - * Test if value is not a number - */ -boolean_T rtIsNaN(real_T value) -{ -#if defined(_MSC_VER) && (_MSC_VER <= 1200) - return _isnan(value)? TRUE:FALSE; -#else - return (value!=value)? 1U:0U; -#endif -} - -/* Function: rtIsNaNF ================================================= - * Abstract: - * Test if single-precision value is not a number - */ -boolean_T rtIsNaNF(real32_T value) -{ -#if defined(_MSC_VER) && (_MSC_VER <= 1200) - return _isnan((real_T)value)? true:false; -#else - return (value!=value)? 1U:0U; -#endif -} - - -/* End of code generation (rt_nonfinite.c) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/rt_nonfinite.h b/src/modules/attitude_estimator_ekf/codegen/rt_nonfinite.h deleted file mode 100755 index bd56b30d94..0000000000 --- a/src/modules/attitude_estimator_ekf/codegen/rt_nonfinite.h +++ /dev/null @@ -1,53 +0,0 @@ -/* - * rt_nonfinite.h - * - * Code generation for function 'attitudeKalmanfilter' - * - * C source code generated on: Sat Jan 19 15:25:29 2013 - * - */ - -#ifndef __RT_NONFINITE_H__ -#define __RT_NONFINITE_H__ - -#if defined(_MSC_VER) && (_MSC_VER <= 1200) -#include -#endif -#include -#include "rtwtypes.h" - -extern real_T rtInf; -extern real_T rtMinusInf; -extern real_T rtNaN; -extern real32_T rtInfF; -extern real32_T rtMinusInfF; -extern real32_T rtNaNF; -extern void rt_InitInfAndNaN(size_t realSize); -extern boolean_T rtIsInf(real_T value); -extern boolean_T rtIsInfF(real32_T value); -extern boolean_T rtIsNaN(real_T value); -extern boolean_T rtIsNaNF(real32_T value); - -typedef struct { - struct { - uint32_T wordH; - uint32_T wordL; - } words; -} BigEndianIEEEDouble; - -typedef struct { - struct { - uint32_T wordL; - uint32_T wordH; - } words; -} LittleEndianIEEEDouble; - -typedef struct { - union { - real32_T wordLreal; - uint32_T wordLuint; - } wordL; -} IEEESingle; - -#endif -/* End of code generation (rt_nonfinite.h) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/rtwtypes.h b/src/modules/attitude_estimator_ekf/codegen/rtwtypes.h old mode 100755 new mode 100644 index 9a5c96267e..9245a24c8f --- a/src/modules/attitude_estimator_ekf/codegen/rtwtypes.h +++ b/src/modules/attitude_estimator_ekf/codegen/rtwtypes.h @@ -1,159 +1,160 @@ -/* - * rtwtypes.h - * - * Code generation for function 'attitudeKalmanfilter' - * - * C source code generated on: Sat Jan 19 15:25:29 2013 - * - */ - -#ifndef __RTWTYPES_H__ -#define __RTWTYPES_H__ -#ifndef TRUE -# define TRUE (1U) -#endif -#ifndef FALSE -# define FALSE (0U) -#endif -#ifndef __TMWTYPES__ -#define __TMWTYPES__ - -#include - -/*=======================================================================* - * Target hardware information - * Device type: Generic->MATLAB Host Computer - * Number of bits: char: 8 short: 16 int: 32 - * long: 32 native word size: 32 - * Byte ordering: LittleEndian - * Signed integer division rounds to: Zero - * Shift right on a signed integer as arithmetic shift: on - *=======================================================================*/ - -/*=======================================================================* - * Fixed width word size data types: * - * int8_T, int16_T, int32_T - signed 8, 16, or 32 bit integers * - * uint8_T, uint16_T, uint32_T - unsigned 8, 16, or 32 bit integers * - * real32_T, real64_T - 32 and 64 bit floating point numbers * - *=======================================================================*/ - -typedef signed char int8_T; -typedef unsigned char uint8_T; -typedef short int16_T; -typedef unsigned short uint16_T; -typedef int int32_T; -typedef unsigned int uint32_T; -typedef float real32_T; -typedef double real64_T; - -/*===========================================================================* - * Generic type definitions: real_T, time_T, boolean_T, int_T, uint_T, * - * ulong_T, char_T and byte_T. * - *===========================================================================*/ - -typedef double real_T; -typedef double time_T; -typedef unsigned char boolean_T; -typedef int int_T; -typedef unsigned int uint_T; -typedef unsigned long ulong_T; -typedef char char_T; -typedef char_T byte_T; - -/*===========================================================================* - * Complex number type definitions * - *===========================================================================*/ -#define CREAL_T - typedef struct { - real32_T re; - real32_T im; - } creal32_T; - - typedef struct { - real64_T re; - real64_T im; - } creal64_T; - - typedef struct { - real_T re; - real_T im; - } creal_T; - - typedef struct { - int8_T re; - int8_T im; - } cint8_T; - - typedef struct { - uint8_T re; - uint8_T im; - } cuint8_T; - - typedef struct { - int16_T re; - int16_T im; - } cint16_T; - - typedef struct { - uint16_T re; - uint16_T im; - } cuint16_T; - - typedef struct { - int32_T re; - int32_T im; - } cint32_T; - - typedef struct { - uint32_T re; - uint32_T im; - } cuint32_T; - - -/*=======================================================================* - * Min and Max: * - * int8_T, int16_T, int32_T - signed 8, 16, or 32 bit integers * - * uint8_T, uint16_T, uint32_T - unsigned 8, 16, or 32 bit integers * - *=======================================================================*/ - -#define MAX_int8_T ((int8_T)(127)) -#define MIN_int8_T ((int8_T)(-128)) -#define MAX_uint8_T ((uint8_T)(255)) -#define MIN_uint8_T ((uint8_T)(0)) -#define MAX_int16_T ((int16_T)(32767)) -#define MIN_int16_T ((int16_T)(-32768)) -#define MAX_uint16_T ((uint16_T)(65535)) -#define MIN_uint16_T ((uint16_T)(0)) -#define MAX_int32_T ((int32_T)(2147483647)) -#define MIN_int32_T ((int32_T)(-2147483647-1)) -#define MAX_uint32_T ((uint32_T)(0xFFFFFFFFU)) -#define MIN_uint32_T ((uint32_T)(0)) - -/* Logical type definitions */ -#if !defined(__cplusplus) && !defined(__true_false_are_keywords) -# ifndef false -# define false (0U) -# endif -# ifndef true -# define true (1U) -# endif -#endif - -/* - * MATLAB for code generation assumes the code is compiled on a target using a 2's compliment representation - * for signed integer values. - */ -#if ((SCHAR_MIN + 1) != -SCHAR_MAX) -#error "This code must be compiled using a 2's complement representation for signed integer values" -#endif - -/* - * Maximum length of a MATLAB identifier (function/variable) - * including the null-termination character. Referenced by - * rt_logging.c and rt_matrx.c. - */ -#define TMW_NAME_LENGTH_MAX 64 - -#endif -#endif -/* End of code generation (rtwtypes.h) */ +/* + * rtwtypes.h + * + * Code generation for function 'AttitudeEKF' + * + * C source code generated on: Fri Jul 25 14:06:41 2014 + * + */ + +#ifndef __RTWTYPES_H__ +#define __RTWTYPES_H__ +#ifndef TRUE +# define TRUE (1U) +#endif +#ifndef FALSE +# define FALSE (0U) +#endif +#ifndef __TMWTYPES__ +#define __TMWTYPES__ + +#include + +/*=======================================================================* + * Target hardware information + * Device type: ARM Compatible->ARM Cortex + * Number of bits: char: 8 short: 16 int: 32 + * long: 32 + * native word size: 32 + * Byte ordering: LittleEndian + * Signed integer division rounds to: Undefined + * Shift right on a signed integer as arithmetic shift: on + *=======================================================================*/ + +/*=======================================================================* + * Fixed width word size data types: * + * int8_T, int16_T, int32_T - signed 8, 16, or 32 bit integers * + * uint8_T, uint16_T, uint32_T - unsigned 8, 16, or 32 bit integers * + * real32_T, real64_T - 32 and 64 bit floating point numbers * + *=======================================================================*/ + +typedef signed char int8_T; +typedef unsigned char uint8_T; +typedef short int16_T; +typedef unsigned short uint16_T; +typedef int int32_T; +typedef unsigned int uint32_T; +typedef float real32_T; +typedef double real64_T; + +/*===========================================================================* + * Generic type definitions: real_T, time_T, boolean_T, int_T, uint_T, * + * ulong_T, char_T and byte_T. * + *===========================================================================*/ + +typedef double real_T; +typedef double time_T; +typedef unsigned char boolean_T; +typedef int int_T; +typedef unsigned int uint_T; +typedef unsigned long ulong_T; +typedef char char_T; +typedef char_T byte_T; + +/*===========================================================================* + * Complex number type definitions * + *===========================================================================*/ +#define CREAL_T + typedef struct { + real32_T re; + real32_T im; + } creal32_T; + + typedef struct { + real64_T re; + real64_T im; + } creal64_T; + + typedef struct { + real_T re; + real_T im; + } creal_T; + + typedef struct { + int8_T re; + int8_T im; + } cint8_T; + + typedef struct { + uint8_T re; + uint8_T im; + } cuint8_T; + + typedef struct { + int16_T re; + int16_T im; + } cint16_T; + + typedef struct { + uint16_T re; + uint16_T im; + } cuint16_T; + + typedef struct { + int32_T re; + int32_T im; + } cint32_T; + + typedef struct { + uint32_T re; + uint32_T im; + } cuint32_T; + + +/*=======================================================================* + * Min and Max: * + * int8_T, int16_T, int32_T - signed 8, 16, or 32 bit integers * + * uint8_T, uint16_T, uint32_T - unsigned 8, 16, or 32 bit integers * + *=======================================================================*/ + +#define MAX_int8_T ((int8_T)(127)) +#define MIN_int8_T ((int8_T)(-128)) +#define MAX_uint8_T ((uint8_T)(255)) +#define MIN_uint8_T ((uint8_T)(0)) +#define MAX_int16_T ((int16_T)(32767)) +#define MIN_int16_T ((int16_T)(-32768)) +#define MAX_uint16_T ((uint16_T)(65535)) +#define MIN_uint16_T ((uint16_T)(0)) +#define MAX_int32_T ((int32_T)(2147483647)) +#define MIN_int32_T ((int32_T)(-2147483647-1)) +#define MAX_uint32_T ((uint32_T)(0xFFFFFFFFU)) +#define MIN_uint32_T ((uint32_T)(0)) + +/* Logical type definitions */ +#if !defined(__cplusplus) && !defined(__true_false_are_keywords) +# ifndef false +# define false (0U) +# endif +# ifndef true +# define true (1U) +# endif +#endif + +/* + * MATLAB for code generation assumes the code is compiled on a target using a 2's compliment representation + * for signed integer values. + */ +#if ((SCHAR_MIN + 1) != -SCHAR_MAX) +#error "This code must be compiled using a 2's complement representation for signed integer values" +#endif + +/* + * Maximum length of a MATLAB identifier (function/variable) + * including the null-termination character. Referenced by + * rt_logging.c and rt_matrx.c. + */ +#define TMW_NAME_LENGTH_MAX 64 + +#endif +#endif +/* End of code generation (rtwtypes.h) */ diff --git a/src/modules/attitude_estimator_ekf/module.mk b/src/modules/attitude_estimator_ekf/module.mk index 99d0c5bf28..749b0a91c3 100644 --- a/src/modules/attitude_estimator_ekf/module.mk +++ b/src/modules/attitude_estimator_ekf/module.mk @@ -39,16 +39,6 @@ MODULE_COMMAND = attitude_estimator_ekf SRCS = attitude_estimator_ekf_main.cpp \ attitude_estimator_ekf_params.c \ - codegen/eye.c \ - codegen/attitudeKalmanfilter.c \ - codegen/mrdivide.c \ - codegen/rdivide.c \ - codegen/attitudeKalmanfilter_initialize.c \ - codegen/attitudeKalmanfilter_terminate.c \ - codegen/rt_nonfinite.c \ - codegen/rtGetInf.c \ - codegen/rtGetNaN.c \ - codegen/norm.c \ - codegen/cross.c + codegen/AttitudeEKF.c MODULE_STACKSIZE = 1200 From 63fa17ef0dd4b0a184f6e3e298113bb143b2cb44 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 25 Jul 2014 15:23:26 +0200 Subject: [PATCH 003/216] att ekf: add param to enable/disable J --- .../attitude_estimator_ekf_main.cpp | 2 +- .../attitude_estimator_ekf_params.c | 19 +++++++++++++++---- .../attitude_estimator_ekf_params.h | 2 ++ 3 files changed, 18 insertions(+), 5 deletions(-) diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index e1bbf5bc7a..667b74d1d7 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -512,7 +512,7 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds /* Call the estimator */ AttitudeEKF(false, // approx_prediction - false, // use_inertia_matrix + (unsigned char)ekf_params.use_moment_inertia, update_vect, dt, z_k, diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c index 5b57bfb4db..5c33bc2ac7 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c @@ -79,7 +79,6 @@ PARAM_DEFINE_FLOAT(ATT_J11, 0.0018); */ PARAM_DEFINE_FLOAT(ATT_J22, 0.0018); - /* * Moment of inertia matrix diagonal entry (3, 3) * @@ -88,6 +87,16 @@ PARAM_DEFINE_FLOAT(ATT_J22, 0.0018); */ PARAM_DEFINE_FLOAT(ATT_J33, 0.0037); +/* + * Moment of inertia enabled in estimator + * + * If set to != 0 the moment of inertia will be used in the estimator + * + * @group attitude_ekf + * @min 0 + * @max 1 + */ +PARAM_DEFINE_INT32(ATT_J_EN, 0); int parameters_init(struct attitude_estimator_ekf_param_handles *h) { @@ -105,9 +114,10 @@ int parameters_init(struct attitude_estimator_ekf_param_handles *h) h->acc_comp = param_find("ATT_ACC_COMP"); - h->moment_inertia_J[0] = param_find("ATT_J11"); - h->moment_inertia_J[1] = param_find("ATT_J22"); - h->moment_inertia_J[2] = param_find("ATT_J33"); + h->moment_inertia_J[0] = param_find("ATT_J11"); + h->moment_inertia_J[1] = param_find("ATT_J22"); + h->moment_inertia_J[2] = param_find("ATT_J33"); + h->use_moment_inertia = param_find("ATT_J_EN"); return OK; } @@ -131,6 +141,7 @@ int parameters_update(const struct attitude_estimator_ekf_param_handles *h, stru for (int i = 0; i < 3; i++) { param_get(h->moment_inertia_J[i], &(p->moment_inertia_J[3 * i + i])); } + param_get(h->use_moment_inertia, &(p->use_moment_inertia)); return OK; } diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h index fbb6a18ff4..5d3b6b2440 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h @@ -45,6 +45,7 @@ struct attitude_estimator_ekf_params { float r[3]; float q[4]; float moment_inertia_J[9]; + int32_t use_moment_inertia; float roll_off; float pitch_off; float yaw_off; @@ -56,6 +57,7 @@ struct attitude_estimator_ekf_param_handles { param_t r0, r1, r2; param_t q0, q1, q2, q3; param_t moment_inertia_J[3]; /**< diagonal entries of the matrix */ + param_t use_moment_inertia; param_t mag_decl; param_t acc_comp; }; From 963394f0e43a57dd765541b9c226b5cbf70c9073 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 25 Jul 2014 15:42:01 +0200 Subject: [PATCH 004/216] att ekf: add descriptions for params --- .../attitude_estimator_ekf_params.c | 54 +++++++++++++++---- 1 file changed, 45 insertions(+), 9 deletions(-) diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c index 5c33bc2ac7..5637ec102f 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c @@ -44,18 +44,54 @@ /* Extended Kalman Filter covariances */ -/* gyro process noise */ + +/** + * Body angular rate process noise + * + * @group attitude_ekf + */ PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q0, 1e-4f); + +/** + * Body angular acceleration process noise + * + * @group attitude_ekf + */ PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q1, 0.08f); + +/** + * Acceleration process noise + * + * @group attitude_ekf + */ PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q2, 0.009f); -/* gyro offsets process noise */ + +/** + * Magnet field vector process noise + * + * @group attitude_ekf + */ PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q3, 0.005f); -/* gyro measurement noise */ +/** + * Gyro measurement noise + * + * @group attitude_ekf + */ PARAM_DEFINE_FLOAT(EKF_ATT_V4_R0, 0.0008f); -/* accel measurement noise */ + +/** + * Accel measurement noise + * + * @group attitude_ekf + */ PARAM_DEFINE_FLOAT(EKF_ATT_V4_R1, 10000.0f); -/* mag measurement noise */ + +/** + * Mag measurement noise + * + * @group attitude_ekf + */ PARAM_DEFINE_FLOAT(EKF_ATT_V4_R2, 100.0f); /* magnetic declination, in degrees */ @@ -63,7 +99,7 @@ PARAM_DEFINE_FLOAT(ATT_MAG_DECL, 0.0f); PARAM_DEFINE_INT32(ATT_ACC_COMP, 2); -/* +/** * Moment of inertia matrix diagonal entry (1, 1) * * @group attitude_ekf @@ -71,7 +107,7 @@ PARAM_DEFINE_INT32(ATT_ACC_COMP, 2); */ PARAM_DEFINE_FLOAT(ATT_J11, 0.0018); -/* +/** * Moment of inertia matrix diagonal entry (2, 2) * * @group attitude_ekf @@ -79,7 +115,7 @@ PARAM_DEFINE_FLOAT(ATT_J11, 0.0018); */ PARAM_DEFINE_FLOAT(ATT_J22, 0.0018); -/* +/** * Moment of inertia matrix diagonal entry (3, 3) * * @group attitude_ekf @@ -87,7 +123,7 @@ PARAM_DEFINE_FLOAT(ATT_J22, 0.0018); */ PARAM_DEFINE_FLOAT(ATT_J33, 0.0037); -/* +/** * Moment of inertia enabled in estimator * * If set to != 0 the moment of inertia will be used in the estimator From 58aabe648acf296267bd1e29e8a02b33ea9e7cf7 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 21 Aug 2014 11:25:09 +0200 Subject: [PATCH 005/216] update attitude estimator ekf to latest version mainly saves stack size --- .../attitude_estimator_ekf/AttitudeEKF.m | 62 +- .../attitudeKalmanfilter.prj | 7 +- .../attitude_estimator_ekf_main.cpp | 2 +- .../codegen/AttitudeEKF.c | 629 +++++++----------- .../codegen/AttitudeEKF.h | 2 +- .../codegen/AttitudeEKF_types.h | 2 +- .../attitude_estimator_ekf/codegen/rtwtypes.h | 2 +- 7 files changed, 300 insertions(+), 406 deletions(-) diff --git a/src/modules/attitude_estimator_ekf/AttitudeEKF.m b/src/modules/attitude_estimator_ekf/AttitudeEKF.m index 1218cb65da..fea1a773e3 100644 --- a/src/modules/attitude_estimator_ekf/AttitudeEKF.m +++ b/src/modules/attitude_estimator_ekf/AttitudeEKF.m @@ -166,15 +166,16 @@ P_apr=A_lin*P_apo*A_lin'+Q; %% update if zFlag(1)==1&&zFlag(2)==1&&zFlag(3)==1 - R=[r_gyro,0,0,0,0,0,0,0,0; - 0,r_gyro,0,0,0,0,0,0,0; - 0,0,r_gyro,0,0,0,0,0,0; - 0,0,0,r_accel,0,0,0,0,0; - 0,0,0,0,r_accel,0,0,0,0; - 0,0,0,0,0,r_accel,0,0,0; - 0,0,0,0,0,0,r_mag,0,0; - 0,0,0,0,0,0,0,r_mag,0; - 0,0,0,0,0,0,0,0,r_mag]; +% R=[r_gyro,0,0,0,0,0,0,0,0; +% 0,r_gyro,0,0,0,0,0,0,0; +% 0,0,r_gyro,0,0,0,0,0,0; +% 0,0,0,r_accel,0,0,0,0,0; +% 0,0,0,0,r_accel,0,0,0,0; +% 0,0,0,0,0,r_accel,0,0,0; +% 0,0,0,0,0,0,r_mag,0,0; +% 0,0,0,0,0,0,0,r_mag,0; +% 0,0,0,0,0,0,0,0,r_mag]; + R_v=[r_gyro,r_gyro,r_gyro,r_accel,r_accel,r_accel,r_mag,r_mag,r_mag]; %observation matrix %[zw;ze;zmk]; H_k=[ E, Z, Z, Z; @@ -184,7 +185,9 @@ if zFlag(1)==1&&zFlag(2)==1&&zFlag(3)==1 y_k=z(1:9)-H_k*x_apr; - S_k=H_k*P_apr*H_k'+R; + %S_k=H_k*P_apr*H_k'+R; + S_k=H_k*P_apr*H_k'; + S_k(1:9+1:end) = S_k(1:9+1:end) + R_v; K_k=(P_apr*H_k'/(S_k)); @@ -196,13 +199,16 @@ else R=[r_gyro,0,0; 0,r_gyro,0; 0,0,r_gyro]; + R_v=[r_gyro,r_gyro,r_gyro]; %observation matrix H_k=[ E, Z, Z, Z]; y_k=z(1:3)-H_k(1:3,1:12)*x_apr; - S_k=H_k(1:3,1:12)*P_apr*H_k(1:3,1:12)'+R(1:3,1:3); + % S_k=H_k(1:3,1:12)*P_apr*H_k(1:3,1:12)'+R(1:3,1:3); + S_k=H_k(1:3,1:12)*P_apr*H_k(1:3,1:12)'; + S_k(1:3+1:end) = S_k(1:3+1:end) + R_v; K_k=(P_apr*H_k(1:3,1:12)'/(S_k)); @@ -211,13 +217,14 @@ else else if zFlag(1)==1&&zFlag(2)==1&&zFlag(3)==0 - R=[r_gyro,0,0,0,0,0; - 0,r_gyro,0,0,0,0; - 0,0,r_gyro,0,0,0; - 0,0,0,r_accel,0,0; - 0,0,0,0,r_accel,0; - 0,0,0,0,0,r_accel]; +% R=[r_gyro,0,0,0,0,0; +% 0,r_gyro,0,0,0,0; +% 0,0,r_gyro,0,0,0; +% 0,0,0,r_accel,0,0; +% 0,0,0,0,r_accel,0; +% 0,0,0,0,0,r_accel]; + R_v=[r_gyro,r_gyro,r_gyro,r_accel,r_accel,r_accel]; %observation matrix H_k=[ E, Z, Z, Z; @@ -225,7 +232,9 @@ else y_k=z(1:6)-H_k(1:6,1:12)*x_apr; - S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)'+R(1:6,1:6); + % S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)'+R(1:6,1:6); + S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)'; + S_k(1:6+1:end) = S_k(1:6+1:end) + R_v; K_k=(P_apr*H_k(1:6,1:12)'/(S_k)); @@ -233,12 +242,13 @@ else P_apo=(eye(12)-K_k*H_k(1:6,1:12))*P_apr; else if zFlag(1)==1&&zFlag(2)==0&&zFlag(3)==1 - R=[r_gyro,0,0,0,0,0; - 0,r_gyro,0,0,0,0; - 0,0,r_gyro,0,0,0; - 0,0,0,r_mag,0,0; - 0,0,0,0,r_mag,0; - 0,0,0,0,0,r_mag]; +% R=[r_gyro,0,0,0,0,0; +% 0,r_gyro,0,0,0,0; +% 0,0,r_gyro,0,0,0; +% 0,0,0,r_mag,0,0; +% 0,0,0,0,r_mag,0; +% 0,0,0,0,0,r_mag]; + R_v=[r_gyro,r_gyro,r_gyro,r_mag,r_mag,r_mag]; %observation matrix H_k=[ E, Z, Z, Z; @@ -246,7 +256,9 @@ else y_k=[z(1:3);z(7:9)]-H_k(1:6,1:12)*x_apr; - S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)'+R(1:6,1:6); + %S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)'+R(1:6,1:6); + S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)'; + S_k(1:6+1:end) = S_k(1:6+1:end) + R_v; K_k=(P_apr*H_k(1:6,1:12)'/(S_k)); diff --git a/src/modules/attitude_estimator_ekf/attitudeKalmanfilter.prj b/src/modules/attitude_estimator_ekf/attitudeKalmanfilter.prj index a8315bf571..9ea5203463 100644 --- a/src/modules/attitude_estimator_ekf/attitudeKalmanfilter.prj +++ b/src/modules/attitude_estimator_ekf/attitudeKalmanfilter.prj @@ -1,6 +1,6 @@ - + false false @@ -112,7 +112,7 @@ false option.DynamicMemoryAllocation.Threshold 65536 - 200000 + 4000 false option.FilePartitionMethod.SingleFile true @@ -215,7 +215,6 @@ - @@ -492,7 +491,7 @@ false true false - 3.15.5-1-ARCH + 3.16.1-1-ARCH false true glnxa64 diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index 667b74d1d7..766171a0fd 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -132,7 +132,7 @@ int attitude_estimator_ekf_main(int argc, char *argv[]) attitude_estimator_ekf_task = task_spawn_cmd("attitude_estimator_ekf", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, - 14000, + 7200, attitude_estimator_ekf_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); exit(0); diff --git a/src/modules/attitude_estimator_ekf/codegen/AttitudeEKF.c b/src/modules/attitude_estimator_ekf/codegen/AttitudeEKF.c index d568e57376..68db382cfe 100644 --- a/src/modules/attitude_estimator_ekf/codegen/AttitudeEKF.c +++ b/src/modules/attitude_estimator_ekf/codegen/AttitudeEKF.c @@ -3,7 +3,7 @@ * * Code generation for function 'AttitudeEKF' * - * C source code generated on: Fri Jul 25 14:06:41 2014 + * C source code generated on: Thu Aug 21 11:17:28 2014 * */ @@ -438,67 +438,66 @@ void AttitudeEKF(unsigned char approx_prediction, unsigned char float fv4[3]; float x_apr[12]; signed char I[144]; - float A_lin[144]; + static float A_lin[144]; static const signed char iv1[36] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; - float b_A_lin[144]; + static float b_A_lin[144]; float v[12]; - float P_apr[144]; - float b_P_apr[108]; - static const signed char b[108] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 }; - - float K_k[108]; - float a[81]; + static float P_apr[144]; + float a[108]; static const signed char b_a[108] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 }; - float b_r_gyro[81]; - float c_a[81]; - float d_a[36]; - static const signed char e_a[36] = { 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, + float S_k[81]; + static const signed char b[108] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 }; + + float b_r_gyro[9]; + float K_k[108]; + float b_S_k[36]; + static const signed char c_a[36] = { 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; static const signed char b_b[36] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; - float c_r_gyro[9]; - float b_K_k[36]; + float c_r_gyro[3]; + float B[36]; int r3; float a21; - float f_a[36]; - float c_P_apr[72]; + float Y[36]; + float d_a[72]; + static const signed char e_a[72] = { 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, + 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, + 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0 }; + static const signed char c_b[72] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0 }; - float c_K_k[72]; - static const signed char g_a[72] = { 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, - 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, - 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0 }; + float d_r_gyro[6]; + float c_S_k[6]; + float b_K_k[72]; + static const signed char f_a[72] = { 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, + 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, + 0, 0, 0, 0, 0, 1 }; - float b_z[6]; static const signed char d_b[72] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 }; - static const signed char h_a[72] = { 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, - 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, - 0, 0, 0, 0, 0, 1 }; - - float i_a[6]; - float c_z[6]; + float b_z[6]; /* LQG Postion Estimator and Controller */ /* Observer: */ @@ -898,138 +897,71 @@ void AttitudeEKF(unsigned char approx_prediction, unsigned char /* % update */ /* 'AttitudeEKF:167' if zFlag(1)==1&&zFlag(2)==1&&zFlag(3)==1 */ if ((zFlag[0] == 1) && (zFlag[1] == 1) && (zFlag[2] == 1)) { - /* 'AttitudeEKF:169' R=[r_gyro,0,0,0,0,0,0,0,0; */ - /* 'AttitudeEKF:170' 0,r_gyro,0,0,0,0,0,0,0; */ - /* 'AttitudeEKF:171' 0,0,r_gyro,0,0,0,0,0,0; */ - /* 'AttitudeEKF:172' 0,0,0,r_accel,0,0,0,0,0; */ - /* 'AttitudeEKF:173' 0,0,0,0,r_accel,0,0,0,0; */ - /* 'AttitudeEKF:174' 0,0,0,0,0,r_accel,0,0,0; */ - /* 'AttitudeEKF:175' 0,0,0,0,0,0,r_mag,0,0; */ - /* 'AttitudeEKF:176' 0,0,0,0,0,0,0,r_mag,0; */ - /* 'AttitudeEKF:177' 0,0,0,0,0,0,0,0,r_mag]; */ + /* R=[r_gyro,0,0,0,0,0,0,0,0; */ + /* 0,r_gyro,0,0,0,0,0,0,0; */ + /* 0,0,r_gyro,0,0,0,0,0,0; */ + /* 0,0,0,r_accel,0,0,0,0,0; */ + /* 0,0,0,0,r_accel,0,0,0,0; */ + /* 0,0,0,0,0,r_accel,0,0,0; */ + /* 0,0,0,0,0,0,r_mag,0,0; */ + /* 0,0,0,0,0,0,0,r_mag,0; */ + /* 0,0,0,0,0,0,0,0,r_mag]; */ + /* 'AttitudeEKF:178' R_v=[r_gyro,r_gyro,r_gyro,r_accel,r_accel,r_accel,r_mag,r_mag,r_mag]; */ /* observation matrix */ /* [zw;ze;zmk]; */ - /* 'AttitudeEKF:180' H_k=[ E, Z, Z, Z; */ - /* 'AttitudeEKF:181' Z, Z, E, Z; */ - /* 'AttitudeEKF:182' Z, Z, Z, E]; */ - /* 'AttitudeEKF:184' y_k=z(1:9)-H_k*x_apr; */ - /* 'AttitudeEKF:187' S_k=H_k*P_apr*H_k'+R; */ - /* 'AttitudeEKF:188' K_k=(P_apr*H_k'/(S_k)); */ - for (r2 = 0; r2 < 12; r2++) { - for (i = 0; i < 9; i++) { - b_P_apr[r2 + 12 * i] = 0.0F; - for (r1 = 0; r1 < 12; r1++) { - b_P_apr[r2 + 12 * i] += P_apr[r2 + 12 * r1] * (float)b[r1 + 12 * i]; - } - } - } - + /* 'AttitudeEKF:181' H_k=[ E, Z, Z, Z; */ + /* 'AttitudeEKF:182' Z, Z, E, Z; */ + /* 'AttitudeEKF:183' Z, Z, Z, E]; */ + /* 'AttitudeEKF:185' y_k=z(1:9)-H_k*x_apr; */ + /* S_k=H_k*P_apr*H_k'+R; */ + /* 'AttitudeEKF:189' S_k=H_k*P_apr*H_k'; */ for (r2 = 0; r2 < 9; r2++) { for (i = 0; i < 12; i++) { - K_k[r2 + 9 * i] = 0.0F; - for (r1 = 0; r1 < 12; r1++) { - K_k[r2 + 9 * i] += (float)b_a[r2 + 9 * r1] * P_apr[r1 + 12 * i]; - } - } - - for (i = 0; i < 9; i++) { a[r2 + 9 * i] = 0.0F; for (r1 = 0; r1 < 12; r1++) { - a[r2 + 9 * i] += K_k[r2 + 9 * r1] * (float)b[r1 + 12 * i]; + a[r2 + 9 * i] += (float)b_a[r2 + 9 * r1] * P_apr[r1 + 12 * i]; + } + } + + for (i = 0; i < 9; i++) { + S_k[r2 + 9 * i] = 0.0F; + for (r1 = 0; r1 < 12; r1++) { + S_k[r2 + 9 * i] += a[r2 + 9 * r1] * (float)b[r1 + 12 * i]; } } } + /* 'AttitudeEKF:190' S_k(1:9+1:end) = S_k(1:9+1:end) + R_v; */ b_r_gyro[0] = r_gyro; - b_r_gyro[9] = 0.0F; - b_r_gyro[18] = 0.0F; - b_r_gyro[27] = 0.0F; - b_r_gyro[36] = 0.0F; - b_r_gyro[45] = 0.0F; - b_r_gyro[54] = 0.0F; - b_r_gyro[63] = 0.0F; - b_r_gyro[72] = 0.0F; - b_r_gyro[1] = 0.0F; - b_r_gyro[10] = r_gyro; - b_r_gyro[19] = 0.0F; - b_r_gyro[28] = 0.0F; - b_r_gyro[37] = 0.0F; - b_r_gyro[46] = 0.0F; - b_r_gyro[55] = 0.0F; - b_r_gyro[64] = 0.0F; - b_r_gyro[73] = 0.0F; - b_r_gyro[2] = 0.0F; - b_r_gyro[11] = 0.0F; - b_r_gyro[20] = r_gyro; - b_r_gyro[29] = 0.0F; - b_r_gyro[38] = 0.0F; - b_r_gyro[47] = 0.0F; - b_r_gyro[56] = 0.0F; - b_r_gyro[65] = 0.0F; - b_r_gyro[74] = 0.0F; - b_r_gyro[3] = 0.0F; - b_r_gyro[12] = 0.0F; - b_r_gyro[21] = 0.0F; - b_r_gyro[30] = r_accel; - b_r_gyro[39] = 0.0F; - b_r_gyro[48] = 0.0F; - b_r_gyro[57] = 0.0F; - b_r_gyro[66] = 0.0F; - b_r_gyro[75] = 0.0F; - b_r_gyro[4] = 0.0F; - b_r_gyro[13] = 0.0F; - b_r_gyro[22] = 0.0F; - b_r_gyro[31] = 0.0F; - b_r_gyro[40] = r_accel; - b_r_gyro[49] = 0.0F; - b_r_gyro[58] = 0.0F; - b_r_gyro[67] = 0.0F; - b_r_gyro[76] = 0.0F; - b_r_gyro[5] = 0.0F; - b_r_gyro[14] = 0.0F; - b_r_gyro[23] = 0.0F; - b_r_gyro[32] = 0.0F; - b_r_gyro[41] = 0.0F; - b_r_gyro[50] = r_accel; - b_r_gyro[59] = 0.0F; - b_r_gyro[68] = 0.0F; - b_r_gyro[77] = 0.0F; - b_r_gyro[6] = 0.0F; - b_r_gyro[15] = 0.0F; - b_r_gyro[24] = 0.0F; - b_r_gyro[33] = 0.0F; - b_r_gyro[42] = 0.0F; - b_r_gyro[51] = 0.0F; - b_r_gyro[60] = r_mag; - b_r_gyro[69] = 0.0F; - b_r_gyro[78] = 0.0F; - b_r_gyro[7] = 0.0F; - b_r_gyro[16] = 0.0F; - b_r_gyro[25] = 0.0F; - b_r_gyro[34] = 0.0F; - b_r_gyro[43] = 0.0F; - b_r_gyro[52] = 0.0F; - b_r_gyro[61] = 0.0F; - b_r_gyro[70] = r_mag; - b_r_gyro[79] = 0.0F; - b_r_gyro[8] = 0.0F; - b_r_gyro[17] = 0.0F; - b_r_gyro[26] = 0.0F; - b_r_gyro[35] = 0.0F; - b_r_gyro[44] = 0.0F; - b_r_gyro[53] = 0.0F; - b_r_gyro[62] = 0.0F; - b_r_gyro[71] = 0.0F; - b_r_gyro[80] = r_mag; + b_r_gyro[1] = r_gyro; + b_r_gyro[2] = r_gyro; + b_r_gyro[3] = r_accel; + b_r_gyro[4] = r_accel; + b_r_gyro[5] = r_accel; + b_r_gyro[6] = r_mag; + b_r_gyro[7] = r_mag; + b_r_gyro[8] = r_mag; for (r2 = 0; r2 < 9; r2++) { + b_O[r2] = S_k[10 * r2] + b_r_gyro[r2]; + } + + for (r2 = 0; r2 < 9; r2++) { + S_k[10 * r2] = b_O[r2]; + } + + /* 'AttitudeEKF:191' K_k=(P_apr*H_k'/(S_k)); */ + for (r2 = 0; r2 < 12; r2++) { for (i = 0; i < 9; i++) { - c_a[i + 9 * r2] = a[i + 9 * r2] + b_r_gyro[i + 9 * r2]; + a[r2 + 12 * i] = 0.0F; + for (r1 = 0; r1 < 12; r1++) { + a[r2 + 12 * i] += P_apr[r2 + 12 * r1] * (float)b[r1 + 12 * i]; + } } } - mrdivide(b_P_apr, c_a, K_k); + mrdivide(a, S_k, K_k); - /* 'AttitudeEKF:191' x_apo=x_apr+K_k*y_k; */ + /* 'AttitudeEKF:194' x_apo=x_apr+K_k*y_k; */ for (r2 = 0; r2 < 9; r2++) { maxval = 0.0F; for (i = 0; i < 12; i++) { @@ -1048,7 +980,7 @@ void AttitudeEKF(unsigned char approx_prediction, unsigned char x_apo[r2] = x_apr[r2] + maxval; } - /* 'AttitudeEKF:192' P_apo=(eye(12)-K_k*H_k)*P_apr; */ + /* 'AttitudeEKF:195' P_apo=(eye(12)-K_k*H_k)*P_apr; */ memset(&I[0], 0, 144U * sizeof(signed char)); for (i = 0; i < 12; i++) { I[i + 12 * i] = 1; @@ -1074,53 +1006,56 @@ void AttitudeEKF(unsigned char approx_prediction, unsigned char } } } else { - /* 'AttitudeEKF:193' else */ - /* 'AttitudeEKF:194' if zFlag(1)==1&&zFlag(2)==0&&zFlag(3)==0 */ + /* 'AttitudeEKF:196' else */ + /* 'AttitudeEKF:197' if zFlag(1)==1&&zFlag(2)==0&&zFlag(3)==0 */ if ((zFlag[0] == 1) && (zFlag[1] == 0) && (zFlag[2] == 0)) { - /* 'AttitudeEKF:196' R=[r_gyro,0,0; */ - /* 'AttitudeEKF:197' 0,r_gyro,0; */ - /* 'AttitudeEKF:198' 0,0,r_gyro]; */ + /* 'AttitudeEKF:199' R=[r_gyro,0,0; */ + /* 'AttitudeEKF:200' 0,r_gyro,0; */ + /* 'AttitudeEKF:201' 0,0,r_gyro]; */ + /* 'AttitudeEKF:202' R_v=[r_gyro,r_gyro,r_gyro]; */ /* observation matrix */ - /* 'AttitudeEKF:201' H_k=[ E, Z, Z, Z]; */ - /* 'AttitudeEKF:203' y_k=z(1:3)-H_k(1:3,1:12)*x_apr; */ - /* 'AttitudeEKF:205' S_k=H_k(1:3,1:12)*P_apr*H_k(1:3,1:12)'+R(1:3,1:3); */ - /* 'AttitudeEKF:206' K_k=(P_apr*H_k(1:3,1:12)'/(S_k)); */ + /* 'AttitudeEKF:205' H_k=[ E, Z, Z, Z]; */ + /* 'AttitudeEKF:207' y_k=z(1:3)-H_k(1:3,1:12)*x_apr; */ + /* S_k=H_k(1:3,1:12)*P_apr*H_k(1:3,1:12)'+R(1:3,1:3); */ + /* 'AttitudeEKF:210' S_k=H_k(1:3,1:12)*P_apr*H_k(1:3,1:12)'; */ for (r2 = 0; r2 < 3; r2++) { for (i = 0; i < 12; i++) { - d_a[r2 + 3 * i] = 0.0F; + b_S_k[r2 + 3 * i] = 0.0F; for (r1 = 0; r1 < 12; r1++) { - d_a[r2 + 3 * i] += (float)e_a[r2 + 3 * r1] * P_apr[r1 + 12 * i]; + b_S_k[r2 + 3 * i] += (float)c_a[r2 + 3 * r1] * P_apr[r1 + 12 * i]; } } - } - for (r2 = 0; r2 < 3; r2++) { for (i = 0; i < 3; i++) { - b_O[r2 + 3 * i] = 0.0F; + O[r2 + 3 * i] = 0.0F; for (r1 = 0; r1 < 12; r1++) { - b_O[r2 + 3 * i] += d_a[i + 3 * r1] * (float)b_b[r1 + 12 * r2]; + O[r2 + 3 * i] += b_S_k[r2 + 3 * r1] * (float)b_b[r1 + 12 * i]; } } } + /* 'AttitudeEKF:211' S_k(1:3+1:end) = S_k(1:3+1:end) + R_v; */ c_r_gyro[0] = r_gyro; - c_r_gyro[1] = 0.0F; - c_r_gyro[2] = 0.0F; - c_r_gyro[3] = 0.0F; - c_r_gyro[4] = r_gyro; - c_r_gyro[5] = 0.0F; - c_r_gyro[6] = 0.0F; - c_r_gyro[7] = 0.0F; - c_r_gyro[8] = r_gyro; + c_r_gyro[1] = r_gyro; + c_r_gyro[2] = r_gyro; + for (r2 = 0; r2 < 3; r2++) { + b_muk[r2] = O[r2 << 2] + c_r_gyro[r2]; + } + + for (r2 = 0; r2 < 3; r2++) { + O[r2 << 2] = b_muk[r2]; + } + + /* 'AttitudeEKF:212' K_k=(P_apr*H_k(1:3,1:12)'/(S_k)); */ for (r2 = 0; r2 < 3; r2++) { for (i = 0; i < 3; i++) { - O[i + 3 * r2] = b_O[i + 3 * r2] + c_r_gyro[i + 3 * r2]; + b_O[i + 3 * r2] = O[r2 + 3 * i]; } for (i = 0; i < 12; i++) { - b_K_k[r2 + 3 * i] = 0.0F; + B[r2 + 3 * i] = 0.0F; for (r1 = 0; r1 < 12; r1++) { - b_K_k[r2 + 3 * i] += P_apr[i + 12 * r1] * (float)b_b[r1 + 12 * r2]; + B[r2 + 3 * i] += P_apr[i + 12 * r1] * (float)b_b[r1 + 12 * r2]; } } } @@ -1128,58 +1063,58 @@ void AttitudeEKF(unsigned char approx_prediction, unsigned char r1 = 0; r2 = 1; r3 = 2; - maxval = (real32_T)fabs(O[0]); - a21 = (real32_T)fabs(O[1]); + maxval = (real32_T)fabs(b_O[0]); + a21 = (real32_T)fabs(b_O[1]); if (a21 > maxval) { maxval = a21; r1 = 1; r2 = 0; } - if ((real32_T)fabs(O[2]) > maxval) { + if ((real32_T)fabs(b_O[2]) > maxval) { r1 = 2; r2 = 1; r3 = 0; } - O[r2] /= O[r1]; - O[r3] /= O[r1]; - O[3 + r2] -= O[r2] * O[3 + r1]; - O[3 + r3] -= O[r3] * O[3 + r1]; - O[6 + r2] -= O[r2] * O[6 + r1]; - O[6 + r3] -= O[r3] * O[6 + r1]; - if ((real32_T)fabs(O[3 + r3]) > (real32_T)fabs(O[3 + r2])) { + b_O[r2] /= b_O[r1]; + b_O[r3] /= b_O[r1]; + b_O[3 + r2] -= b_O[r2] * b_O[3 + r1]; + b_O[3 + r3] -= b_O[r3] * b_O[3 + r1]; + b_O[6 + r2] -= b_O[r2] * b_O[6 + r1]; + b_O[6 + r3] -= b_O[r3] * b_O[6 + r1]; + if ((real32_T)fabs(b_O[3 + r3]) > (real32_T)fabs(b_O[3 + r2])) { i = r2; r2 = r3; r3 = i; } - O[3 + r3] /= O[3 + r2]; - O[6 + r3] -= O[3 + r3] * O[6 + r2]; + b_O[3 + r3] /= b_O[3 + r2]; + b_O[6 + r3] -= b_O[3 + r3] * b_O[6 + r2]; for (i = 0; i < 12; i++) { - f_a[3 * i] = b_K_k[r1 + 3 * i]; - f_a[1 + 3 * i] = b_K_k[r2 + 3 * i] - f_a[3 * i] * O[r2]; - f_a[2 + 3 * i] = (b_K_k[r3 + 3 * i] - f_a[3 * i] * O[r3]) - f_a[1 + 3 * - i] * O[3 + r3]; - f_a[2 + 3 * i] /= O[6 + r3]; - f_a[3 * i] -= f_a[2 + 3 * i] * O[6 + r1]; - f_a[1 + 3 * i] -= f_a[2 + 3 * i] * O[6 + r2]; - f_a[1 + 3 * i] /= O[3 + r2]; - f_a[3 * i] -= f_a[1 + 3 * i] * O[3 + r1]; - f_a[3 * i] /= O[r1]; + Y[3 * i] = B[r1 + 3 * i]; + Y[1 + 3 * i] = B[r2 + 3 * i] - Y[3 * i] * b_O[r2]; + Y[2 + 3 * i] = (B[r3 + 3 * i] - Y[3 * i] * b_O[r3]) - Y[1 + 3 * i] * + b_O[3 + r3]; + Y[2 + 3 * i] /= b_O[6 + r3]; + Y[3 * i] -= Y[2 + 3 * i] * b_O[6 + r1]; + Y[1 + 3 * i] -= Y[2 + 3 * i] * b_O[6 + r2]; + Y[1 + 3 * i] /= b_O[3 + r2]; + Y[3 * i] -= Y[1 + 3 * i] * b_O[3 + r1]; + Y[3 * i] /= b_O[r1]; } for (r2 = 0; r2 < 3; r2++) { for (i = 0; i < 12; i++) { - b_K_k[i + 12 * r2] = f_a[r2 + 3 * i]; + b_S_k[i + 12 * r2] = Y[r2 + 3 * i]; } } - /* 'AttitudeEKF:209' x_apo=x_apr+K_k*y_k; */ + /* 'AttitudeEKF:215' x_apo=x_apr+K_k*y_k; */ for (r2 = 0; r2 < 3; r2++) { maxval = 0.0F; for (i = 0; i < 12; i++) { - maxval += (float)e_a[r2 + 3 * i] * x_apr[i]; + maxval += (float)c_a[r2 + 3 * i] * x_apr[i]; } b_muk[r2] = z[r2] - maxval; @@ -1188,13 +1123,13 @@ void AttitudeEKF(unsigned char approx_prediction, unsigned char for (r2 = 0; r2 < 12; r2++) { maxval = 0.0F; for (i = 0; i < 3; i++) { - maxval += b_K_k[r2 + 12 * i] * b_muk[i]; + maxval += b_S_k[r2 + 12 * i] * b_muk[i]; } x_apo[r2] = x_apr[r2] + maxval; } - /* 'AttitudeEKF:210' P_apo=(eye(12)-K_k*H_k(1:3,1:12))*P_apr; */ + /* 'AttitudeEKF:216' P_apo=(eye(12)-K_k*H_k(1:3,1:12))*P_apr; */ memset(&I[0], 0, 144U * sizeof(signed char)); for (i = 0; i < 12; i++) { I[i + 12 * i] = 1; @@ -1204,7 +1139,7 @@ void AttitudeEKF(unsigned char approx_prediction, unsigned char for (i = 0; i < 12; i++) { maxval = 0.0F; for (r1 = 0; r1 < 3; r1++) { - maxval += b_K_k[r2 + 12 * r1] * (float)e_a[r1 + 3 * i]; + maxval += b_S_k[r2 + 12 * r1] * (float)c_a[r1 + 3 * i]; } A_lin[r2 + 12 * i] = (float)I[r2 + 12 * i] - maxval; @@ -1220,111 +1155,85 @@ void AttitudeEKF(unsigned char approx_prediction, unsigned char } } } else { - /* 'AttitudeEKF:211' else */ - /* 'AttitudeEKF:212' if zFlag(1)==1&&zFlag(2)==1&&zFlag(3)==0 */ + /* 'AttitudeEKF:217' else */ + /* 'AttitudeEKF:218' if zFlag(1)==1&&zFlag(2)==1&&zFlag(3)==0 */ if ((zFlag[0] == 1) && (zFlag[1] == 1) && (zFlag[2] == 0)) { - /* 'AttitudeEKF:214' R=[r_gyro,0,0,0,0,0; */ - /* 'AttitudeEKF:215' 0,r_gyro,0,0,0,0; */ - /* 'AttitudeEKF:216' 0,0,r_gyro,0,0,0; */ - /* 'AttitudeEKF:217' 0,0,0,r_accel,0,0; */ - /* 'AttitudeEKF:218' 0,0,0,0,r_accel,0; */ - /* 'AttitudeEKF:219' 0,0,0,0,0,r_accel]; */ + /* R=[r_gyro,0,0,0,0,0; */ + /* 0,r_gyro,0,0,0,0; */ + /* 0,0,r_gyro,0,0,0; */ + /* 0,0,0,r_accel,0,0; */ + /* 0,0,0,0,r_accel,0; */ + /* 0,0,0,0,0,r_accel]; */ + /* 'AttitudeEKF:227' R_v=[r_gyro,r_gyro,r_gyro,r_accel,r_accel,r_accel]; */ /* observation matrix */ - /* 'AttitudeEKF:223' H_k=[ E, Z, Z, Z; */ - /* 'AttitudeEKF:224' Z, Z, E, Z]; */ - /* 'AttitudeEKF:226' y_k=z(1:6)-H_k(1:6,1:12)*x_apr; */ - /* 'AttitudeEKF:228' S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)'+R(1:6,1:6); */ - /* 'AttitudeEKF:229' K_k=(P_apr*H_k(1:6,1:12)'/(S_k)); */ - for (r2 = 0; r2 < 12; r2++) { - for (i = 0; i < 6; i++) { - c_P_apr[r2 + 12 * i] = 0.0F; - for (r1 = 0; r1 < 12; r1++) { - c_P_apr[r2 + 12 * i] += P_apr[r2 + 12 * r1] * (float)c_b[r1 + 12 * - i]; - } - } - } - + /* 'AttitudeEKF:230' H_k=[ E, Z, Z, Z; */ + /* 'AttitudeEKF:231' Z, Z, E, Z]; */ + /* 'AttitudeEKF:233' y_k=z(1:6)-H_k(1:6,1:12)*x_apr; */ + /* S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)'+R(1:6,1:6); */ + /* 'AttitudeEKF:236' S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)'; */ for (r2 = 0; r2 < 6; r2++) { for (i = 0; i < 12; i++) { - c_K_k[r2 + 6 * i] = 0.0F; - for (r1 = 0; r1 < 12; r1++) { - c_K_k[r2 + 6 * i] += (float)g_a[r2 + 6 * r1] * P_apr[r1 + 12 * i]; - } - } - - for (i = 0; i < 6; i++) { d_a[r2 + 6 * i] = 0.0F; for (r1 = 0; r1 < 12; r1++) { - d_a[r2 + 6 * i] += c_K_k[r2 + 6 * r1] * (float)c_b[r1 + 12 * i]; + d_a[r2 + 6 * i] += (float)e_a[r2 + 6 * r1] * P_apr[r1 + 12 * i]; + } + } + + for (i = 0; i < 6; i++) { + b_S_k[r2 + 6 * i] = 0.0F; + for (r1 = 0; r1 < 12; r1++) { + b_S_k[r2 + 6 * i] += d_a[r2 + 6 * r1] * (float)c_b[r1 + 12 * i]; } } } - b_K_k[0] = r_gyro; - b_K_k[6] = 0.0F; - b_K_k[12] = 0.0F; - b_K_k[18] = 0.0F; - b_K_k[24] = 0.0F; - b_K_k[30] = 0.0F; - b_K_k[1] = 0.0F; - b_K_k[7] = r_gyro; - b_K_k[13] = 0.0F; - b_K_k[19] = 0.0F; - b_K_k[25] = 0.0F; - b_K_k[31] = 0.0F; - b_K_k[2] = 0.0F; - b_K_k[8] = 0.0F; - b_K_k[14] = r_gyro; - b_K_k[20] = 0.0F; - b_K_k[26] = 0.0F; - b_K_k[32] = 0.0F; - b_K_k[3] = 0.0F; - b_K_k[9] = 0.0F; - b_K_k[15] = 0.0F; - b_K_k[21] = r_accel; - b_K_k[27] = 0.0F; - b_K_k[33] = 0.0F; - b_K_k[4] = 0.0F; - b_K_k[10] = 0.0F; - b_K_k[16] = 0.0F; - b_K_k[22] = 0.0F; - b_K_k[28] = r_accel; - b_K_k[34] = 0.0F; - b_K_k[5] = 0.0F; - b_K_k[11] = 0.0F; - b_K_k[17] = 0.0F; - b_K_k[23] = 0.0F; - b_K_k[29] = 0.0F; - b_K_k[35] = r_accel; + /* 'AttitudeEKF:237' S_k(1:6+1:end) = S_k(1:6+1:end) + R_v; */ + d_r_gyro[0] = r_gyro; + d_r_gyro[1] = r_gyro; + d_r_gyro[2] = r_gyro; + d_r_gyro[3] = r_accel; + d_r_gyro[4] = r_accel; + d_r_gyro[5] = r_accel; for (r2 = 0; r2 < 6; r2++) { + c_S_k[r2] = b_S_k[7 * r2] + d_r_gyro[r2]; + } + + for (r2 = 0; r2 < 6; r2++) { + b_S_k[7 * r2] = c_S_k[r2]; + } + + /* 'AttitudeEKF:238' K_k=(P_apr*H_k(1:6,1:12)'/(S_k)); */ + for (r2 = 0; r2 < 12; r2++) { for (i = 0; i < 6; i++) { - f_a[i + 6 * r2] = d_a[i + 6 * r2] + b_K_k[i + 6 * r2]; + d_a[r2 + 12 * i] = 0.0F; + for (r1 = 0; r1 < 12; r1++) { + d_a[r2 + 12 * i] += P_apr[r2 + 12 * r1] * (float)c_b[r1 + 12 * i]; + } } } - b_mrdivide(c_P_apr, f_a, c_K_k); + b_mrdivide(d_a, b_S_k, b_K_k); - /* 'AttitudeEKF:232' x_apo=x_apr+K_k*y_k; */ + /* 'AttitudeEKF:241' x_apo=x_apr+K_k*y_k; */ for (r2 = 0; r2 < 6; r2++) { maxval = 0.0F; for (i = 0; i < 12; i++) { - maxval += (float)g_a[r2 + 6 * i] * x_apr[i]; + maxval += (float)e_a[r2 + 6 * i] * x_apr[i]; } - b_z[r2] = z[r2] - maxval; + d_r_gyro[r2] = z[r2] - maxval; } for (r2 = 0; r2 < 12; r2++) { maxval = 0.0F; for (i = 0; i < 6; i++) { - maxval += c_K_k[r2 + 12 * i] * b_z[i]; + maxval += b_K_k[r2 + 12 * i] * d_r_gyro[i]; } x_apo[r2] = x_apr[r2] + maxval; } - /* 'AttitudeEKF:233' P_apo=(eye(12)-K_k*H_k(1:6,1:12))*P_apr; */ + /* 'AttitudeEKF:242' P_apo=(eye(12)-K_k*H_k(1:6,1:12))*P_apr; */ memset(&I[0], 0, 144U * sizeof(signed char)); for (i = 0; i < 12; i++) { I[i + 12 * i] = 1; @@ -1334,7 +1243,7 @@ void AttitudeEKF(unsigned char approx_prediction, unsigned char for (i = 0; i < 12; i++) { maxval = 0.0F; for (r1 = 0; r1 < 6; r1++) { - maxval += c_K_k[r2 + 12 * r1] * (float)g_a[r1 + 6 * i]; + maxval += b_K_k[r2 + 12 * r1] * (float)e_a[r1 + 6 * i]; } A_lin[r2 + 12 * i] = (float)I[r2 + 12 * i] - maxval; @@ -1350,119 +1259,93 @@ void AttitudeEKF(unsigned char approx_prediction, unsigned char } } } else { - /* 'AttitudeEKF:234' else */ - /* 'AttitudeEKF:235' if zFlag(1)==1&&zFlag(2)==0&&zFlag(3)==1 */ + /* 'AttitudeEKF:243' else */ + /* 'AttitudeEKF:244' if zFlag(1)==1&&zFlag(2)==0&&zFlag(3)==1 */ if ((zFlag[0] == 1) && (zFlag[1] == 0) && (zFlag[2] == 1)) { - /* 'AttitudeEKF:236' R=[r_gyro,0,0,0,0,0; */ - /* 'AttitudeEKF:237' 0,r_gyro,0,0,0,0; */ - /* 'AttitudeEKF:238' 0,0,r_gyro,0,0,0; */ - /* 'AttitudeEKF:239' 0,0,0,r_mag,0,0; */ - /* 'AttitudeEKF:240' 0,0,0,0,r_mag,0; */ - /* 'AttitudeEKF:241' 0,0,0,0,0,r_mag]; */ + /* R=[r_gyro,0,0,0,0,0; */ + /* 0,r_gyro,0,0,0,0; */ + /* 0,0,r_gyro,0,0,0; */ + /* 0,0,0,r_mag,0,0; */ + /* 0,0,0,0,r_mag,0; */ + /* 0,0,0,0,0,r_mag]; */ + /* 'AttitudeEKF:251' R_v=[r_gyro,r_gyro,r_gyro,r_mag,r_mag,r_mag]; */ /* observation matrix */ - /* 'AttitudeEKF:244' H_k=[ E, Z, Z, Z; */ - /* 'AttitudeEKF:245' Z, Z, Z, E]; */ - /* 'AttitudeEKF:247' y_k=[z(1:3);z(7:9)]-H_k(1:6,1:12)*x_apr; */ - /* 'AttitudeEKF:249' S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)'+R(1:6,1:6); */ - /* 'AttitudeEKF:250' K_k=(P_apr*H_k(1:6,1:12)'/(S_k)); */ - for (r2 = 0; r2 < 12; r2++) { - for (i = 0; i < 6; i++) { - c_P_apr[r2 + 12 * i] = 0.0F; - for (r1 = 0; r1 < 12; r1++) { - c_P_apr[r2 + 12 * i] += P_apr[r2 + 12 * r1] * (float)d_b[r1 + 12 - * i]; - } - } - } - + /* 'AttitudeEKF:254' H_k=[ E, Z, Z, Z; */ + /* 'AttitudeEKF:255' Z, Z, Z, E]; */ + /* 'AttitudeEKF:257' y_k=[z(1:3);z(7:9)]-H_k(1:6,1:12)*x_apr; */ + /* S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)'+R(1:6,1:6); */ + /* 'AttitudeEKF:260' S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)'; */ for (r2 = 0; r2 < 6; r2++) { for (i = 0; i < 12; i++) { - c_K_k[r2 + 6 * i] = 0.0F; - for (r1 = 0; r1 < 12; r1++) { - c_K_k[r2 + 6 * i] += (float)h_a[r2 + 6 * r1] * P_apr[r1 + 12 * i]; - } - } - - for (i = 0; i < 6; i++) { d_a[r2 + 6 * i] = 0.0F; for (r1 = 0; r1 < 12; r1++) { - d_a[r2 + 6 * i] += c_K_k[r2 + 6 * r1] * (float)d_b[r1 + 12 * i]; + d_a[r2 + 6 * i] += (float)f_a[r2 + 6 * r1] * P_apr[r1 + 12 * i]; + } + } + + for (i = 0; i < 6; i++) { + b_S_k[r2 + 6 * i] = 0.0F; + for (r1 = 0; r1 < 12; r1++) { + b_S_k[r2 + 6 * i] += d_a[r2 + 6 * r1] * (float)d_b[r1 + 12 * i]; } } } - b_K_k[0] = r_gyro; - b_K_k[6] = 0.0F; - b_K_k[12] = 0.0F; - b_K_k[18] = 0.0F; - b_K_k[24] = 0.0F; - b_K_k[30] = 0.0F; - b_K_k[1] = 0.0F; - b_K_k[7] = r_gyro; - b_K_k[13] = 0.0F; - b_K_k[19] = 0.0F; - b_K_k[25] = 0.0F; - b_K_k[31] = 0.0F; - b_K_k[2] = 0.0F; - b_K_k[8] = 0.0F; - b_K_k[14] = r_gyro; - b_K_k[20] = 0.0F; - b_K_k[26] = 0.0F; - b_K_k[32] = 0.0F; - b_K_k[3] = 0.0F; - b_K_k[9] = 0.0F; - b_K_k[15] = 0.0F; - b_K_k[21] = r_mag; - b_K_k[27] = 0.0F; - b_K_k[33] = 0.0F; - b_K_k[4] = 0.0F; - b_K_k[10] = 0.0F; - b_K_k[16] = 0.0F; - b_K_k[22] = 0.0F; - b_K_k[28] = r_mag; - b_K_k[34] = 0.0F; - b_K_k[5] = 0.0F; - b_K_k[11] = 0.0F; - b_K_k[17] = 0.0F; - b_K_k[23] = 0.0F; - b_K_k[29] = 0.0F; - b_K_k[35] = r_mag; + /* 'AttitudeEKF:261' S_k(1:6+1:end) = S_k(1:6+1:end) + R_v; */ + d_r_gyro[0] = r_gyro; + d_r_gyro[1] = r_gyro; + d_r_gyro[2] = r_gyro; + d_r_gyro[3] = r_mag; + d_r_gyro[4] = r_mag; + d_r_gyro[5] = r_mag; for (r2 = 0; r2 < 6; r2++) { + c_S_k[r2] = b_S_k[7 * r2] + d_r_gyro[r2]; + } + + for (r2 = 0; r2 < 6; r2++) { + b_S_k[7 * r2] = c_S_k[r2]; + } + + /* 'AttitudeEKF:262' K_k=(P_apr*H_k(1:6,1:12)'/(S_k)); */ + for (r2 = 0; r2 < 12; r2++) { for (i = 0; i < 6; i++) { - f_a[i + 6 * r2] = d_a[i + 6 * r2] + b_K_k[i + 6 * r2]; + d_a[r2 + 12 * i] = 0.0F; + for (r1 = 0; r1 < 12; r1++) { + d_a[r2 + 12 * i] += P_apr[r2 + 12 * r1] * (float)d_b[r1 + 12 * i]; + } } } - b_mrdivide(c_P_apr, f_a, c_K_k); + b_mrdivide(d_a, b_S_k, b_K_k); - /* 'AttitudeEKF:253' x_apo=x_apr+K_k*y_k; */ + /* 'AttitudeEKF:265' x_apo=x_apr+K_k*y_k; */ for (r2 = 0; r2 < 3; r2++) { - b_z[r2] = z[r2]; + d_r_gyro[r2] = z[r2]; } for (r2 = 0; r2 < 3; r2++) { - b_z[r2 + 3] = z[6 + r2]; + d_r_gyro[r2 + 3] = z[6 + r2]; } for (r2 = 0; r2 < 6; r2++) { - i_a[r2] = 0.0F; + c_S_k[r2] = 0.0F; for (i = 0; i < 12; i++) { - i_a[r2] += (float)h_a[r2 + 6 * i] * x_apr[i]; + c_S_k[r2] += (float)f_a[r2 + 6 * i] * x_apr[i]; } - c_z[r2] = b_z[r2] - i_a[r2]; + b_z[r2] = d_r_gyro[r2] - c_S_k[r2]; } for (r2 = 0; r2 < 12; r2++) { maxval = 0.0F; for (i = 0; i < 6; i++) { - maxval += c_K_k[r2 + 12 * i] * c_z[i]; + maxval += b_K_k[r2 + 12 * i] * b_z[i]; } x_apo[r2] = x_apr[r2] + maxval; } - /* 'AttitudeEKF:254' P_apo=(eye(12)-K_k*H_k(1:6,1:12))*P_apr; */ + /* 'AttitudeEKF:266' P_apo=(eye(12)-K_k*H_k(1:6,1:12))*P_apr; */ memset(&I[0], 0, 144U * sizeof(signed char)); for (i = 0; i < 12; i++) { I[i + 12 * i] = 1; @@ -1472,7 +1355,7 @@ void AttitudeEKF(unsigned char approx_prediction, unsigned char for (i = 0; i < 12; i++) { maxval = 0.0F; for (r1 = 0; r1 < 6; r1++) { - maxval += c_K_k[r2 + 12 * r1] * (float)h_a[r1 + 6 * i]; + maxval += b_K_k[r2 + 12 * r1] * (float)f_a[r1 + 6 * i]; } A_lin[r2 + 12 * i] = (float)I[r2 + 12 * i] - maxval; @@ -1488,13 +1371,13 @@ void AttitudeEKF(unsigned char approx_prediction, unsigned char } } } else { - /* 'AttitudeEKF:255' else */ - /* 'AttitudeEKF:256' x_apo=x_apr; */ + /* 'AttitudeEKF:267' else */ + /* 'AttitudeEKF:268' x_apo=x_apr; */ for (i = 0; i < 12; i++) { x_apo[i] = x_apr[i]; } - /* 'AttitudeEKF:257' P_apo=P_apr; */ + /* 'AttitudeEKF:269' P_apo=P_apr; */ memcpy(&P_apo[0], &P_apr[0], 144U * sizeof(float)); } } @@ -1502,57 +1385,57 @@ void AttitudeEKF(unsigned char approx_prediction, unsigned char } /* % euler anglels extraction */ - /* 'AttitudeEKF:266' z_n_b = -x_apo(7:9)./norm(x_apo(7:9)); */ + /* 'AttitudeEKF:278' z_n_b = -x_apo(7:9)./norm(x_apo(7:9)); */ maxval = norm(*(float (*)[3])&x_apo[6]); a21 = norm(*(float (*)[3])&x_apo[9]); for (i = 0; i < 3; i++) { - /* 'AttitudeEKF:267' m_n_b = x_apo(10:12)./norm(x_apo(10:12)); */ + /* 'AttitudeEKF:279' m_n_b = x_apo(10:12)./norm(x_apo(10:12)); */ muk[i] = -x_apo[i + 6] / maxval; zek[i] = x_apo[i + 9] / a21; } - /* 'AttitudeEKF:269' y_n_b=cross(z_n_b,m_n_b); */ + /* 'AttitudeEKF:281' y_n_b=cross(z_n_b,m_n_b); */ wak[0] = muk[1] * zek[2] - muk[2] * zek[1]; wak[1] = muk[2] * zek[0] - muk[0] * zek[2]; wak[2] = muk[0] * zek[1] - muk[1] * zek[0]; - /* 'AttitudeEKF:270' y_n_b=y_n_b./norm(y_n_b); */ + /* 'AttitudeEKF:282' y_n_b=y_n_b./norm(y_n_b); */ maxval = norm(wak); for (r2 = 0; r2 < 3; r2++) { wak[r2] /= maxval; } - /* 'AttitudeEKF:272' x_n_b=(cross(y_n_b,z_n_b)); */ + /* 'AttitudeEKF:284' x_n_b=(cross(y_n_b,z_n_b)); */ zek[0] = wak[1] * muk[2] - wak[2] * muk[1]; zek[1] = wak[2] * muk[0] - wak[0] * muk[2]; zek[2] = wak[0] * muk[1] - wak[1] * muk[0]; - /* 'AttitudeEKF:273' x_n_b=x_n_b./norm(x_n_b); */ + /* 'AttitudeEKF:285' x_n_b=x_n_b./norm(x_n_b); */ maxval = norm(zek); for (r2 = 0; r2 < 3; r2++) { zek[r2] /= maxval; } - /* 'AttitudeEKF:276' xa_apo=x_apo; */ + /* 'AttitudeEKF:288' xa_apo=x_apo; */ for (i = 0; i < 12; i++) { xa_apo[i] = x_apo[i]; } - /* 'AttitudeEKF:277' Pa_apo=P_apo; */ + /* 'AttitudeEKF:289' Pa_apo=P_apo; */ memcpy(&Pa_apo[0], &P_apo[0], 144U * sizeof(float)); /* rotation matrix from earth to body system */ - /* 'AttitudeEKF:279' Rot_matrix=[x_n_b,y_n_b,z_n_b]; */ + /* 'AttitudeEKF:291' Rot_matrix=[x_n_b,y_n_b,z_n_b]; */ for (r2 = 0; r2 < 3; r2++) { Rot_matrix[r2] = zek[r2]; Rot_matrix[3 + r2] = wak[r2]; Rot_matrix[6 + r2] = muk[r2]; } - /* 'AttitudeEKF:282' phi=atan2(Rot_matrix(2,3),Rot_matrix(3,3)); */ - /* 'AttitudeEKF:283' theta=-asin(Rot_matrix(1,3)); */ - /* 'AttitudeEKF:284' psi=atan2(Rot_matrix(1,2),Rot_matrix(1,1)); */ - /* 'AttitudeEKF:285' eulerAngles=[phi;theta;psi]; */ + /* 'AttitudeEKF:294' phi=atan2(Rot_matrix(2,3),Rot_matrix(3,3)); */ + /* 'AttitudeEKF:295' theta=-asin(Rot_matrix(1,3)); */ + /* 'AttitudeEKF:296' psi=atan2(Rot_matrix(1,2),Rot_matrix(1,1)); */ + /* 'AttitudeEKF:297' eulerAngles=[phi;theta;psi]; */ eulerAngles[0] = (real32_T)atan2(Rot_matrix[7], Rot_matrix[8]); eulerAngles[1] = -(real32_T)asin(Rot_matrix[6]); eulerAngles[2] = (real32_T)atan2(Rot_matrix[3], Rot_matrix[0]); diff --git a/src/modules/attitude_estimator_ekf/codegen/AttitudeEKF.h b/src/modules/attitude_estimator_ekf/codegen/AttitudeEKF.h index fc02752109..7094da1da5 100644 --- a/src/modules/attitude_estimator_ekf/codegen/AttitudeEKF.h +++ b/src/modules/attitude_estimator_ekf/codegen/AttitudeEKF.h @@ -3,7 +3,7 @@ * * Code generation for function 'AttitudeEKF' * - * C source code generated on: Fri Jul 25 14:06:41 2014 + * C source code generated on: Thu Aug 21 11:17:28 2014 * */ diff --git a/src/modules/attitude_estimator_ekf/codegen/AttitudeEKF_types.h b/src/modules/attitude_estimator_ekf/codegen/AttitudeEKF_types.h index 63eb7e5018..3f7522ffa0 100644 --- a/src/modules/attitude_estimator_ekf/codegen/AttitudeEKF_types.h +++ b/src/modules/attitude_estimator_ekf/codegen/AttitudeEKF_types.h @@ -3,7 +3,7 @@ * * Code generation for function 'AttitudeEKF' * - * C source code generated on: Fri Jul 25 14:06:41 2014 + * C source code generated on: Thu Aug 21 11:17:28 2014 * */ diff --git a/src/modules/attitude_estimator_ekf/codegen/rtwtypes.h b/src/modules/attitude_estimator_ekf/codegen/rtwtypes.h index 9245a24c8f..b5a02a7a6d 100644 --- a/src/modules/attitude_estimator_ekf/codegen/rtwtypes.h +++ b/src/modules/attitude_estimator_ekf/codegen/rtwtypes.h @@ -3,7 +3,7 @@ * * Code generation for function 'AttitudeEKF' * - * C source code generated on: Fri Jul 25 14:06:41 2014 + * C source code generated on: Thu Aug 21 11:17:28 2014 * */ From 2c023c5d015fc63f7499a3b60751c771ec8a72b1 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 16 Oct 2014 22:52:08 +0200 Subject: [PATCH 006/216] mavlink: use altitude AMSL in VFR message --- src/modules/mavlink/mavlink_messages.cpp | 11 ++--------- 1 file changed, 2 insertions(+), 9 deletions(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index c182cfdb95..a730b6c98f 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -778,9 +778,6 @@ private: MavlinkOrbSubscription *_airspeed_sub; uint64_t _airspeed_time; - MavlinkOrbSubscription *_sensor_combined_sub; - uint64_t _sensor_combined_time; - /* do not allow top copying this class */ MavlinkStreamVFRHUD(MavlinkStreamVFRHUD &); MavlinkStreamVFRHUD& operator = (const MavlinkStreamVFRHUD &); @@ -796,9 +793,7 @@ protected: _act_sub(_mavlink->add_orb_subscription(ORB_ID(actuator_controls_0))), _act_time(0), _airspeed_sub(_mavlink->add_orb_subscription(ORB_ID(airspeed))), - _airspeed_time(0), - _sensor_combined_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_combined))), - _sensor_combined_time(0) + _airspeed_time(0) {} void send(const hrt_abstime t) @@ -808,14 +803,12 @@ protected: struct actuator_armed_s armed; struct actuator_controls_s act; struct airspeed_s airspeed; - struct sensor_combined_s sensor_combined; bool updated = _att_sub->update(&_att_time, &att); updated |= _pos_sub->update(&_pos_time, &pos); updated |= _armed_sub->update(&_armed_time, &armed); updated |= _act_sub->update(&_act_time, &act); updated |= _airspeed_sub->update(&_airspeed_time, &airspeed); - updated |= _sensor_combined_sub->update(&_sensor_combined_time, &sensor_combined); if (updated) { mavlink_vfr_hud_t msg; @@ -824,7 +817,7 @@ protected: msg.groundspeed = sqrtf(pos.vel_n * pos.vel_n + pos.vel_e * pos.vel_e); msg.heading = _wrap_2pi(att.yaw) * M_RAD_TO_DEG_F; msg.throttle = armed.armed ? act.control[3] * 100.0f : 0.0f; - msg.alt = sensor_combined.baro_alt_meter; + msg.alt = pos.alt; msg.climb = -pos.vel_d; _mavlink->send_message(MAVLINK_MSG_ID_VFR_HUD, &msg); From 5c77fc0012eaaad5b92e1b31cedf8bf5b035b988 Mon Sep 17 00:00:00 2001 From: philipoe Date: Mon, 20 Oct 2014 15:55:45 +0200 Subject: [PATCH 007/216] commander: Added time of RC-loss/gain to the mavlink_log_critical message to better inform the user --- src/modules/commander/commander.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 616b855df4..23fdcb414c 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1499,7 +1499,7 @@ int commander_thread_main(int argc, char *argv[]) } else { if (status.rc_signal_lost) { - mavlink_log_critical(mavlink_fd, "RC signal regained"); + mavlink_log_critical(mavlink_fd, "RC SIGNAL REGAINED (at t=%.3fs)",(double)(hrt_absolute_time())/1.0e6); status_changed = true; } } @@ -1592,7 +1592,7 @@ int commander_thread_main(int argc, char *argv[]) } else { if (!status.rc_signal_lost) { - mavlink_log_critical(mavlink_fd, "RC SIGNAL LOST"); + mavlink_log_critical(mavlink_fd, "RC SIGNAL LOST (at t=%.3fs)",(double)(hrt_absolute_time())/1.0e6); status.rc_signal_lost = true; status_changed = true; } From 4afa8645832c71ea1c35dc73d9424df80fac531a Mon Sep 17 00:00:00 2001 From: philipoe Date: Mon, 20 Oct 2014 16:25:38 +0200 Subject: [PATCH 008/216] commander: Added duration of rc-loss to mavlink_log_critical message --- src/modules/commander/commander.cpp | 3 ++- src/modules/uORB/topics/vehicle_status.h | 1 + 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 23fdcb414c..d8ebee4b6f 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1499,7 +1499,7 @@ int commander_thread_main(int argc, char *argv[]) } else { if (status.rc_signal_lost) { - mavlink_log_critical(mavlink_fd, "RC SIGNAL REGAINED (at t=%.3fs)",(double)(hrt_absolute_time())/1.0e6); + mavlink_log_critical(mavlink_fd, "RC SIGNAL REGAINED after %.3fs (at t=%.3fs)",(double)(hrt_absolute_time()-status.rc_signal_lost_timestamp)/1.0e6,(double)(hrt_absolute_time())/1.0e6); status_changed = true; } } @@ -1594,6 +1594,7 @@ int commander_thread_main(int argc, char *argv[]) if (!status.rc_signal_lost) { mavlink_log_critical(mavlink_fd, "RC SIGNAL LOST (at t=%.3fs)",(double)(hrt_absolute_time())/1.0e6); status.rc_signal_lost = true; + status.rc_signal_lost_timestamp=sp_man.timestamp; status_changed = true; } } diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index a1b2667e39..8d797e21ab 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -201,6 +201,7 @@ struct vehicle_status_s { bool rc_signal_found_once; bool rc_signal_lost; /**< true if RC reception lost */ + uint64_t rc_signal_lost_timestamp; /**< Time at which the RC reception was lost */ bool rc_signal_lost_cmd; /**< true if RC lost mode is commanded */ bool rc_input_blocked; /**< set if RC input should be ignored */ From 4656db01c215c94dda70d7b579037ec368d3275e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 22 Oct 2014 17:59:53 +0200 Subject: [PATCH 009/216] Enable IO safety parameter --- src/drivers/px4io/px4io.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index d212be7661..821eb52ffa 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -817,6 +817,11 @@ PX4IO::init() } + /* set safety to off if circuit breaker enabled */ + if (circuit_breaker_enabled("CBRK_IO_SAFETY", CBRK_IO_SAFETY_KEY)) { + (void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FORCE_SAFETY_OFF, PX4IO_FORCE_SAFETY_MAGIC); + } + /* try to claim the generic PWM output device node as well - it's OK if we fail at this */ ret = register_driver(PWM_OUTPUT_DEVICE_PATH, &fops, 0666, (void *)this); From 276dc7fbbcc9d2b8baf65376b7ceb5e362ff978b Mon Sep 17 00:00:00 2001 From: dominiho Date: Tue, 28 Oct 2014 12:35:20 +0100 Subject: [PATCH 010/216] added px4flow integral frame, adjusted px4flow i2c driver, adjusted postition_estimator_inav --- src/drivers/px4flow/px4flow.cpp | 611 +++++++++--------- .../position_estimator_inav_main.c | 11 +- src/modules/uORB/topics/optical_flow.h | 23 +- 3 files changed, 319 insertions(+), 326 deletions(-) diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp index 04aba9eae4..f74db1b527 100644 --- a/src/drivers/px4flow/px4flow.cpp +++ b/src/drivers/px4flow/px4flow.cpp @@ -73,15 +73,15 @@ #include /* Configuration Constants */ -#define PX4FLOW_BUS PX4_I2C_BUS_EXPANSION +#define PX4FLOW_BUS PX4_I2C_BUS_ESC//PX4_I2C_BUS_EXPANSION #define I2C_FLOW_ADDRESS 0x42 //* 7-bit address. 8-bit address is 0x84 //range 0x42 - 0x49 /* PX4FLOW Registers addresses */ -#define PX4FLOW_REG 0x00 /* Measure Register */ - -#define PX4FLOW_CONVERSION_INTERVAL 8000 /* 8ms 125Hz */ +//#define PX4FLOW_REG 0x00 /* Measure Register 0*/ +#define PX4FLOW_REG 0x16 /* Measure Register 22*/ +#define PX4FLOW_CONVERSION_INTERVAL 20000 //in microseconds! 20000 = 50 Hz 100000 = 10Hz /* oddly, ERROR is not defined for c++ */ #ifdef ERROR # undef ERROR @@ -92,94 +92,103 @@ static const int ERROR = -1; # error This requires CONFIG_SCHED_WORKQUEUE. #endif -//struct i2c_frame -//{ -// uint16_t frame_count; -// int16_t pixel_flow_x_sum; -// int16_t pixel_flow_y_sum; -// int16_t flow_comp_m_x; -// int16_t flow_comp_m_y; -// int16_t qual; -// int16_t gyro_x_rate; -// int16_t gyro_y_rate; -// int16_t gyro_z_rate; -// uint8_t gyro_range; -// uint8_t sonar_timestamp; -// int16_t ground_distance; -//}; -// -//struct i2c_frame f; +struct i2c_frame { + uint16_t frame_count; + int16_t pixel_flow_x_sum; + int16_t pixel_flow_y_sum; + int16_t flow_comp_m_x; + int16_t flow_comp_m_y; + int16_t qual; + int16_t gyro_x_rate; + int16_t gyro_y_rate; + int16_t gyro_z_rate; + uint8_t gyro_range; + uint8_t sonar_timestamp; + int16_t ground_distance; +}; +struct i2c_frame f; -class PX4FLOW : public device::I2C -{ +typedef struct i2c_integral_frame { + uint16_t frame_count_since_last_readout; + int16_t pixel_flow_x_integral; + int16_t pixel_flow_y_integral; + int16_t gyro_x_rate_integral; + int16_t gyro_y_rate_integral; + int16_t gyro_z_rate_integral; + uint32_t integration_timespan; + uint32_t time_since_last_sonar_update; + uint16_t ground_distance; + uint8_t qual; +}__attribute__((packed)); +struct i2c_integral_frame f_integral; + + +class PX4FLOW: public device::I2C { public: PX4FLOW(int bus = PX4FLOW_BUS, int address = I2C_FLOW_ADDRESS); virtual ~PX4FLOW(); - virtual int init(); + virtual int init(); - virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); - virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); /** - * Diagnostics - print some basic information about the driver. - */ - void print_info(); + * Diagnostics - print some basic information about the driver. + */ + void print_info(); protected: - virtual int probe(); + virtual int probe(); private: - work_s _work; - RingBuffer *_reports; - bool _sensor_ok; - int _measure_ticks; - bool _collect_phase; + work_s _work; + RingBuffer *_reports;bool _sensor_ok; + int _measure_ticks;bool _collect_phase; - orb_advert_t _px4flow_topic; + orb_advert_t _px4flow_topic; - perf_counter_t _sample_perf; - perf_counter_t _comms_errors; - perf_counter_t _buffer_overflows; + perf_counter_t _sample_perf; + perf_counter_t _comms_errors; + perf_counter_t _buffer_overflows; /** - * Test whether the device supported by the driver is present at a - * specific address. - * - * @param address The I2C bus address to probe. - * @return True if the device is present. - */ - int probe_address(uint8_t address); + * Test whether the device supported by the driver is present at a + * specific address. + * + * @param address The I2C bus address to probe. + * @return True if the device is present. + */ + int probe_address(uint8_t address); /** - * Initialise the automatic measurement state machine and start it. - * - * @note This function is called at open and error time. It might make sense - * to make it more aggressive about resetting the bus in case of errors. - */ - void start(); + * Initialise the automatic measurement state machine and start it. + * + * @note This function is called at open and error time. It might make sense + * to make it more aggressive about resetting the bus in case of errors. + */ + void start(); /** - * Stop the automatic measurement state machine. - */ - void stop(); + * Stop the automatic measurement state machine. + */ + void stop(); /** - * Perform a poll cycle; collect from the previous measurement - * and start a new one. - */ - void cycle(); - int measure(); - int collect(); + * Perform a poll cycle; collect from the previous measurement + * and start a new one. + */ + void cycle(); + int measure(); + int collect(); /** - * Static trampoline from the workq context; because we don't have a - * generic workq wrapper yet. - * - * @param arg Instance pointer for the driver that is polling. - */ - static void cycle_trampoline(void *arg); - + * Static trampoline from the workq context; because we don't have a + * generic workq wrapper yet. + * + * @param arg Instance pointer for the driver that is polling. + */ + static void cycle_trampoline(void *arg); }; @@ -189,16 +198,12 @@ private: extern "C" __EXPORT int px4flow_main(int argc, char *argv[]); PX4FLOW::PX4FLOW(int bus, int address) : - I2C("PX4FLOW", PX4FLOW_DEVICE_PATH, bus, address, 400000),//400khz - _reports(nullptr), - _sensor_ok(false), - _measure_ticks(0), - _collect_phase(false), - _px4flow_topic(-1), - _sample_perf(perf_alloc(PC_ELAPSED, "px4flow_read")), - _comms_errors(perf_alloc(PC_COUNT, "px4flow_comms_errors")), - _buffer_overflows(perf_alloc(PC_COUNT, "px4flow_buffer_overflows")) -{ + I2C("PX4FLOW", PX4FLOW_DEVICE_PATH, bus, address, 400000), //400khz + _reports(nullptr), _sensor_ok(false), _measure_ticks(0), _collect_phase( + false), _px4flow_topic(-1), _sample_perf( + perf_alloc(PC_ELAPSED, "px4flow_read")), _comms_errors( + perf_alloc(PC_COUNT, "px4flow_comms_errors")), _buffer_overflows( + perf_alloc(PC_COUNT, "px4flow_buffer_overflows")) { // enable debug() calls _debug_enabled = true; @@ -206,8 +211,7 @@ PX4FLOW::PX4FLOW(int bus, int address) : memset(&_work, 0, sizeof(_work)); } -PX4FLOW::~PX4FLOW() -{ +PX4FLOW::~PX4FLOW() { /* make sure we are truly inactive */ stop(); @@ -216,9 +220,7 @@ PX4FLOW::~PX4FLOW() delete _reports; } -int -PX4FLOW::init() -{ +int PX4FLOW::init() { int ret = ERROR; /* do I2C init (and probe) first */ @@ -226,103 +228,79 @@ PX4FLOW::init() goto out; /* allocate basic report buffers */ - _reports = new RingBuffer(2, sizeof(struct optical_flow_s)); + _reports = new RingBuffer(2, sizeof(optical_flow_s)); if (_reports == nullptr) goto out; - /* get a publish handle on the px4flow topic */ - struct optical_flow_s zero_report; - memset(&zero_report, 0, sizeof(zero_report)); - _px4flow_topic = orb_advertise(ORB_ID(optical_flow), &zero_report); - - if (_px4flow_topic < 0) - debug("failed to create px4flow object. Did you start uOrb?"); - ret = OK; /* sensor is ok, but we don't really know if it is within range */ _sensor_ok = true; -out: - return ret; + out: return ret; } -int -PX4FLOW::probe() -{ - uint8_t val[22]; - - // to be sure this is not a ll40ls Lidar (which can also be on - // 0x42) we check if a 22 byte transfer works from address - // 0. The ll40ls gives an error for that, whereas the flow - // happily returns some data - if (transfer(nullptr, 0, &val[0], 22) != OK) { - return -EIO; - } - - // that worked, so start a measurement cycle +int PX4FLOW::probe() { return measure(); } -int -PX4FLOW::ioctl(struct file *filp, int cmd, unsigned long arg) -{ +int PX4FLOW::ioctl(struct file *filp, int cmd, unsigned long arg) { switch (cmd) { case SENSORIOCSPOLLRATE: { - switch (arg) { + switch (arg) { - /* switching to manual polling */ - case SENSOR_POLLRATE_MANUAL: - stop(); - _measure_ticks = 0; - return OK; + /* switching to manual polling */ + case SENSOR_POLLRATE_MANUAL: + stop(); + _measure_ticks = 0; + return OK; - /* external signalling (DRDY) not supported */ - case SENSOR_POLLRATE_EXTERNAL: + /* external signalling (DRDY) not supported */ + case SENSOR_POLLRATE_EXTERNAL: - /* zero would be bad */ - case 0: + /* zero would be bad */ + case 0: + return -EINVAL; + + /* set default/max polling rate */ + case SENSOR_POLLRATE_MAX: + case SENSOR_POLLRATE_DEFAULT: { + /* do we need to start internal polling? */ + bool want_start = (_measure_ticks == 0); + + /* set interval for next measurement to minimum legal value */ + _measure_ticks = USEC2TICK(PX4FLOW_CONVERSION_INTERVAL); + + /* if we need to start the poll state machine, do it */ + if (want_start) + start(); + + return OK; + } + + /* adjust to a legal polling interval in Hz */ + default: { + /* do we need to start internal polling? */ + bool want_start = (_measure_ticks == 0); + + /* convert hz to tick interval via microseconds */ + unsigned ticks = USEC2TICK(1000000 / arg); + + /* check against maximum rate */ + if (ticks < USEC2TICK(PX4FLOW_CONVERSION_INTERVAL)) return -EINVAL; - /* set default/max polling rate */ - case SENSOR_POLLRATE_MAX: - case SENSOR_POLLRATE_DEFAULT: { - /* do we need to start internal polling? */ - bool want_start = (_measure_ticks == 0); + /* update interval for next measurement */ + _measure_ticks = ticks; - /* set interval for next measurement to minimum legal value */ - _measure_ticks = USEC2TICK(PX4FLOW_CONVERSION_INTERVAL); + /* if we need to start the poll state machine, do it */ + if (want_start) + start(); - /* if we need to start the poll state machine, do it */ - if (want_start) - start(); - - return OK; - } - - /* adjust to a legal polling interval in Hz */ - default: { - /* do we need to start internal polling? */ - bool want_start = (_measure_ticks == 0); - - /* convert hz to tick interval via microseconds */ - unsigned ticks = USEC2TICK(1000000 / arg); - - /* check against maximum rate */ - if (ticks < USEC2TICK(PX4FLOW_CONVERSION_INTERVAL)) - return -EINVAL; - - /* update interval for next measurement */ - _measure_ticks = ticks; - - /* if we need to start the poll state machine, do it */ - if (want_start) - start(); - - return OK; - } - } + return OK; } + } + } case SENSORIOCGPOLLRATE: if (_measure_ticks == 0) @@ -358,11 +336,10 @@ PX4FLOW::ioctl(struct file *filp, int cmd, unsigned long arg) } } -ssize_t -PX4FLOW::read(struct file *filp, char *buffer, size_t buflen) -{ +ssize_t PX4FLOW::read(struct file *filp, char *buffer, size_t buflen) { unsigned count = buflen / sizeof(struct optical_flow_s); - struct optical_flow_s *rbuf = reinterpret_cast(buffer); + struct optical_flow_s *rbuf = + reinterpret_cast(buffer); int ret = 0; /* buffer must be large enough */ @@ -398,8 +375,8 @@ PX4FLOW::read(struct file *filp, char *buffer, size_t buflen) break; } - /* wait for it to complete */ - usleep(PX4FLOW_CONVERSION_INTERVAL); +// /* wait for it to complete */ +// usleep(PX4FLOW_CONVERSION_INTERVAL); /* run the collection phase */ if (OK != collect()) { @@ -407,6 +384,9 @@ PX4FLOW::read(struct file *filp, char *buffer, size_t buflen) break; } + /* wait for it to complete */ + usleep(PX4FLOW_CONVERSION_INTERVAL); + /* state machine will have generated a report, copy it out */ if (_reports->get(rbuf)) { ret = sizeof(*rbuf); @@ -417,9 +397,7 @@ PX4FLOW::read(struct file *filp, char *buffer, size_t buflen) return ret; } -int -PX4FLOW::measure() -{ +int PX4FLOW::measure() { int ret; /* @@ -428,11 +406,10 @@ PX4FLOW::measure() uint8_t cmd = PX4FLOW_REG; ret = transfer(&cmd, 1, nullptr, 0); - if (OK != ret) - { + if (OK != ret) { perf_count(_comms_errors); log("i2c::transfer returned %d", ret); - printf("i2c::transfer flow returned %d"); + printf("i2c::transfer flow returned %d"); return ret; } ret = OK; @@ -440,56 +417,90 @@ PX4FLOW::measure() return ret; } -int -PX4FLOW::collect() -{ - int ret = -EIO; +int PX4FLOW::collect() { + int ret = -EIO; /* read from the sensor */ - uint8_t val[22] = {0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0}; + uint8_t val[46] = { 0 }; + perf_begin(_sample_perf); - ret = transfer(nullptr, 0, &val[0], 22); + if(PX4FLOW_REG==0x00){ + ret = transfer(nullptr, 0, &val[0], 45); //read 45 bytes (22+23 : frame1 + frame2) + } + if(PX4FLOW_REG==0x16){ + ret = transfer(nullptr, 0, &val[0], 23); //read 23 bytes (only frame2) + } - if (ret < 0) - { + if (ret < 0) { log("error reading from sensor: %d", ret); perf_count(_comms_errors); perf_end(_sample_perf); return ret; } -// f.frame_count = val[1] << 8 | val[0]; -// f.pixel_flow_x_sum= val[3] << 8 | val[2]; -// f.pixel_flow_y_sum= val[5] << 8 | val[4]; -// f.flow_comp_m_x= val[7] << 8 | val[6]; -// f.flow_comp_m_y= val[9] << 8 | val[8]; -// f.qual= val[11] << 8 | val[10]; -// f.gyro_x_rate= val[13] << 8 | val[12]; -// f.gyro_y_rate= val[15] << 8 | val[14]; -// f.gyro_z_rate= val[17] << 8 | val[16]; -// f.gyro_range= val[18]; -// f.sonar_timestamp= val[19]; -// f.ground_distance= val[21] << 8 | val[20]; + if (PX4FLOW_REG == 0) { + f.frame_count = val[1] << 8 | val[0]; + f.pixel_flow_x_sum = val[3] << 8 | val[2]; + f.pixel_flow_y_sum = val[5] << 8 | val[4]; + f.flow_comp_m_x = val[7] << 8 | val[6]; + f.flow_comp_m_y = val[9] << 8 | val[8]; + f.qual = val[11] << 8 | val[10]; + f.gyro_x_rate = val[13] << 8 | val[12]; + f.gyro_y_rate = val[15] << 8 | val[14]; + f.gyro_z_rate = val[17] << 8 | val[16]; + f.gyro_range = val[18]; + f.sonar_timestamp = val[19]; + f.ground_distance = val[21] << 8 | val[20]; + + f_integral.frame_count_since_last_readout = val[23] << 8 | val[22]; + f_integral.pixel_flow_x_integral = val[25] << 8 | val[24]; + f_integral.pixel_flow_y_integral = val[27] << 8 | val[26]; + f_integral.gyro_x_rate_integral = val[29] << 8 | val[28]; + f_integral.gyro_y_rate_integral = val[31] << 8 | val[30]; + f_integral.gyro_z_rate_integral = val[33] << 8 | val[32]; + f_integral.integration_timespan = val[37] << 24 | val[36] << 16 + | val[35] << 8 | val[34]; + f_integral.time_since_last_sonar_update = val[41] << 24 | val[40] << 16 + | val[39] << 8 | val[38]; + f_integral.ground_distance = val[43] << 8 | val[42]; + f_integral.qual = val[44]; + } + if(PX4FLOW_REG==0x16){ + f_integral.frame_count_since_last_readout = val[1] << 8 | val[0]; + f_integral.pixel_flow_x_integral =val[3] << 8 | val[2]; + f_integral.pixel_flow_y_integral =val[5] << 8 | val[4]; + f_integral.gyro_x_rate_integral =val[7] << 8 | val[6]; + f_integral.gyro_y_rate_integral =val[9] << 8 | val[8]; + f_integral.gyro_z_rate_integral =val[11] << 8 | val[10]; + f_integral.integration_timespan = val[15] <<24 |val[14] << 16 |val[13] << 8 |val[12]; + f_integral.time_since_last_sonar_update = val[19] <<24 |val[18] << 16 |val[17] << 8 |val[16]; + f_integral.ground_distance =val[21] <<8 |val[20]; + f_integral.qual =val[22]; + } - int16_t flowcx = val[7] << 8 | val[6]; - int16_t flowcy = val[9] << 8 | val[8]; - int16_t gdist = val[21] << 8 | val[20]; struct optical_flow_s report; - report.flow_comp_x_m = float(flowcx)/1000.0f; - report.flow_comp_y_m = float(flowcy)/1000.0f; - report.flow_raw_x= val[3] << 8 | val[2]; - report.flow_raw_y= val[5] << 8 | val[4]; - report.ground_distance_m =float(gdist)/1000.0f; - report.quality= val[10]; - report.sensor_id = 0; report.timestamp = hrt_absolute_time(); + report.pixel_flow_x_integral = float(f_integral.pixel_flow_x_integral) / 10000.0f;//convert to radians + report.pixel_flow_y_integral = float(f_integral.pixel_flow_y_integral) / 10000.0f;//convert to radians + report.frame_count_since_last_readout = f_integral.frame_count_since_last_readout; + report.ground_distance_m = float(f_integral.ground_distance) / 1000.0f;//convert to meters + report.quality = f_integral.qual;//0:bad ; 255 max quality + report.gyro_x_rate_integral= float(f_integral.gyro_x_rate_integral)/10000.0f;//convert to radians + report.gyro_y_rate_integral= float(f_integral.gyro_y_rate_integral)/10000.0f;//convert to radians + report.gyro_z_rate_integral= float(f_integral.gyro_z_rate_integral)/10000.0f;//convert to radians + report.integration_timespan= f_integral.integration_timespan;//microseconds + report.time_since_last_sonar_update = f_integral.time_since_last_sonar_update;//microseconds + report.sensor_id = 0; - - /* publish it */ - orb_publish(ORB_ID(optical_flow), _px4flow_topic, &report); + if (_px4flow_topic < 0) { + _px4flow_topic = orb_advertise(ORB_ID(optical_flow), &report); + } else { + /* publish it */ + orb_publish(ORB_ID(optical_flow), _px4flow_topic, &report); + } /* post a report to the ring */ if (_reports->force(&report)) { @@ -505,22 +516,17 @@ PX4FLOW::collect() return ret; } -void -PX4FLOW::start() -{ +void PX4FLOW::start() { /* reset the report ring and state machine */ _collect_phase = false; _reports->flush(); /* schedule a cycle to start things */ - work_queue(HPWORK, &_work, (worker_t)&PX4FLOW::cycle_trampoline, this, 1); + work_queue(HPWORK, &_work, (worker_t) & PX4FLOW::cycle_trampoline, this, 1); /* notify about state change */ - struct subsystem_info_s info = { - true, - true, - true, - SUBSYSTEM_TYPE_OPTICALFLOW}; + struct subsystem_info_s info = { true, true, true, + SUBSYSTEM_TYPE_OPTICALFLOW }; static orb_advert_t pub = -1; if (pub > 0) { @@ -530,71 +536,54 @@ PX4FLOW::start() } } -void -PX4FLOW::stop() -{ +void PX4FLOW::stop() { work_cancel(HPWORK, &_work); } -void -PX4FLOW::cycle_trampoline(void *arg) -{ - PX4FLOW *dev = (PX4FLOW *)arg; +void PX4FLOW::cycle_trampoline(void *arg) { + PX4FLOW *dev = (PX4FLOW *) arg; dev->cycle(); } -void -PX4FLOW::cycle() -{ - /* collection phase? */ - if (_collect_phase) { +void PX4FLOW::cycle() { +// /* collection phase? */ - /* perform collection */ - if (OK != collect()) { - log("collection error"); - /* restart the measurement state machine */ - start(); - return; - } +// static uint64_t deltatime = hrt_absolute_time(); - /* next phase is measurement */ - _collect_phase = false; - - /* - * Is there a collect->measure gap? - */ - if (_measure_ticks > USEC2TICK(PX4FLOW_CONVERSION_INTERVAL)) { - - /* schedule a fresh cycle call when we are ready to measure again */ - work_queue(HPWORK, - &_work, - (worker_t)&PX4FLOW::cycle_trampoline, - this, - _measure_ticks - USEC2TICK(PX4FLOW_CONVERSION_INTERVAL)); - - return; - } - } - - /* measurement phase */ if (OK != measure()) log("measure error"); - /* next phase is collection */ - _collect_phase = true; + //usleep(PX4FLOW_CONVERSION_INTERVAL/40); + + /* perform collection */ + if (OK != collect()) { + log("collection error"); + /* restart the measurement state machine */ + start(); + return; + } + + +// deltatime = hrt_absolute_time()-deltatime; +// +// +// if(deltatime>PX4FLOW_CONVERSION_INTERVAL){ +// deltatime=PX4FLOW_CONVERSION_INTERVAL; +// } + + +// work_queue(HPWORK, &_work, (worker_t) & PX4FLOW::cycle_trampoline, this, +// _measure_ticks-USEC2TICK(deltatime)); + + work_queue(HPWORK, &_work, (worker_t) & PX4FLOW::cycle_trampoline, this, + _measure_ticks); + +// deltatime = hrt_absolute_time(); - /* schedule a fresh cycle call when the measurement is done */ - work_queue(HPWORK, - &_work, - (worker_t)&PX4FLOW::cycle_trampoline, - this, - USEC2TICK(PX4FLOW_CONVERSION_INTERVAL)); } -void -PX4FLOW::print_info() -{ +void PX4FLOW::print_info() { perf_print_counter(_sample_perf); perf_print_counter(_comms_errors); perf_print_counter(_buffer_overflows); @@ -605,8 +594,7 @@ PX4FLOW::print_info() /** * Local functions in support of the shell command. */ -namespace px4flow -{ +namespace px4flow { /* oddly, ERROR is not defined for c++ */ #ifdef ERROR @@ -614,20 +602,18 @@ namespace px4flow #endif const int ERROR = -1; -PX4FLOW *g_dev; +PX4FLOW *g_dev; -void start(); -void stop(); -void test(); -void reset(); -void info(); +void start(); +void stop(); +void test(); +void reset(); +void info(); /** * Start the driver. */ -void -start() -{ +void start() { int fd; if (g_dev != nullptr) @@ -653,10 +639,9 @@ start() exit(0); -fail: + fail: - if (g_dev != nullptr) - { + if (g_dev != nullptr) { delete g_dev; g_dev = nullptr; } @@ -667,15 +652,11 @@ fail: /** * Stop the driver */ -void stop() -{ - if (g_dev != nullptr) - { +void stop() { + if (g_dev != nullptr) { delete g_dev; g_dev = nullptr; - } - else - { + } else { errx(1, "driver not running"); } exit(0); @@ -686,9 +667,7 @@ void stop() * make sure we can collect data from the sensor in polled * and automatic modes. */ -void -test() -{ +void test() { struct optical_flow_s report; ssize_t sz; int ret; @@ -696,26 +675,27 @@ test() int fd = open(PX4FLOW_DEVICE_PATH, O_RDONLY); if (fd < 0) - err(1, "%s open failed (try 'px4flow start' if the driver is not running", PX4FLOW_DEVICE_PATH); + err(1, + "%s open failed (try 'px4flow start' if the driver is not running", + PX4FLOW_DEVICE_PATH); /* do a simple demand read */ sz = read(fd, &report, sizeof(report)); if (sz != sizeof(report)) - // err(1, "immediate read failed"); + // err(1, "immediate read failed"); - warnx("single read"); - warnx("flowx: %0.2f m/s", (double)report.flow_comp_x_m); - warnx("flowy: %0.2f m/s", (double)report.flow_comp_y_m); + warnx("single read"); + warnx("flowx: %0.2f m/s", (double) f.pixel_flow_x_sum); + warnx("flowy: %0.2f m/s", (double) f.pixel_flow_y_sum); warnx("time: %lld", report.timestamp); - - /* start the sensor polling at 2Hz */ - if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) - errx(1, "failed to set 2Hz poll rate"); + /* start the sensor polling at 10Hz */ + if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 10)) // 2)) + errx(1, "failed to set 10Hz poll rate"); /* read the sensor 5x and report each value */ - for (unsigned i = 0; i < 5; i++) { + for (unsigned i = 0; i < 10; i++) { struct pollfd fds; /* wait for data to be ready */ @@ -733,9 +713,22 @@ test() err(1, "periodic read failed"); warnx("periodic read %u", i); - warnx("flowx: %0.2f m/s", (double)report.flow_comp_x_m); - warnx("flowy: %0.2f m/s", (double)report.flow_comp_y_m); - warnx("time: %lld", report.timestamp); + + warnx("framecount_total: %u", f.frame_count); + warnx("framecount_integral: %u", + f_integral.frame_count_since_last_readout); + warnx("pixel_flow_x_integral: %i", f_integral.pixel_flow_x_integral); + warnx("pixel_flow_y_integral: %i", f_integral.pixel_flow_y_integral); + warnx("gyro_x_rate_integral: %i", f_integral.gyro_x_rate_integral); + warnx("gyro_y_rate_integral: %i", f_integral.gyro_y_rate_integral); + warnx("gyro_z_rate_integral: %i", f_integral.gyro_z_rate_integral); + warnx("integration_timespan [us]: %u", f_integral.integration_timespan); + warnx("ground_distance: %0.2f m", + (double) f_integral.ground_distance / 1000); + warnx("time since last sonar update [us]: %i", + f_integral.time_since_last_sonar_update); + warnx("quality integration average : %i", f_integral.qual); + warnx("quality : %i", f.qual); } @@ -746,9 +739,7 @@ test() /** * Reset the driver. */ -void -reset() -{ +void reset() { int fd = open(PX4FLOW_DEVICE_PATH, O_RDONLY); if (fd < 0) @@ -766,9 +757,7 @@ reset() /** * Print a little info about the driver. */ -void -info() -{ +void info() { if (g_dev == nullptr) errx(1, "driver not running"); @@ -780,20 +769,18 @@ info() } // namespace -int -px4flow_main(int argc, char *argv[]) -{ +int px4flow_main(int argc, char *argv[]) { /* * Start/load the driver. */ if (!strcmp(argv[1], "start")) px4flow::start(); - /* - * Stop the driver - */ - if (!strcmp(argv[1], "stop")) - px4flow::stop(); + /* + * Stop the driver + */ + if (!strcmp(argv[1], "stop")) + px4flow::stop(); /* * Test the driver/device. diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 81bbaa6587..bc24e054e7 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -298,7 +298,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float w_flow = 0.0f; float sonar_prev = 0.0f; - hrt_abstime flow_prev = 0; // time of last flow measurement + //hrt_abstime flow_prev = 0; // time of last flow measurement hrt_abstime sonar_time = 0; // time of last sonar measurement (not filtered) hrt_abstime sonar_valid_time = 0; // time of last sonar measurement used for correction (filtered) @@ -491,8 +491,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(optical_flow), optical_flow_sub, &flow); /* calculate time from previous update */ - float flow_dt = flow_prev > 0 ? (flow.flow_timestamp - flow_prev) * 1e-6f : 0.1f; - flow_prev = flow.flow_timestamp; +// float flow_dt = flow_prev > 0 ? (flow.flow_timestamp - flow_prev) * 1e-6f : 0.1f; +// flow_prev = flow.flow_timestamp; if ((flow.ground_distance_m > 0.31f) && (flow.ground_distance_m < 4.0f) && @@ -550,8 +550,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* convert raw flow to angular flow (rad/s) */ float flow_ang[2]; - flow_ang[0] = flow.flow_raw_x * params.flow_k / 1000.0f / flow_dt; - flow_ang[1] = flow.flow_raw_y * params.flow_k / 1000.0f / flow_dt; + //todo check direction of x und y axis + flow_ang[0] = flow.pixel_flow_x_integral/(float)flow.integration_timespan*1000000.0f;//flow.flow_raw_x * params.flow_k / 1000.0f / flow_dt; + flow_ang[1] = flow.pixel_flow_y_integral/(float)flow.integration_timespan*1000000.0f;//flow.flow_raw_y * params.flow_k / 1000.0f / flow_dt; /* flow measurements vector */ float flow_m[3]; flow_m[0] = -flow_ang[0] * flow_dist; diff --git a/src/modules/uORB/topics/optical_flow.h b/src/modules/uORB/topics/optical_flow.h index 0196ae86b0..f3731d2137 100644 --- a/src/modules/uORB/topics/optical_flow.h +++ b/src/modules/uORB/topics/optical_flow.h @@ -55,16 +55,21 @@ */ struct optical_flow_s { - uint64_t timestamp; /**< in microseconds since system start */ - - uint64_t flow_timestamp; /**< timestamp from flow sensor */ - int16_t flow_raw_x; /**< flow in pixels in X direction, not rotation-compensated */ - int16_t flow_raw_y; /**< flow in pixels in Y direction, not rotation-compensated */ - float flow_comp_x_m; /**< speed over ground in meters, rotation-compensated */ - float flow_comp_y_m; /**< speed over ground in meters, rotation-compensated */ - float ground_distance_m; /**< Altitude / distance to ground in meters */ - uint8_t quality; /**< Quality of the measurement, 0: bad quality, 255: maximum quality */ + uint64_t timestamp; /**< in microseconds since system start */ uint8_t sensor_id; /**< id of the sensor emitting the flow value */ + float pixel_flow_x_integral; /**< accumulated optical flow in radians around x axis */ + float pixel_flow_y_integral; /**< accumulated optical flow in radians around y axis */ + float gyro_x_rate_integral; /**< accumulated gyro value in radians around x axis */ + float gyro_y_rate_integral; /**< accumulated gyro value in radians around y axis */ + float gyro_z_rate_integral; /**< accumulated gyro value in radians around z axis */ + float ground_distance_m; /**< Altitude / distance to ground in meters */ + uint32_t integration_timespan; /** Date: Tue, 28 Oct 2014 12:54:27 +0100 Subject: [PATCH 011/216] Fixed formatting of flow driver --- src/drivers/px4flow/px4flow.cpp | 339 +++++++++++++++++++------------- 1 file changed, 206 insertions(+), 133 deletions(-) diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp index f74db1b527..24592a3019 100644 --- a/src/drivers/px4flow/px4flow.cpp +++ b/src/drivers/px4flow/px4flow.cpp @@ -76,7 +76,7 @@ #define PX4FLOW_BUS PX4_I2C_BUS_ESC//PX4_I2C_BUS_EXPANSION #define I2C_FLOW_ADDRESS 0x42 //* 7-bit address. 8-bit address is 0x84 //range 0x42 - 0x49 - + /* PX4FLOW Registers addresses */ //#define PX4FLOW_REG 0x00 /* Measure Register 0*/ #define PX4FLOW_REG 0x16 /* Measure Register 22*/ @@ -119,11 +119,12 @@ typedef struct i2c_integral_frame { uint32_t time_since_last_sonar_update; uint16_t ground_distance; uint8_t qual; -}__attribute__((packed)); +} __attribute__((packed)); struct i2c_integral_frame f_integral; -class PX4FLOW: public device::I2C { +class PX4FLOW: public device::I2C +{ public: PX4FLOW(int bus = PX4FLOW_BUS, int address = I2C_FLOW_ADDRESS); virtual ~PX4FLOW(); @@ -144,8 +145,8 @@ protected: private: work_s _work; - RingBuffer *_reports;bool _sensor_ok; - int _measure_ticks;bool _collect_phase; + RingBuffer *_reports; bool _sensor_ok; + int _measure_ticks; bool _collect_phase; orb_advert_t _px4flow_topic; @@ -198,12 +199,13 @@ private: extern "C" __EXPORT int px4flow_main(int argc, char *argv[]); PX4FLOW::PX4FLOW(int bus, int address) : - I2C("PX4FLOW", PX4FLOW_DEVICE_PATH, bus, address, 400000), //400khz - _reports(nullptr), _sensor_ok(false), _measure_ticks(0), _collect_phase( - false), _px4flow_topic(-1), _sample_perf( - perf_alloc(PC_ELAPSED, "px4flow_read")), _comms_errors( + I2C("PX4FLOW", PX4FLOW_DEVICE_PATH, bus, address, 400000), //400khz + _reports(nullptr), _sensor_ok(false), _measure_ticks(0), _collect_phase( + false), _px4flow_topic(-1), _sample_perf( + perf_alloc(PC_ELAPSED, "px4flow_read")), _comms_errors( perf_alloc(PC_COUNT, "px4flow_comms_errors")), _buffer_overflows( - perf_alloc(PC_COUNT, "px4flow_buffer_overflows")) { + perf_alloc(PC_COUNT, "px4flow_buffer_overflows")) +{ // enable debug() calls _debug_enabled = true; @@ -211,117 +213,131 @@ PX4FLOW::PX4FLOW(int bus, int address) : memset(&_work, 0, sizeof(_work)); } -PX4FLOW::~PX4FLOW() { +PX4FLOW::~PX4FLOW() +{ /* make sure we are truly inactive */ stop(); /* free any existing reports */ - if (_reports != nullptr) + if (_reports != nullptr) { delete _reports; + } } -int PX4FLOW::init() { +int PX4FLOW::init() +{ int ret = ERROR; /* do I2C init (and probe) first */ - if (I2C::init() != OK) + if (I2C::init() != OK) { goto out; + } /* allocate basic report buffers */ _reports = new RingBuffer(2, sizeof(optical_flow_s)); - if (_reports == nullptr) + if (_reports == nullptr) { goto out; + } ret = OK; /* sensor is ok, but we don't really know if it is within range */ _sensor_ok = true; - out: return ret; +out: return ret; } -int PX4FLOW::probe() { +int PX4FLOW::probe() +{ return measure(); } -int PX4FLOW::ioctl(struct file *filp, int cmd, unsigned long arg) { +int PX4FLOW::ioctl(struct file *filp, int cmd, unsigned long arg) +{ switch (cmd) { case SENSORIOCSPOLLRATE: { - switch (arg) { + switch (arg) { - /* switching to manual polling */ - case SENSOR_POLLRATE_MANUAL: - stop(); - _measure_ticks = 0; - return OK; + /* switching to manual polling */ + case SENSOR_POLLRATE_MANUAL: + stop(); + _measure_ticks = 0; + return OK; /* external signalling (DRDY) not supported */ - case SENSOR_POLLRATE_EXTERNAL: + case SENSOR_POLLRATE_EXTERNAL: /* zero would be bad */ - case 0: - return -EINVAL; - - /* set default/max polling rate */ - case SENSOR_POLLRATE_MAX: - case SENSOR_POLLRATE_DEFAULT: { - /* do we need to start internal polling? */ - bool want_start = (_measure_ticks == 0); - - /* set interval for next measurement to minimum legal value */ - _measure_ticks = USEC2TICK(PX4FLOW_CONVERSION_INTERVAL); - - /* if we need to start the poll state machine, do it */ - if (want_start) - start(); - - return OK; - } - - /* adjust to a legal polling interval in Hz */ - default: { - /* do we need to start internal polling? */ - bool want_start = (_measure_ticks == 0); - - /* convert hz to tick interval via microseconds */ - unsigned ticks = USEC2TICK(1000000 / arg); - - /* check against maximum rate */ - if (ticks < USEC2TICK(PX4FLOW_CONVERSION_INTERVAL)) + case 0: return -EINVAL; - /* update interval for next measurement */ - _measure_ticks = ticks; + /* set default/max polling rate */ + case SENSOR_POLLRATE_MAX: + case SENSOR_POLLRATE_DEFAULT: { + /* do we need to start internal polling? */ + bool want_start = (_measure_ticks == 0); - /* if we need to start the poll state machine, do it */ - if (want_start) - start(); + /* set interval for next measurement to minimum legal value */ + _measure_ticks = USEC2TICK(PX4FLOW_CONVERSION_INTERVAL); - return OK; + /* if we need to start the poll state machine, do it */ + if (want_start) { + start(); + } + + return OK; + } + + /* adjust to a legal polling interval in Hz */ + default: { + /* do we need to start internal polling? */ + bool want_start = (_measure_ticks == 0); + + /* convert hz to tick interval via microseconds */ + unsigned ticks = USEC2TICK(1000000 / arg); + + /* check against maximum rate */ + if (ticks < USEC2TICK(PX4FLOW_CONVERSION_INTERVAL)) { + return -EINVAL; + } + + /* update interval for next measurement */ + _measure_ticks = ticks; + + /* if we need to start the poll state machine, do it */ + if (want_start) { + start(); + } + + return OK; + } + } } - } - } case SENSORIOCGPOLLRATE: - if (_measure_ticks == 0) + if (_measure_ticks == 0) { return SENSOR_POLLRATE_MANUAL; + } return (1000 / _measure_ticks); case SENSORIOCSQUEUEDEPTH: { - /* lower bound is mandatory, upper bound is a sanity check */ - if ((arg < 1) || (arg > 100)) - return -EINVAL; + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 1) || (arg > 100)) { + return -EINVAL; + } + + irqstate_t flags = irqsave(); + + if (!_reports->resize(arg)) { + irqrestore(flags); + return -ENOMEM; + } - irqstate_t flags = irqsave(); - if (!_reports->resize(arg)) { irqrestore(flags); - return -ENOMEM; - } - irqrestore(flags); - return OK; - } + return OK; + } case SENSORIOCGQUEUEDEPTH: return _reports->size(); @@ -336,15 +352,17 @@ int PX4FLOW::ioctl(struct file *filp, int cmd, unsigned long arg) { } } -ssize_t PX4FLOW::read(struct file *filp, char *buffer, size_t buflen) { +ssize_t PX4FLOW::read(struct file *filp, char *buffer, size_t buflen) +{ unsigned count = buflen / sizeof(struct optical_flow_s); struct optical_flow_s *rbuf = - reinterpret_cast(buffer); + reinterpret_cast(buffer); int ret = 0; /* buffer must be large enough */ - if (count < 1) + if (count < 1) { return -ENOSPC; + } /* if automatic measurement is enabled */ if (_measure_ticks > 0) { @@ -397,7 +415,8 @@ ssize_t PX4FLOW::read(struct file *filp, char *buffer, size_t buflen) { return ret; } -int PX4FLOW::measure() { +int PX4FLOW::measure() +{ int ret; /* @@ -412,12 +431,14 @@ int PX4FLOW::measure() { printf("i2c::transfer flow returned %d"); return ret; } + ret = OK; return ret; } -int PX4FLOW::collect() { +int PX4FLOW::collect() +{ int ret = -EIO; /* read from the sensor */ @@ -426,10 +447,11 @@ int PX4FLOW::collect() { perf_begin(_sample_perf); - if(PX4FLOW_REG==0x00){ + if (PX4FLOW_REG == 0x00) { ret = transfer(nullptr, 0, &val[0], 45); //read 45 bytes (22+23 : frame1 + frame2) } - if(PX4FLOW_REG==0x16){ + + if (PX4FLOW_REG == 0x16) { ret = transfer(nullptr, 0, &val[0], 23); //read 23 bytes (only frame2) } @@ -461,42 +483,56 @@ int PX4FLOW::collect() { f_integral.gyro_y_rate_integral = val[31] << 8 | val[30]; f_integral.gyro_z_rate_integral = val[33] << 8 | val[32]; f_integral.integration_timespan = val[37] << 24 | val[36] << 16 - | val[35] << 8 | val[34]; + | val[35] << 8 | val[34]; f_integral.time_since_last_sonar_update = val[41] << 24 | val[40] << 16 | val[39] << 8 | val[38]; f_integral.ground_distance = val[43] << 8 | val[42]; f_integral.qual = val[44]; } - if(PX4FLOW_REG==0x16){ + + if (PX4FLOW_REG == 0x16) { f_integral.frame_count_since_last_readout = val[1] << 8 | val[0]; - f_integral.pixel_flow_x_integral =val[3] << 8 | val[2]; - f_integral.pixel_flow_y_integral =val[5] << 8 | val[4]; - f_integral.gyro_x_rate_integral =val[7] << 8 | val[6]; - f_integral.gyro_y_rate_integral =val[9] << 8 | val[8]; - f_integral.gyro_z_rate_integral =val[11] << 8 | val[10]; - f_integral.integration_timespan = val[15] <<24 |val[14] << 16 |val[13] << 8 |val[12]; - f_integral.time_since_last_sonar_update = val[19] <<24 |val[18] << 16 |val[17] << 8 |val[16]; - f_integral.ground_distance =val[21] <<8 |val[20]; - f_integral.qual =val[22]; + f_integral.pixel_flow_x_integral = val[3] << 8 | val[2]; + f_integral.pixel_flow_y_integral = val[5] << 8 | val[4]; + f_integral.gyro_x_rate_integral = val[7] << 8 | val[6]; + f_integral.gyro_y_rate_integral = val[9] << 8 | val[8]; + f_integral.gyro_z_rate_integral = val[11] << 8 | val[10]; + f_integral.integration_timespan = val[15] << 24 | val[14] << 16 | val[13] << 8 | val[12]; + f_integral.time_since_last_sonar_update = val[19] << 24 | val[18] << 16 | val[17] << 8 | val[16]; + f_integral.ground_distance = val[21] << 8 | val[20]; + f_integral.qual = val[22]; } struct optical_flow_s report; + report.timestamp = hrt_absolute_time(); + report.pixel_flow_x_integral = float(f_integral.pixel_flow_x_integral) / 10000.0f;//convert to radians + report.pixel_flow_y_integral = float(f_integral.pixel_flow_y_integral) / 10000.0f;//convert to radians + report.frame_count_since_last_readout = f_integral.frame_count_since_last_readout; + report.ground_distance_m = float(f_integral.ground_distance) / 1000.0f;//convert to meters + report.quality = f_integral.qual;//0:bad ; 255 max quality - report.gyro_x_rate_integral= float(f_integral.gyro_x_rate_integral)/10000.0f;//convert to radians - report.gyro_y_rate_integral= float(f_integral.gyro_y_rate_integral)/10000.0f;//convert to radians - report.gyro_z_rate_integral= float(f_integral.gyro_z_rate_integral)/10000.0f;//convert to radians - report.integration_timespan= f_integral.integration_timespan;//microseconds + + report.gyro_x_rate_integral = float(f_integral.gyro_x_rate_integral) / 10000.0f; //convert to radians + + report.gyro_y_rate_integral = float(f_integral.gyro_y_rate_integral) / 10000.0f; //convert to radians + + report.gyro_z_rate_integral = float(f_integral.gyro_z_rate_integral) / 10000.0f; //convert to radians + + report.integration_timespan = f_integral.integration_timespan; //microseconds + report.time_since_last_sonar_update = f_integral.time_since_last_sonar_update;//microseconds + report.sensor_id = 0; if (_px4flow_topic < 0) { _px4flow_topic = orb_advertise(ORB_ID(optical_flow), &report); + } else { /* publish it */ orb_publish(ORB_ID(optical_flow), _px4flow_topic, &report); @@ -516,7 +552,8 @@ int PX4FLOW::collect() { return ret; } -void PX4FLOW::start() { +void PX4FLOW::start() +{ /* reset the report ring and state machine */ _collect_phase = false; _reports->flush(); @@ -526,33 +563,39 @@ void PX4FLOW::start() { /* notify about state change */ struct subsystem_info_s info = { true, true, true, - SUBSYSTEM_TYPE_OPTICALFLOW }; + SUBSYSTEM_TYPE_OPTICALFLOW + }; static orb_advert_t pub = -1; if (pub > 0) { orb_publish(ORB_ID(subsystem_info), pub, &info); + } else { pub = orb_advertise(ORB_ID(subsystem_info), &info); } } -void PX4FLOW::stop() { +void PX4FLOW::stop() +{ work_cancel(HPWORK, &_work); } -void PX4FLOW::cycle_trampoline(void *arg) { +void PX4FLOW::cycle_trampoline(void *arg) +{ PX4FLOW *dev = (PX4FLOW *) arg; dev->cycle(); } -void PX4FLOW::cycle() { +void PX4FLOW::cycle() +{ // /* collection phase? */ // static uint64_t deltatime = hrt_absolute_time(); - if (OK != measure()) + if (OK != measure()) { log("measure error"); + } //usleep(PX4FLOW_CONVERSION_INTERVAL/40); @@ -577,13 +620,14 @@ void PX4FLOW::cycle() { // _measure_ticks-USEC2TICK(deltatime)); work_queue(HPWORK, &_work, (worker_t) & PX4FLOW::cycle_trampoline, this, - _measure_ticks); + _measure_ticks); // deltatime = hrt_absolute_time(); } -void PX4FLOW::print_info() { +void PX4FLOW::print_info() +{ perf_print_counter(_sample_perf); perf_print_counter(_comms_errors); perf_print_counter(_buffer_overflows); @@ -594,7 +638,8 @@ void PX4FLOW::print_info() { /** * Local functions in support of the shell command. */ -namespace px4flow { +namespace px4flow +{ /* oddly, ERROR is not defined for c++ */ #ifdef ERROR @@ -613,33 +658,39 @@ void info(); /** * Start the driver. */ -void start() { +void start() +{ int fd; - if (g_dev != nullptr) + if (g_dev != nullptr) { errx(1, "already started"); + } /* create the driver */ g_dev = new PX4FLOW(PX4FLOW_BUS); - if (g_dev == nullptr) + if (g_dev == nullptr) { goto fail; + } - if (OK != g_dev->init()) + if (OK != g_dev->init()) { goto fail; + } /* set the poll rate to default, starts automatic data collection */ fd = open(PX4FLOW_DEVICE_PATH, O_RDONLY); - if (fd < 0) + if (fd < 0) { goto fail; + } - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX) < 0) + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX) < 0) { goto fail; + } exit(0); - fail: +fail: if (g_dev != nullptr) { delete g_dev; @@ -652,13 +703,16 @@ void start() { /** * Stop the driver */ -void stop() { +void stop() +{ if (g_dev != nullptr) { delete g_dev; g_dev = nullptr; + } else { errx(1, "driver not running"); } + exit(0); } @@ -667,7 +721,8 @@ void stop() { * make sure we can collect data from the sensor in polled * and automatic modes. */ -void test() { +void test() +{ struct optical_flow_s report; ssize_t sz; int ret; @@ -676,8 +731,8 @@ void test() { if (fd < 0) err(1, - "%s open failed (try 'px4flow start' if the driver is not running", - PX4FLOW_DEVICE_PATH); + "%s open failed (try 'px4flow start' if the driver is not running", + PX4FLOW_DEVICE_PATH); /* do a simple demand read */ sz = read(fd, &report, sizeof(report)); @@ -685,14 +740,18 @@ void test() { if (sz != sizeof(report)) // err(1, "immediate read failed"); + { warnx("single read"); + } + warnx("flowx: %0.2f m/s", (double) f.pixel_flow_x_sum); warnx("flowy: %0.2f m/s", (double) f.pixel_flow_y_sum); warnx("time: %lld", report.timestamp); /* start the sensor polling at 10Hz */ - if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 10)) // 2)) + if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 10)) { // 2)) errx(1, "failed to set 10Hz poll rate"); + } /* read the sensor 5x and report each value */ for (unsigned i = 0; i < 10; i++) { @@ -703,20 +762,22 @@ void test() { fds.events = POLLIN; ret = poll(&fds, 1, 2000); - if (ret != 1) + if (ret != 1) { errx(1, "timed out waiting for sensor data"); + } /* now go get it */ sz = read(fd, &report, sizeof(report)); - if (sz != sizeof(report)) + if (sz != sizeof(report)) { err(1, "periodic read failed"); + } warnx("periodic read %u", i); warnx("framecount_total: %u", f.frame_count); warnx("framecount_integral: %u", - f_integral.frame_count_since_last_readout); + f_integral.frame_count_since_last_readout); warnx("pixel_flow_x_integral: %i", f_integral.pixel_flow_x_integral); warnx("pixel_flow_y_integral: %i", f_integral.pixel_flow_y_integral); warnx("gyro_x_rate_integral: %i", f_integral.gyro_x_rate_integral); @@ -724,9 +785,9 @@ void test() { warnx("gyro_z_rate_integral: %i", f_integral.gyro_z_rate_integral); warnx("integration_timespan [us]: %u", f_integral.integration_timespan); warnx("ground_distance: %0.2f m", - (double) f_integral.ground_distance / 1000); + (double) f_integral.ground_distance / 1000); warnx("time since last sonar update [us]: %i", - f_integral.time_since_last_sonar_update); + f_integral.time_since_last_sonar_update); warnx("quality integration average : %i", f_integral.qual); warnx("quality : %i", f.qual); @@ -739,17 +800,21 @@ void test() { /** * Reset the driver. */ -void reset() { +void reset() +{ int fd = open(PX4FLOW_DEVICE_PATH, O_RDONLY); - if (fd < 0) + if (fd < 0) { err(1, "failed "); + } - if (ioctl(fd, SENSORIOCRESET, 0) < 0) + if (ioctl(fd, SENSORIOCRESET, 0) < 0) { err(1, "driver reset failed"); + } - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { err(1, "driver poll restart failed"); + } exit(0); } @@ -757,9 +822,11 @@ void reset() { /** * Print a little info about the driver. */ -void info() { - if (g_dev == nullptr) +void info() +{ + if (g_dev == nullptr) { errx(1, "driver not running"); + } printf("state @ %p\n", g_dev); g_dev->print_info(); @@ -769,36 +836,42 @@ void info() { } // namespace -int px4flow_main(int argc, char *argv[]) { +int px4flow_main(int argc, char *argv[]) +{ /* * Start/load the driver. */ - if (!strcmp(argv[1], "start")) + if (!strcmp(argv[1], "start")) { px4flow::start(); + } /* * Stop the driver */ - if (!strcmp(argv[1], "stop")) + if (!strcmp(argv[1], "stop")) { px4flow::stop(); + } /* * Test the driver/device. */ - if (!strcmp(argv[1], "test")) + if (!strcmp(argv[1], "test")) { px4flow::test(); + } /* * Reset the driver. */ - if (!strcmp(argv[1], "reset")) + if (!strcmp(argv[1], "reset")) { px4flow::reset(); + } /* * Print driver information. */ - if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) + if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) { px4flow::info(); + } errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'"); } From 1418254fca7ec97ea2b502ce227a77a6957424b9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 28 Oct 2014 13:10:39 +0100 Subject: [PATCH 012/216] Formatting fixes --- src/drivers/px4flow/px4flow.cpp | 104 +++++++++++++++++++------------- 1 file changed, 63 insertions(+), 41 deletions(-) diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp index 29f26cab96..96c0b720cb 100644 --- a/src/drivers/px4flow/px4flow.cpp +++ b/src/drivers/px4flow/px4flow.cpp @@ -129,30 +129,32 @@ public: PX4FLOW(int bus = PX4FLOW_BUS, int address = I2C_FLOW_ADDRESS); virtual ~PX4FLOW(); - virtual int init(); + virtual int init(); - virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); - virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); /** * Diagnostics - print some basic information about the driver. */ - void print_info(); + void print_info(); protected: - virtual int probe(); + virtual int probe(); private: - work_s _work; - RingBuffer *_reports; bool _sensor_ok; - int _measure_ticks; bool _collect_phase; + work_s _work; + RingBuffer *_reports; + bool _sensor_ok; + int _measure_ticks; + bool _collect_phase; - orb_advert_t _px4flow_topic; + orb_advert_t _px4flow_topic; - perf_counter_t _sample_perf; - perf_counter_t _comms_errors; - perf_counter_t _buffer_overflows; + perf_counter_t _sample_perf; + perf_counter_t _comms_errors; + perf_counter_t _buffer_overflows; /** * Test whether the device supported by the driver is present at a @@ -161,7 +163,7 @@ private: * @param address The I2C bus address to probe. * @return True if the device is present. */ - int probe_address(uint8_t address); + int probe_address(uint8_t address); /** * Initialise the automatic measurement state machine and start it. @@ -169,27 +171,27 @@ private: * @note This function is called at open and error time. It might make sense * to make it more aggressive about resetting the bus in case of errors. */ - void start(); + void start(); /** * Stop the automatic measurement state machine. */ - void stop(); + void stop(); /** * Perform a poll cycle; collect from the previous measurement * and start a new one. */ - void cycle(); - int measure(); - int collect(); + void cycle(); + int measure(); + int collect(); /** * Static trampoline from the workq context; because we don't have a * generic workq wrapper yet. * * @param arg Instance pointer for the driver that is polling. */ - static void cycle_trampoline(void *arg); + static void cycle_trampoline(void *arg); }; @@ -200,11 +202,14 @@ extern "C" __EXPORT int px4flow_main(int argc, char *argv[]); PX4FLOW::PX4FLOW(int bus, int address) : I2C("PX4FLOW", PX4FLOW_DEVICE_PATH, bus, address, 400000), //400khz - _reports(nullptr), _sensor_ok(false), _measure_ticks(0), _collect_phase( - false), _px4flow_topic(-1), _sample_perf( - perf_alloc(PC_ELAPSED, "px4flow_read")), _comms_errors( - perf_alloc(PC_COUNT, "px4flow_comms_errors")), _buffer_overflows( - perf_alloc(PC_COUNT, "px4flow_buffer_overflows")) + _reports(nullptr), + _sensor_ok(false), + _measure_ticks(0), + _collect_phase(false), + _px4flow_topic(-1), + _sample_perf(perf_alloc(PC_ELAPSED, "px4flow_read")), + _comms_errors(perf_alloc(PC_COUNT, "px4flow_comms_errors")), + _buffer_overflows(perf_alloc(PC_COUNT, "px4flow_buffer_overflows")) { // enable debug() calls _debug_enabled = true; @@ -224,7 +229,8 @@ PX4FLOW::~PX4FLOW() } } -int PX4FLOW::init() +int +PX4FLOW::init() { int ret = ERROR; @@ -246,7 +252,8 @@ int PX4FLOW::init() out: return ret; } -int PX4FLOW::probe() +int +PX4FLOW::probe() { uint8_t val[22]; @@ -262,7 +269,8 @@ int PX4FLOW::probe() return measure(); } -int PX4FLOW::ioctl(struct file *filp, int cmd, unsigned long arg) +int +PX4FLOW::ioctl(struct file *filp, int cmd, unsigned long arg) { switch (cmd) { @@ -363,7 +371,8 @@ int PX4FLOW::ioctl(struct file *filp, int cmd, unsigned long arg) } } -ssize_t PX4FLOW::read(struct file *filp, char *buffer, size_t buflen) +ssize_t +PX4FLOW::read(struct file *filp, char *buffer, size_t buflen) { unsigned count = buflen / sizeof(struct optical_flow_s); struct optical_flow_s *rbuf = @@ -426,7 +435,8 @@ ssize_t PX4FLOW::read(struct file *filp, char *buffer, size_t buflen) return ret; } -int PX4FLOW::measure() +int +PX4FLOW::measure() { int ret; @@ -448,7 +458,8 @@ int PX4FLOW::measure() return ret; } -int PX4FLOW::collect() +int +PX4FLOW::collect() { int ret = -EIO; @@ -562,7 +573,8 @@ int PX4FLOW::collect() return ret; } -void PX4FLOW::start() +void +PX4FLOW::start() { /* reset the report ring and state machine */ _collect_phase = false; @@ -588,19 +600,22 @@ void PX4FLOW::start() } } -void PX4FLOW::stop() +void +PX4FLOW::stop() { work_cancel(HPWORK, &_work); } -void PX4FLOW::cycle_trampoline(void *arg) +void +PX4FLOW::cycle_trampoline(void *arg) { PX4FLOW *dev = (PX4FLOW *) arg; dev->cycle(); } -void PX4FLOW::cycle() +void +PX4FLOW::cycle() { if (OK != measure()) { log("measure error"); @@ -619,7 +634,8 @@ void PX4FLOW::cycle() } -void PX4FLOW::print_info() +void +PX4FLOW::print_info() { perf_print_counter(_sample_perf); perf_print_counter(_comms_errors); @@ -651,7 +667,8 @@ void info(); /** * Start the driver. */ -void start() +void +start() { int fd; @@ -696,7 +713,8 @@ fail: /** * Stop the driver */ -void stop() +void +stop() { if (g_dev != nullptr) { delete g_dev; @@ -714,7 +732,8 @@ void stop() * make sure we can collect data from the sensor in polled * and automatic modes. */ -void test() +void +test() { struct optical_flow_s report; ssize_t sz; @@ -792,7 +811,8 @@ void test() /** * Reset the driver. */ -void reset() +void +reset() { int fd = open(PX4FLOW_DEVICE_PATH, O_RDONLY); @@ -814,7 +834,8 @@ void reset() /** * Print a little info about the driver. */ -void info() +void +info() { if (g_dev == nullptr) { errx(1, "driver not running"); @@ -828,7 +849,8 @@ void info() } // namespace -int px4flow_main(int argc, char *argv[]) +int +px4flow_main(int argc, char *argv[]) { /* * Start/load the driver. From fd4418278e0dbd9442c33a0fe8fcf11d86804092 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 28 Oct 2014 13:14:37 +0100 Subject: [PATCH 013/216] More formatting and cast fixes --- src/drivers/px4flow/px4flow.cpp | 35 +++++++++++---------------------- 1 file changed, 11 insertions(+), 24 deletions(-) diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp index 96c0b720cb..e9b2de6291 100644 --- a/src/drivers/px4flow/px4flow.cpp +++ b/src/drivers/px4flow/px4flow.cpp @@ -145,9 +145,9 @@ protected: private: work_s _work; - RingBuffer *_reports; + RingBuffer *_reports; bool _sensor_ok; - int _measure_ticks; + int _measure_ticks; bool _collect_phase; orb_advert_t _px4flow_topic; @@ -249,7 +249,8 @@ PX4FLOW::init() ret = OK; /* sensor is ok, but we don't really know if it is within range */ _sensor_ok = true; -out: return ret; +out: + return ret; } int @@ -375,8 +376,7 @@ ssize_t PX4FLOW::read(struct file *filp, char *buffer, size_t buflen) { unsigned count = buflen / sizeof(struct optical_flow_s); - struct optical_flow_s *rbuf = - reinterpret_cast(buffer); + struct optical_flow_s *rbuf = reinterpret_cast(buffer); int ret = 0; /* buffer must be large enough */ @@ -413,9 +413,6 @@ PX4FLOW::read(struct file *filp, char *buffer, size_t buflen) break; } -// /* wait for it to complete */ -// usleep(PX4FLOW_CONVERSION_INTERVAL); - /* run the collection phase */ if (OK != collect()) { ret = -EIO; @@ -528,25 +525,15 @@ PX4FLOW::collect() struct optical_flow_s report; report.timestamp = hrt_absolute_time(); - - report.pixel_flow_x_integral = float(f_integral.pixel_flow_x_integral) / 10000.0f;//convert to radians - - report.pixel_flow_y_integral = float(f_integral.pixel_flow_y_integral) / 10000.0f;//convert to radians - + report.pixel_flow_x_integral = static_cast(f_integral.pixel_flow_x_integral) / 10000.0f;//convert to radians + report.pixel_flow_y_integral = static_cast(f_integral.pixel_flow_y_integral) / 10000.0f;//convert to radians report.frame_count_since_last_readout = f_integral.frame_count_since_last_readout; - - report.ground_distance_m = float(f_integral.ground_distance) / 1000.0f;//convert to meters - + report.ground_distance_m = static_cast(f_integral.ground_distance) / 1000.0f;//convert to meters report.quality = f_integral.qual; //0:bad ; 255 max quality - - report.gyro_x_rate_integral = float(f_integral.gyro_x_rate_integral) / 10000.0f; //convert to radians - - report.gyro_y_rate_integral = float(f_integral.gyro_y_rate_integral) / 10000.0f; //convert to radians - - report.gyro_z_rate_integral = float(f_integral.gyro_z_rate_integral) / 10000.0f; //convert to radians - + report.gyro_x_rate_integral = static_cast(f_integral.gyro_x_rate_integral) / 10000.0f; //convert to radians + report.gyro_y_rate_integral = static_cast(f_integral.gyro_y_rate_integral) / 10000.0f; //convert to radians + report.gyro_z_rate_integral = static_cast(f_integral.gyro_z_rate_integral) / 10000.0f; //convert to radians report.integration_timespan = f_integral.integration_timespan; //microseconds - report.time_since_last_sonar_update = f_integral.time_since_last_sonar_update;//microseconds report.sensor_id = 0; From 16f33ee6b5b92e244bb413bad5530882fac6644d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 28 Oct 2014 13:17:00 +0100 Subject: [PATCH 014/216] More formatting fixes --- src/drivers/px4flow/px4flow.cpp | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp index e9b2de6291..818b34a5ed 100644 --- a/src/drivers/px4flow/px4flow.cpp +++ b/src/drivers/px4flow/px4flow.cpp @@ -568,7 +568,7 @@ PX4FLOW::start() _reports->flush(); /* schedule a cycle to start things */ - work_queue(HPWORK, &_work, (worker_t) & PX4FLOW::cycle_trampoline, this, 1); + work_queue(HPWORK, &_work, (worker_t)&PX4FLOW::cycle_trampoline, this, 1); /* notify about state change */ struct subsystem_info_s info = { @@ -596,7 +596,7 @@ PX4FLOW::stop() void PX4FLOW::cycle_trampoline(void *arg) { - PX4FLOW *dev = (PX4FLOW *) arg; + PX4FLOW *dev = (PX4FLOW *)arg; dev->cycle(); } @@ -616,7 +616,7 @@ PX4FLOW::cycle() return; } - work_queue(HPWORK, &_work, (worker_t) & PX4FLOW::cycle_trampoline, this, + work_queue(HPWORK, &_work, (worker_t)&PX4FLOW::cycle_trampoline, this, _measure_ticks); } @@ -643,13 +643,13 @@ namespace px4flow #endif const int ERROR = -1; -PX4FLOW *g_dev; +PX4FLOW *g_dev; -void start(); -void stop(); -void test(); -void reset(); -void info(); +void start(); +void stop(); +void test(); +void reset(); +void info(); /** * Start the driver. From 774ff3db31309c2f2aeb235d751cbdfc9e20baf8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 28 Oct 2014 13:22:25 +0100 Subject: [PATCH 015/216] Scan both buses --- src/drivers/px4flow/px4flow.cpp | 18 +++++++++++++----- 1 file changed, 13 insertions(+), 5 deletions(-) diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp index 818b34a5ed..ed788a46ee 100644 --- a/src/drivers/px4flow/px4flow.cpp +++ b/src/drivers/px4flow/px4flow.cpp @@ -73,13 +73,11 @@ #include /* Configuration Constants */ -#define PX4FLOW_BUS PX4_I2C_BUS_ESC//PX4_I2C_BUS_EXPANSION #define I2C_FLOW_ADDRESS 0x42 //* 7-bit address. 8-bit address is 0x84 //range 0x42 - 0x49 /* PX4FLOW Registers addresses */ -//#define PX4FLOW_REG 0x00 /* Measure Register 0*/ -#define PX4FLOW_REG 0x16 /* Measure Register 22*/ +#define PX4FLOW_REG 0x16 /* Measure Register 22*/ #define PX4FLOW_CONVERSION_INTERVAL 20000 //in microseconds! 20000 = 50 Hz 100000 = 10Hz /* oddly, ERROR is not defined for c++ */ @@ -664,14 +662,24 @@ start() } /* create the driver */ - g_dev = new PX4FLOW(PX4FLOW_BUS); + g_dev = new PX4FLOW(PX4_I2C_BUS_EXPANSION); if (g_dev == nullptr) { goto fail; } if (OK != g_dev->init()) { - goto fail; + delete g_dev; + /* try 2nd bus */ + g_dev = new PX4FLOW(PX4_I2C_BUS_ESC); + + if (g_dev == nullptr) { + goto fail; + } + + if (OK != g_dev->init()) { + goto fail; + } } /* set the poll rate to default, starts automatic data collection */ From f443b77ae9351b360a0c735122b4c4c1629fa870 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 28 Oct 2014 13:24:12 +0100 Subject: [PATCH 016/216] Kill last usleep() --- src/drivers/px4flow/px4flow.cpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp index ed788a46ee..3607adf9c1 100644 --- a/src/drivers/px4flow/px4flow.cpp +++ b/src/drivers/px4flow/px4flow.cpp @@ -417,9 +417,6 @@ PX4FLOW::read(struct file *filp, char *buffer, size_t buflen) break; } - /* wait for it to complete */ - usleep(PX4FLOW_CONVERSION_INTERVAL); - /* state machine will have generated a report, copy it out */ if (_reports->get(rbuf)) { ret = sizeof(*rbuf); From 2e4931e3cae181ee1092c693d548437641b4e1d3 Mon Sep 17 00:00:00 2001 From: dominiho Date: Tue, 28 Oct 2014 14:04:50 +0100 Subject: [PATCH 017/216] deleted last PX4FLOW_BUS to enable scan on both buses --- src/drivers/px4flow/px4flow.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp index 3607adf9c1..87bcb32ecb 100644 --- a/src/drivers/px4flow/px4flow.cpp +++ b/src/drivers/px4flow/px4flow.cpp @@ -124,7 +124,7 @@ struct i2c_integral_frame f_integral; class PX4FLOW: public device::I2C { public: - PX4FLOW(int bus = PX4FLOW_BUS, int address = I2C_FLOW_ADDRESS); + PX4FLOW(int bus, int address = I2C_FLOW_ADDRESS); virtual ~PX4FLOW(); virtual int init(); From 4a3a16475e3d900cb2b41a0f5fb4bc1f46077b7d Mon Sep 17 00:00:00 2001 From: dominiho Date: Tue, 28 Oct 2014 15:05:41 +0100 Subject: [PATCH 018/216] scan also 3rd available bus --- src/drivers/px4flow/px4flow.cpp | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp index 87bcb32ecb..c3660a967d 100644 --- a/src/drivers/px4flow/px4flow.cpp +++ b/src/drivers/px4flow/px4flow.cpp @@ -675,7 +675,17 @@ start() } if (OK != g_dev->init()) { - goto fail; + delete g_dev; + /* try 3rd bus */ + g_dev = new PX4FLOW(PX4_I2C_BUS_ONBOARD); + + if (g_dev == nullptr) { + goto fail; + } + + if (OK != g_dev->init()) { + goto fail; + } } } From 9f64953bb9f7a51042922f30ad15a367d82fb4d5 Mon Sep 17 00:00:00 2001 From: dominiho Date: Thu, 30 Oct 2014 16:39:02 +0100 Subject: [PATCH 019/216] replaced optical_flow mavlink message with optical_flow_rad, added gyro_temperature, adapted sd2log for px4flow integral frame --- src/drivers/px4flow/px4flow.cpp | 14 +++++--- src/modules/mavlink/mavlink_main.cpp | 2 +- src/modules/mavlink/mavlink_messages.cpp | 40 +++++++++++---------- src/modules/mavlink/mavlink_receiver.cpp | 46 +++++++++++++----------- src/modules/mavlink/mavlink_receiver.h | 2 +- src/modules/sdlog2/sdlog2.c | 13 ++++--- src/modules/sdlog2/sdlog2_messages.h | 18 ++++++---- src/modules/uORB/topics/optical_flow.h | 1 + 8 files changed, 80 insertions(+), 56 deletions(-) diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp index c3660a967d..daaf02a777 100644 --- a/src/drivers/px4flow/px4flow.cpp +++ b/src/drivers/px4flow/px4flow.cpp @@ -116,6 +116,7 @@ typedef struct i2c_integral_frame { uint32_t integration_timespan; uint32_t time_since_last_sonar_update; uint16_t ground_distance; + int16_t gyro_temperature; uint8_t qual; } __attribute__((packed)); struct i2c_integral_frame f_integral; @@ -456,16 +457,16 @@ PX4FLOW::collect() int ret = -EIO; /* read from the sensor */ - uint8_t val[46] = { 0 }; + uint8_t val[47] = { 0 }; perf_begin(_sample_perf); if (PX4FLOW_REG == 0x00) { - ret = transfer(nullptr, 0, &val[0], 45); // read 45 bytes (22+23 : frame1 + frame2) + ret = transfer(nullptr, 0, &val[0], 47); // read 47 bytes (22+25 : frame1 + frame2) } if (PX4FLOW_REG == 0x16) { - ret = transfer(nullptr, 0, &val[0], 23); // read 23 bytes (only frame2) + ret = transfer(nullptr, 0, &val[0], 25); // read 25 bytes (only frame2) } if (ret < 0) { @@ -500,7 +501,8 @@ PX4FLOW::collect() f_integral.time_since_last_sonar_update = val[41] << 24 | val[40] << 16 | val[39] << 8 | val[38]; f_integral.ground_distance = val[43] << 8 | val[42]; - f_integral.qual = val[44]; + f_integral.gyro_temperature = val[45] << 8 | val[44]; + f_integral.qual = val[46]; } if (PX4FLOW_REG == 0x16) { @@ -513,7 +515,8 @@ PX4FLOW::collect() f_integral.integration_timespan = val[15] << 24 | val[14] << 16 | val[13] << 8 | val[12]; f_integral.time_since_last_sonar_update = val[19] << 24 | val[18] << 16 | val[17] << 8 | val[16]; f_integral.ground_distance = val[21] << 8 | val[20]; - f_integral.qual = val[22]; + f_integral.gyro_temperature = val[23] << 8 | val[22]; + f_integral.qual = val[24]; } @@ -530,6 +533,7 @@ PX4FLOW::collect() report.gyro_z_rate_integral = static_cast(f_integral.gyro_z_rate_integral) / 10000.0f; //convert to radians report.integration_timespan = f_integral.integration_timespan; //microseconds report.time_since_last_sonar_update = f_integral.time_since_last_sonar_update;//microseconds + report.gyro_temperature = f_integral.gyro_temperature;//Temperature * 100 in centi-degrees Celsius report.sensor_id = 0; diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 4448f8d6f3..3c1d360c23 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1403,7 +1403,7 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("POSITION_TARGET_GLOBAL_INT", 3.0f); configure_stream("ATTITUDE_TARGET", 3.0f); configure_stream("DISTANCE_SENSOR", 0.5f); - configure_stream("OPTICAL_FLOW", 5.0f); + configure_stream("OPTICAL_FLOW_RAD", 5.0f); break; case MAVLINK_MODE_ONBOARD: diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index cccb698bf9..d955b5f76e 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1786,33 +1786,32 @@ protected: } }; - -class MavlinkStreamOpticalFlow : public MavlinkStream +class MavlinkStreamOpticalFlowRad : public MavlinkStream { public: const char *get_name() const { - return MavlinkStreamOpticalFlow::get_name_static(); + return MavlinkStreamOpticalFlowRad::get_name_static(); } static const char *get_name_static() { - return "OPTICAL_FLOW"; + return "OPTICAL_FLOW_RAD"; } uint8_t get_id() { - return MAVLINK_MSG_ID_OPTICAL_FLOW; + return MAVLINK_MSG_ID_OPTICAL_FLOW_RAD; } static MavlinkStream *new_instance(Mavlink *mavlink) { - return new MavlinkStreamOpticalFlow(mavlink); + return new MavlinkStreamOpticalFlowRad(mavlink); } unsigned get_size() { - return _flow_sub->is_published() ? (MAVLINK_MSG_ID_OPTICAL_FLOW_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; + return _flow_sub->is_published() ? (MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; } private: @@ -1820,11 +1819,11 @@ private: uint64_t _flow_time; /* do not allow top copying this class */ - MavlinkStreamOpticalFlow(MavlinkStreamOpticalFlow &); - MavlinkStreamOpticalFlow& operator = (const MavlinkStreamOpticalFlow &); + MavlinkStreamOpticalFlowRad(MavlinkStreamOpticalFlowRad &); + MavlinkStreamOpticalFlowRad& operator = (const MavlinkStreamOpticalFlowRad &); protected: - explicit MavlinkStreamOpticalFlow(Mavlink *mavlink) : MavlinkStream(mavlink), + explicit MavlinkStreamOpticalFlowRad(Mavlink *mavlink) : MavlinkStream(mavlink), _flow_sub(_mavlink->add_orb_subscription(ORB_ID(optical_flow))), _flow_time(0) {} @@ -1834,18 +1833,23 @@ protected: struct optical_flow_s flow; if (_flow_sub->update(&_flow_time, &flow)) { - mavlink_optical_flow_t msg; + mavlink_optical_flow_rad_t msg; msg.time_usec = flow.timestamp; msg.sensor_id = flow.sensor_id; - msg.flow_x = flow.flow_raw_x; - msg.flow_y = flow.flow_raw_y; - msg.flow_comp_m_x = flow.flow_comp_x_m; - msg.flow_comp_m_y = flow.flow_comp_y_m; + msg.integrated_x = flow.pixel_flow_x_integral; + msg.integrated_y = flow.pixel_flow_y_integral; + msg.integrated_xgyro = flow.gyro_x_rate_integral; + msg.integrated_ygyro = flow.gyro_y_rate_integral; + msg.integrated_zgyro = flow.gyro_z_rate_integral; + msg.distance = flow.ground_distance_m; msg.quality = flow.quality; - msg.ground_distance = flow.ground_distance_m; + msg.integration_time_us = flow.integration_timespan; + msg.sensor_id = flow.sensor_id; + msg.time_delta_distance_us = flow.time_since_last_sonar_update; + msg.temperature = flow.gyro_temperature; - _mavlink->send_message(MAVLINK_MSG_ID_OPTICAL_FLOW, &msg); + _mavlink->send_message(MAVLINK_MSG_ID_OPTICAL_FLOW_RAD, &msg); } } }; @@ -2151,7 +2155,7 @@ StreamListItem *streams_list[] = { new StreamListItem(&MavlinkStreamAttitudeTarget::new_instance, &MavlinkStreamAttitudeTarget::get_name_static), new StreamListItem(&MavlinkStreamRCChannelsRaw::new_instance, &MavlinkStreamRCChannelsRaw::get_name_static), new StreamListItem(&MavlinkStreamManualControl::new_instance, &MavlinkStreamManualControl::get_name_static), - new StreamListItem(&MavlinkStreamOpticalFlow::new_instance, &MavlinkStreamOpticalFlow::get_name_static), + new StreamListItem(&MavlinkStreamOpticalFlowRad::new_instance, &MavlinkStreamOpticalFlowRad::get_name_static), new StreamListItem(&MavlinkStreamAttitudeControls::new_instance, &MavlinkStreamAttitudeControls::get_name_static), new StreamListItem(&MavlinkStreamNamedValueFloat::new_instance, &MavlinkStreamNamedValueFloat::get_name_static), new StreamListItem(&MavlinkStreamCameraCapture::new_instance, &MavlinkStreamCameraCapture::get_name_static), diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index e8d783847c..bee58f89b6 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -144,8 +144,8 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) handle_message_command_int(msg); break; - case MAVLINK_MSG_ID_OPTICAL_FLOW: - handle_message_optical_flow(msg); + case MAVLINK_MSG_ID_OPTICAL_FLOW_RAD: + handle_message_optical_flow_rad(msg); break; case MAVLINK_MSG_ID_SET_MODE: @@ -352,24 +352,27 @@ MavlinkReceiver::handle_message_command_int(mavlink_message_t *msg) } void -MavlinkReceiver::handle_message_optical_flow(mavlink_message_t *msg) +MavlinkReceiver::handle_message_optical_flow_rad(mavlink_message_t *msg) { /* optical flow */ - mavlink_optical_flow_t flow; - mavlink_msg_optical_flow_decode(msg, &flow); + mavlink_optical_flow_rad_t flow; + mavlink_msg_optical_flow_rad_decode(msg, &flow); struct optical_flow_s f; memset(&f, 0, sizeof(f)); - f.timestamp = hrt_absolute_time(); - f.flow_timestamp = flow.time_usec; - f.flow_raw_x = flow.flow_x; - f.flow_raw_y = flow.flow_y; - f.flow_comp_x_m = flow.flow_comp_m_x; - f.flow_comp_y_m = flow.flow_comp_m_y; - f.ground_distance_m = flow.ground_distance; + f.timestamp = flow.time_usec; + f.integration_timespan = flow.integration_time_us; + f.pixel_flow_x_integral = flow.integrated_x; + f.pixel_flow_y_integral = flow.integrated_y; + f.gyro_x_rate_integral = flow.integrated_xgyro; + f.gyro_y_rate_integral = flow.integrated_ygyro; + f.gyro_z_rate_integral = flow.integrated_zgyro; + f.time_since_last_sonar_update = flow.time_delta_distance_us; + f.ground_distance_m = flow.distance; f.quality = flow.quality; f.sensor_id = flow.sensor_id; + f.gyro_temperature = flow.temperature; if (_flow_pub < 0) { _flow_pub = orb_advertise(ORB_ID(optical_flow), &f); @@ -389,15 +392,18 @@ MavlinkReceiver::handle_message_hil_optical_flow(mavlink_message_t *msg) struct optical_flow_s f; memset(&f, 0, sizeof(f)); - f.timestamp = hrt_absolute_time(); - f.flow_timestamp = flow.time_usec; - f.flow_raw_x = flow.flow_x; - f.flow_raw_y = flow.flow_y; - f.flow_comp_x_m = flow.flow_comp_m_x; - f.flow_comp_y_m = flow.flow_comp_m_y; - f.ground_distance_m = flow.ground_distance; + f.timestamp = flow.time_usec; + f.integration_timespan = flow.integration_time_us; + f.pixel_flow_x_integral = flow.integrated_x; + f.pixel_flow_y_integral = flow.integrated_y; + f.gyro_x_rate_integral = flow.integrated_xgyro; + f.gyro_y_rate_integral = flow.integrated_ygyro; + f.gyro_z_rate_integral = flow.integrated_zgyro; + f.time_since_last_sonar_update = flow.time_delta_distance_us; + f.ground_distance_m = flow.distance; f.quality = flow.quality; f.sensor_id = flow.sensor_id; + f.gyro_temperature = flow.temperature; if (_flow_pub < 0) { _flow_pub = orb_advertise(ORB_ID(optical_flow), &f); @@ -413,7 +419,7 @@ MavlinkReceiver::handle_message_hil_optical_flow(mavlink_message_t *msg) r.timestamp = hrt_absolute_time(); r.error_count = 0; r.type = RANGE_FINDER_TYPE_LASER; - r.distance = flow.ground_distance; + r.distance = flow.distance; r.minimum_distance = 0.0f; r.maximum_distance = 40.0f; // this is set to match the typical range of real sensors, could be made configurable r.valid = (r.distance > r.minimum_distance) && (r.distance < r.maximum_distance); diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index e5f2c6a73c..a057074a78 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -112,7 +112,7 @@ private: void handle_message(mavlink_message_t *msg); void handle_message_command_long(mavlink_message_t *msg); void handle_message_command_int(mavlink_message_t *msg); - void handle_message_optical_flow(mavlink_message_t *msg); + void handle_message_optical_flow_rad(mavlink_message_t *msg); void handle_message_hil_optical_flow(mavlink_message_t *msg); void handle_message_set_mode(mavlink_message_t *msg); void handle_message_vicon_position_estimate(mavlink_message_t *msg); diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 9bde374324..54c7b0c836 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1507,11 +1507,14 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- FLOW --- */ if (copy_if_updated(ORB_ID(optical_flow), subs.flow_sub, &buf.flow)) { log_msg.msg_type = LOG_FLOW_MSG; - log_msg.body.log_FLOW.flow_raw_x = buf.flow.flow_raw_x; - log_msg.body.log_FLOW.flow_raw_y = buf.flow.flow_raw_y; - log_msg.body.log_FLOW.flow_comp_x = buf.flow.flow_comp_x_m; - log_msg.body.log_FLOW.flow_comp_y = buf.flow.flow_comp_y_m; - log_msg.body.log_FLOW.distance = buf.flow.ground_distance_m; + log_msg.body.log_FLOW.ground_distance_m = buf.flow.ground_distance_m; + log_msg.body.log_FLOW.gyro_temperature = buf.flow.gyro_temperature; + log_msg.body.log_FLOW.gyro_x_rate_integral = buf.flow.gyro_x_rate_integral; + log_msg.body.log_FLOW.gyro_y_rate_integral = buf.flow.gyro_y_rate_integral; + log_msg.body.log_FLOW.gyro_z_rate_integral = buf.flow.gyro_z_rate_integral; + log_msg.body.log_FLOW.integration_timespan = buf.flow.integration_timespan; + log_msg.body.log_FLOW.pixel_flow_x_integral = buf.flow.pixel_flow_x_integral; + log_msg.body.log_FLOW.pixel_flow_y_integral = buf.flow.pixel_flow_y_integral; log_msg.body.log_FLOW.quality = buf.flow.quality; log_msg.body.log_FLOW.sensor_id = buf.flow.sensor_id; LOGBUFFER_WRITE_AND_COUNT(FLOW); diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index d49fc0c799..5264ff1c8a 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -200,13 +200,19 @@ struct log_ARSP_s { /* --- FLOW - OPTICAL FLOW --- */ #define LOG_FLOW_MSG 15 struct log_FLOW_s { - int16_t flow_raw_x; - int16_t flow_raw_y; - float flow_comp_x; - float flow_comp_y; - float distance; - uint8_t quality; + uint64_t timestamp; uint8_t sensor_id; + float pixel_flow_x_integral; + float pixel_flow_y_integral; + float gyro_x_rate_integral; + float gyro_y_rate_integral; + float gyro_z_rate_integral; + float ground_distance_m; + uint32_t integration_timespan; + uint32_t time_since_last_sonar_update; + uint16_t frame_count_since_last_readout; + int16_t gyro_temperature; + uint8_t quality; }; /* --- GPOS - GLOBAL POSITION ESTIMATE --- */ diff --git a/src/modules/uORB/topics/optical_flow.h b/src/modules/uORB/topics/optical_flow.h index f3731d2137..d3dc46ee0b 100644 --- a/src/modules/uORB/topics/optical_flow.h +++ b/src/modules/uORB/topics/optical_flow.h @@ -66,6 +66,7 @@ struct optical_flow_s { uint32_t integration_timespan; /** Date: Thu, 30 Oct 2014 17:51:49 +0100 Subject: [PATCH 020/216] removed debug printf, adjusted test routine single read --- src/drivers/px4flow/px4flow.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp index daaf02a777..af3a5d39c6 100644 --- a/src/drivers/px4flow/px4flow.cpp +++ b/src/drivers/px4flow/px4flow.cpp @@ -442,7 +442,6 @@ PX4FLOW::measure() if (OK != ret) { perf_count(_comms_errors); log("i2c::transfer returned %d", ret); - printf("i2c::transfer flow returned %d"); return ret; } @@ -761,9 +760,10 @@ test() } warnx("single read"); - warnx("flowx: %0.2f m/s", (double) f.pixel_flow_x_sum); - warnx("flowy: %0.2f m/s", (double) f.pixel_flow_y_sum); - warnx("time: %lld", report.timestamp); + warnx("pixel_flow_x_integral: %i", f_integral.pixel_flow_x_integral); + warnx("pixel_flow_y_integral: %i", f_integral.pixel_flow_y_integral); + warnx("framecount_integral: %u", + f_integral.frame_count_since_last_readout); /* start the sensor polling at 10Hz */ if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 10)) { From 078e81a5cb6a0558eea9e589595889d1aad0cbe7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 31 Oct 2014 11:42:36 +0100 Subject: [PATCH 021/216] Build for ESC bus conditionally --- src/drivers/px4flow/px4flow.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp index af3a5d39c6..b075f87060 100644 --- a/src/drivers/px4flow/px4flow.cpp +++ b/src/drivers/px4flow/px4flow.cpp @@ -669,6 +669,8 @@ start() } if (OK != g_dev->init()) { + + #ifdef PX4_I2C_BUS_ESC delete g_dev; /* try 2nd bus */ g_dev = new PX4FLOW(PX4_I2C_BUS_ESC); @@ -678,6 +680,8 @@ start() } if (OK != g_dev->init()) { + #endif + delete g_dev; /* try 3rd bus */ g_dev = new PX4FLOW(PX4_I2C_BUS_ONBOARD); @@ -689,7 +693,10 @@ start() if (OK != g_dev->init()) { goto fail; } + + #ifdef PX4_I2C_BUS_ESC } + #endif } /* set the poll rate to default, starts automatic data collection */ From 739c407cfd14d065b104977924875b3ee40d5e25 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 31 Oct 2014 11:42:46 +0100 Subject: [PATCH 022/216] Fix flow example --- .../flow_position_estimator/flow_position_estimator_main.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/examples/flow_position_estimator/flow_position_estimator_main.c b/src/examples/flow_position_estimator/flow_position_estimator_main.c index c775428ef1..0b8c01f798 100644 --- a/src/examples/flow_position_estimator/flow_position_estimator_main.c +++ b/src/examples/flow_position_estimator/flow_position_estimator_main.c @@ -308,8 +308,8 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) if (vehicle_liftoff || params.debug) { /* copy flow */ - flow_speed[0] = flow.flow_comp_x_m; - flow_speed[1] = flow.flow_comp_y_m; + flow_speed[0] = flow.pixel_flow_x_integral / (flow.integration_timespan / 1e6f) * flow.ground_distance_m; + flow_speed[1] = flow.pixel_flow_y_integral / (flow.integration_timespan / 1e6f) * flow.ground_distance_m; flow_speed[2] = 0.0f; /* convert to bodyframe velocity */ From 5c2ddd258bf3ba7877c535289a1a6f0d72bcd9f2 Mon Sep 17 00:00:00 2001 From: tumbili Date: Tue, 4 Nov 2014 22:47:56 +0100 Subject: [PATCH 023/216] merged with NuttX subproject --- NuttX | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/NuttX b/NuttX index 5ee4b2b2c2..41fffa0df1 160000 --- a/NuttX +++ b/NuttX @@ -1 +1 @@ -Subproject commit 5ee4b2b2c26bbc35d1669840f0676e8aa383b984 +Subproject commit 41fffa0df192e4a26d2325c7ac5d9b5d7ba0211c From b06f7f4f2e47dbaf33b255c9ecdffaa51f35f4a4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 17 Nov 2014 14:56:11 +0100 Subject: [PATCH 024/216] Reduce ROMFS footprint --- ROMFS/px4fmu_common/init.d/rc.interface | 21 +++++++++++---------- 1 file changed, 11 insertions(+), 10 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.interface b/ROMFS/px4fmu_common/init.d/rc.interface index e44cd0953d..10c6d75118 100644 --- a/ROMFS/px4fmu_common/init.d/rc.interface +++ b/ROMFS/px4fmu_common/init.d/rc.interface @@ -8,12 +8,11 @@ then # # Load mixer # - set MIXERSD /fs/microsd/etc/mixers/$MIXER.mix #Use the mixer file from the SD-card if it exists - if [ -f $MIXERSD ] + if [ -f /fs/microsd/etc/mixers/$MIXER.mix ] then - set MIXER_FILE $MIXERSD + set MIXER_FILE /fs/microsd/etc/mixers/$MIXER.mix else set MIXER_FILE /etc/mixers/$MIXER.mix fi @@ -35,19 +34,21 @@ then echo "[init] Mixer loaded: $MIXER_FILE" else echo "[init] Error loading mixer: $MIXER_FILE" - tone_alarm $TUNE_OUT_ERROR + tone_alarm $TUNE_ERR fi + + unset MIXER_FILE else if [ $MIXER != skip ] then echo "[init] Mixer not defined" - tone_alarm $TUNE_OUT_ERROR + tone_alarm $TUNE_ERR fi fi if [ $OUTPUT_MODE == fmu -o $OUTPUT_MODE == io ] then - if [ $PWM_OUTPUTS != none ] + if [ $PWM_OUT != none ] then # # Set PWM output frequency @@ -55,7 +56,7 @@ then if [ $PWM_RATE != none ] then echo "[init] Set PWM rate: $PWM_RATE" - pwm rate -c $PWM_OUTPUTS -r $PWM_RATE + pwm rate -c $PWM_OUT -r $PWM_RATE fi # @@ -64,17 +65,17 @@ then if [ $PWM_DISARMED != none ] then echo "[init] Set PWM disarmed: $PWM_DISARMED" - pwm disarmed -c $PWM_OUTPUTS -p $PWM_DISARMED + pwm disarmed -c $PWM_OUT -p $PWM_DISARMED fi if [ $PWM_MIN != none ] then echo "[init] Set PWM min: $PWM_MIN" - pwm min -c $PWM_OUTPUTS -p $PWM_MIN + pwm min -c $PWM_OUT -p $PWM_MIN fi if [ $PWM_MAX != none ] then echo "[init] Set PWM max: $PWM_MAX" - pwm max -c $PWM_OUTPUTS -p $PWM_MAX + pwm max -c $PWM_OUT -p $PWM_MAX fi fi From 489b4c4839a3796c520629850bc703c42bef636c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 17 Nov 2014 14:58:29 +0100 Subject: [PATCH 025/216] Reduce too chatty content, de-allocate non-needed string buffers --- ROMFS/px4fmu_common/init.d/rc.interface | 14 +-- ROMFS/px4fmu_common/init.d/rcS | 150 ++++++++++++------------ 2 files changed, 82 insertions(+), 82 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.interface b/ROMFS/px4fmu_common/init.d/rc.interface index 10c6d75118..0f703c3b38 100644 --- a/ROMFS/px4fmu_common/init.d/rc.interface +++ b/ROMFS/px4fmu_common/init.d/rc.interface @@ -31,9 +31,9 @@ then if mixer load $OUTPUT_DEV $MIXER_FILE then - echo "[init] Mixer loaded: $MIXER_FILE" + echo "[i] Mixer: $MIXER_FILE" else - echo "[init] Error loading mixer: $MIXER_FILE" + echo "[i] Error loading mixer: $MIXER_FILE" tone_alarm $TUNE_ERR fi @@ -41,7 +41,7 @@ then else if [ $MIXER != skip ] then - echo "[init] Mixer not defined" + echo "[i] Mixer not defined" tone_alarm $TUNE_ERR fi fi @@ -55,7 +55,7 @@ then # if [ $PWM_RATE != none ] then - echo "[init] Set PWM rate: $PWM_RATE" + echo "[i] PWM rate: $PWM_RATE" pwm rate -c $PWM_OUT -r $PWM_RATE fi @@ -64,17 +64,17 @@ then # if [ $PWM_DISARMED != none ] then - echo "[init] Set PWM disarmed: $PWM_DISARMED" + echo "[i] PWM disarmed: $PWM_DISARMED" pwm disarmed -c $PWM_OUT -p $PWM_DISARMED fi if [ $PWM_MIN != none ] then - echo "[init] Set PWM min: $PWM_MIN" + echo "[i] PWM min: $PWM_MIN" pwm min -c $PWM_OUT -p $PWM_MIN fi if [ $PWM_MAX != none ] then - echo "[init] Set PWM max: $PWM_MAX" + echo "[i] PWM max: $PWM_MAX" pwm max -c $PWM_OUT -p $PWM_MAX fi fi diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index f9c8226350..7532d38115 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -7,25 +7,25 @@ # set MODE autostart -set RC_FILE /fs/microsd/etc/rc.txt -set CONFIG_FILE /fs/microsd/etc/config.txt -set EXTRAS_FILE /fs/microsd/etc/extras.txt +set FRC /fs/microsd/etc/rc.txt +set FCONFIG /fs/microsd/etc/config.txt +set FEXTRAS /fs/microsd/etc/extras.txt -set TUNE_OUT_ERROR ML<> $LOG_FILE - tone_alarm $TUNE_OUT_ERROR + tone_alarm $TUNE_ERR fi else echo "PX4IO update failed" >> $LOG_FILE - tone_alarm $TUNE_OUT_ERROR + tone_alarm $TUNE_ERR fi fi if [ $IO_PRESENT == no ] then - echo "[init] ERROR: PX4IO not found" - tone_alarm $TUNE_OUT_ERROR + echo "[i] ERROR: PX4IO not found" + tone_alarm $TUNE_ERR fi fi @@ -251,7 +252,7 @@ then then # Need IO for output but it not present, disable output set OUTPUT_MODE none - echo "[init] ERROR: PX4IO not found, disabling output" + echo "[i] ERROR: PX4IO not found, disabling output" # Avoid using ttyS0 for MAVLink on FMUv1 if ver hwcmp PX4FMU_V1 @@ -294,33 +295,31 @@ then then if param compare UAVCAN_ENABLE 0 then - echo "[init] OVERRIDING UAVCAN_ENABLE = 1" + echo "[i] OVERRIDING UAVCAN_ENABLE = 1" param set UAVCAN_ENABLE 1 fi fi if [ $OUTPUT_MODE == io -o $OUTPUT_MODE == uavcan_esc ] then - echo "[init] Use PX4IO PWM as primary output" if px4io start then - echo "[init] PX4IO started" + echo "[i] PX4IO started" sh /etc/init.d/rc.io else - echo "[init] ERROR: PX4IO start failed" - tone_alarm $TUNE_OUT_ERROR + echo "[i] ERROR: PX4IO start failed" + tone_alarm $TUNE_ERR fi fi if [ $OUTPUT_MODE == fmu -o $OUTPUT_MODE == ardrone ] then - echo "[init] Use FMU as primary output" if fmu mode_$FMU_MODE then - echo "[init] FMU mode_$FMU_MODE started" + echo "[i] FMU mode_$FMU_MODE started" else - echo "[init] ERROR: FMU mode_$FMU_MODE start failed" - tone_alarm $TUNE_OUT_ERROR + echo "[i] ERROR: FMU mode_$FMU_MODE start failed" + tone_alarm $TUNE_ERR fi if ver hwcmp PX4FMU_V1 @@ -338,36 +337,34 @@ then if [ $OUTPUT_MODE == mkblctrl ] then - echo "[init] Use MKBLCTRL as primary output" set MKBLCTRL_ARG "" - if [ $MKBLCTRL_MODE == x ] + if [ $MK_MODE == x ] then set MKBLCTRL_ARG "-mkmode x" fi - if [ $MKBLCTRL_MODE == + ] + if [ $MK_MODE == + ] then set MKBLCTRL_ARG "-mkmode +" fi if mkblctrl $MKBLCTRL_ARG then - echo "[init] MKBLCTRL started" + echo "[i] MK started" else - echo "[init] ERROR: MKBLCTRL start failed" - tone_alarm $TUNE_OUT_ERROR + echo "[i] ERROR: MK start failed" + tone_alarm $TUNE_ERR fi fi if [ $OUTPUT_MODE == hil ] then - echo "[init] Use HIL as primary output" if hil mode_port2_pwm8 then - echo "[init] HIL output started" + echo "[i] HIL output started" else - echo "[init] ERROR: HIL output start failed" - tone_alarm $TUNE_OUT_ERROR + echo "[i] ERROR: HIL output start failed" + tone_alarm $TUNE_ERR fi fi @@ -380,11 +377,11 @@ then then if px4io start then - echo "[init] PX4IO started" + echo "[i] PX4IO started" sh /etc/init.d/rc.io else - echo "[init] ERROR: PX4IO start failed" - tone_alarm $TUNE_OUT_ERROR + echo "[i] ERROR: PX4IO start failed" + tone_alarm $TUNE_ERR fi fi else @@ -392,10 +389,10 @@ then then if fmu mode_$FMU_MODE then - echo "[init] FMU mode_$FMU_MODE started" + echo "[i] FMU mode_$FMU_MODE started" else - echo "[init] ERROR: FMU mode_$FMU_MODE start failed" - tone_alarm $TUNE_OUT_ERROR + echo "[i] ERROR: FMU mode_$FMU_MODE start failed" + tone_alarm $TUNE_ERR fi if ver hwcmp PX4FMU_V1 @@ -413,23 +410,24 @@ then fi fi - if [ $MAVLINK_FLAGS == default ] + if [ $MAVLINK_F == default ] then # Normal mode, use baudrate 57600 (default) and data rate 1000 bytes/s if [ $TTYS1_BUSY == yes ] then # Start MAVLink on ttyS0, because FMU ttyS1 pins configured as something else - set MAVLINK_FLAGS "-r 1000 -d /dev/ttyS0" + set MAVLINK_F "-r 1000 -d /dev/ttyS0" # Exit from nsh to free port for mavlink set EXIT_ON_END yes else # Start MAVLink on default port: ttyS1 - set MAVLINK_FLAGS "-r 1000" + set MAVLINK_F "-r 1000" fi fi - mavlink start $MAVLINK_FLAGS + mavlink start $MAVLINK_F + unset MAVLINK_F # # UAVCAN @@ -444,15 +442,16 @@ then if [ $GPS == yes ] then - echo "[init] Start GPS" + echo "[i] Start GPS" if [ $GPS_FAKE == yes ] then - echo "[init] Faking GPS" + echo "[i] Faking GPS" gps start -f else gps start fi fi + unset GPS_FAKE # # Start up ARDrone Motor interface @@ -467,7 +466,7 @@ then # if [ $VEHICLE_TYPE == fw ] then - echo "[init] Vehicle type: FIXED WING" + echo "[i] FIXED WING" if [ $MIXER == none ] then @@ -481,13 +480,13 @@ then set MAV_TYPE 1 fi - param set MAV_TYPE $MAV_TYPE + #param set MAV_TYPE $MAV_TYPE # Load mixer and configure outputs sh /etc/init.d/rc.interface # Start standard fixedwing apps - if [ $LOAD_DEFAULT_APPS == yes ] + if [ $LOAD_DAPPS == yes ] then sh /etc/init.d/rc.fw_apps fi @@ -498,11 +497,11 @@ then # if [ $VEHICLE_TYPE == mc ] then - echo "[init] Vehicle type: MULTICOPTER" + echo "[i] MULTICOPTER" if [ $MIXER == none ] then - echo "Default mixer for multicopter not defined" + echo "Mixer undefined" fi if [ $MAV_TYPE == none ] @@ -546,7 +545,7 @@ then sh /etc/init.d/rc.interface # Start standard multicopter apps - if [ $LOAD_DEFAULT_APPS == yes ] + if [ $LOAD_DAPPS == yes ] then sh /etc/init.d/rc.mc_apps fi @@ -562,22 +561,23 @@ then # if [ $VEHICLE_TYPE == none ] then - echo "[init] Vehicle type: No autostart ID found" + echo "[i] No autostart ID found" fi # Start any custom addons - if [ -f $EXTRAS_FILE ] + if [ -f $FEXTRAS ] then - echo "[init] Starting addons script: $EXTRAS_FILE" - sh $EXTRAS_FILE + echo "[i] Addons script: $FEXTRAS" + sh $FEXTRAS else - echo "[init] No addons script: $EXTRAS_FILE" + echo "[i] No addons script: $FEXTRAS" fi + unset FEXTRAS if [ $EXIT_ON_END == yes ] then - echo "[init] Exit from nsh" + echo "Exit from nsh" exit fi From b808cc9a1b498aedfaccf35777c440e7ea286ac4 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 17 Nov 2014 16:38:20 +0100 Subject: [PATCH 026/216] fix variable name in rc.uavcan --- ROMFS/px4fmu_common/init.d/rc.uavcan | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.uavcan b/ROMFS/px4fmu_common/init.d/rc.uavcan index 9a470a6b8d..55a3726096 100644 --- a/ROMFS/px4fmu_common/init.d/rc.uavcan +++ b/ROMFS/px4fmu_common/init.d/rc.uavcan @@ -13,6 +13,6 @@ then echo "[init] UAVCAN started" else echo "[init] ERROR: Could not start UAVCAN" - tone_alarm $TUNE_OUT_ERROR + tone_alarm $TUNE_ERR fi fi From 32f88bbe847bfeb7d040345df6fb534d223fb2bb Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 17 Nov 2014 17:02:57 +0100 Subject: [PATCH 027/216] rename PWM_OUTPUTS to PWM_OUT in all files --- ROMFS/px4fmu_common/init.d/10015_tbs_discovery | 2 +- ROMFS/px4fmu_common/init.d/10016_3dr_iris | 2 +- ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d | 2 +- ROMFS/px4fmu_common/init.d/11001_hexa_cox | 2 +- ROMFS/px4fmu_common/init.d/12001_octo_cox | 2 +- ROMFS/px4fmu_common/init.d/3030_io_camflyer | 2 +- ROMFS/px4fmu_common/init.d/3031_phantom | 2 +- ROMFS/px4fmu_common/init.d/3033_wingwing | 2 +- ROMFS/px4fmu_common/init.d/4001_quad_x | 2 +- ROMFS/px4fmu_common/init.d/5001_quad_+ | 2 +- ROMFS/px4fmu_common/init.d/6001_hexa_x | 2 +- ROMFS/px4fmu_common/init.d/7001_hexa_+ | 2 +- ROMFS/px4fmu_common/init.d/8001_octo_x | 2 +- ROMFS/px4fmu_common/init.d/9001_octo_+ | 2 +- 14 files changed, 14 insertions(+), 14 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery index c8379e3a1a..7801bbed9f 100644 --- a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery +++ b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery @@ -26,5 +26,5 @@ fi set MIXER FMU_quad_w -set PWM_OUTPUTS 1234 +set PWM_OUT 1234 set PWM_MIN 1200 diff --git a/ROMFS/px4fmu_common/init.d/10016_3dr_iris b/ROMFS/px4fmu_common/init.d/10016_3dr_iris index 0b422de7e2..dc11192f13 100644 --- a/ROMFS/px4fmu_common/init.d/10016_3dr_iris +++ b/ROMFS/px4fmu_common/init.d/10016_3dr_iris @@ -29,7 +29,7 @@ fi set MIXER FMU_quad_w -set PWM_OUTPUTS 1234 +set PWM_OUT 1234 set PWM_MIN 1200 set PWM_MAX 1950 diff --git a/ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d b/ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d index a621de7ce8..d79a527351 100644 --- a/ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d +++ b/ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d @@ -31,4 +31,4 @@ set MIXER FMU_quad_w set PWM_MIN 1210 set PWM_MAX 2100 -set PWM_OUTPUTS 1234 +set PWM_OUT 1234 diff --git a/ROMFS/px4fmu_common/init.d/11001_hexa_cox b/ROMFS/px4fmu_common/init.d/11001_hexa_cox index e4d96fbd56..50f717e3df 100644 --- a/ROMFS/px4fmu_common/init.d/11001_hexa_cox +++ b/ROMFS/px4fmu_common/init.d/11001_hexa_cox @@ -10,4 +10,4 @@ sh /etc/init.d/rc.mc_defaults set MIXER FMU_hexa_cox # Need to set all 8 channels -set PWM_OUTPUTS 12345678 +set PWM_OUT 12345678 diff --git a/ROMFS/px4fmu_common/init.d/12001_octo_cox b/ROMFS/px4fmu_common/init.d/12001_octo_cox index f820251ad3..e0a8381851 100644 --- a/ROMFS/px4fmu_common/init.d/12001_octo_cox +++ b/ROMFS/px4fmu_common/init.d/12001_octo_cox @@ -9,4 +9,4 @@ sh /etc/init.d/rc.mc_defaults set MIXER FMU_octo_cox -set PWM_OUTPUTS 12345678 +set PWM_OUT 12345678 diff --git a/ROMFS/px4fmu_common/init.d/3030_io_camflyer b/ROMFS/px4fmu_common/init.d/3030_io_camflyer index 0f0cb3a89f..fe0269557f 100644 --- a/ROMFS/px4fmu_common/init.d/3030_io_camflyer +++ b/ROMFS/px4fmu_common/init.d/3030_io_camflyer @@ -4,5 +4,5 @@ sh /etc/init.d/rc.fw_defaults set MIXER FMU_Q # Provide ESC a constant 1000 us pulse while disarmed -set PWM_OUTPUTS 4 +set PWM_OUT 4 set PWM_DISARMED 1000 diff --git a/ROMFS/px4fmu_common/init.d/3031_phantom b/ROMFS/px4fmu_common/init.d/3031_phantom index a7749cba6e..d2b2f6c85c 100644 --- a/ROMFS/px4fmu_common/init.d/3031_phantom +++ b/ROMFS/px4fmu_common/init.d/3031_phantom @@ -36,5 +36,5 @@ fi set MIXER phantom # Provide ESC a constant 1000 us pulse -set PWM_OUTPUTS 4 +set PWM_OUT 4 set PWM_DISARMED 1000 diff --git a/ROMFS/px4fmu_common/init.d/3033_wingwing b/ROMFS/px4fmu_common/init.d/3033_wingwing index 919eefb4a8..64c3fb5fb9 100644 --- a/ROMFS/px4fmu_common/init.d/3033_wingwing +++ b/ROMFS/px4fmu_common/init.d/3033_wingwing @@ -44,5 +44,5 @@ fi set MIXER wingwing # Provide ESC a constant 1000 us pulse -set PWM_OUTPUTS 4 +set PWM_OUT 4 set PWM_DISARMED 1000 diff --git a/ROMFS/px4fmu_common/init.d/4001_quad_x b/ROMFS/px4fmu_common/init.d/4001_quad_x index 8fe8961c5c..4677f9fc3a 100644 --- a/ROMFS/px4fmu_common/init.d/4001_quad_x +++ b/ROMFS/px4fmu_common/init.d/4001_quad_x @@ -9,4 +9,4 @@ sh /etc/init.d/rc.mc_defaults set MIXER FMU_quad_x -set PWM_OUTPUTS 1234 +set PWM_OUT 1234 diff --git a/ROMFS/px4fmu_common/init.d/5001_quad_+ b/ROMFS/px4fmu_common/init.d/5001_quad_+ index 5512aa738a..c789113919 100644 --- a/ROMFS/px4fmu_common/init.d/5001_quad_+ +++ b/ROMFS/px4fmu_common/init.d/5001_quad_+ @@ -9,4 +9,4 @@ sh /etc/init.d/rc.mc_defaults set MIXER FMU_quad_+ -set PWM_OUTPUTS 1234 +set PWM_OUT 1234 diff --git a/ROMFS/px4fmu_common/init.d/6001_hexa_x b/ROMFS/px4fmu_common/init.d/6001_hexa_x index 1043ad8adb..0df25b11a1 100644 --- a/ROMFS/px4fmu_common/init.d/6001_hexa_x +++ b/ROMFS/px4fmu_common/init.d/6001_hexa_x @@ -10,4 +10,4 @@ sh /etc/init.d/rc.mc_defaults set MIXER FMU_hexa_x # Need to set all 8 channels -set PWM_OUTPUTS 12345678 +set PWM_OUT 12345678 diff --git a/ROMFS/px4fmu_common/init.d/7001_hexa_+ b/ROMFS/px4fmu_common/init.d/7001_hexa_+ index 84ab88883c..16c772ee1e 100644 --- a/ROMFS/px4fmu_common/init.d/7001_hexa_+ +++ b/ROMFS/px4fmu_common/init.d/7001_hexa_+ @@ -10,4 +10,4 @@ sh /etc/init.d/rc.mc_defaults set MIXER FMU_hexa_+ # Need to set all 8 channels -set PWM_OUTPUTS 12345678 +set PWM_OUT 12345678 diff --git a/ROMFS/px4fmu_common/init.d/8001_octo_x b/ROMFS/px4fmu_common/init.d/8001_octo_x index 74e304cd96..bae36737f9 100644 --- a/ROMFS/px4fmu_common/init.d/8001_octo_x +++ b/ROMFS/px4fmu_common/init.d/8001_octo_x @@ -9,4 +9,4 @@ sh /etc/init.d/rc.mc_defaults set MIXER FMU_octo_x -set PWM_OUTPUTS 12345678 +set PWM_OUT 12345678 diff --git a/ROMFS/px4fmu_common/init.d/9001_octo_+ b/ROMFS/px4fmu_common/init.d/9001_octo_+ index f7c06c6c80..ca5439f68f 100644 --- a/ROMFS/px4fmu_common/init.d/9001_octo_+ +++ b/ROMFS/px4fmu_common/init.d/9001_octo_+ @@ -9,4 +9,4 @@ sh /etc/init.d/rc.mc_defaults set MIXER FMU_octo_+ -set PWM_OUTPUTS 12345678 \ No newline at end of file +set PWM_OUT 12345678 From 1b47f05b1409f5006f288458b5b176a855174e7f Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 17 Nov 2014 17:10:47 +0100 Subject: [PATCH 028/216] rename DO_AUTOCONFIG to AUTOCNF in all files --- ROMFS/px4fmu_common/init.d/10015_tbs_discovery | 2 +- ROMFS/px4fmu_common/init.d/10016_3dr_iris | 2 +- ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d | 2 +- ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil | 2 +- ROMFS/px4fmu_common/init.d/3031_phantom | 2 +- ROMFS/px4fmu_common/init.d/3032_skywalker_x5 | 2 +- ROMFS/px4fmu_common/init.d/3033_wingwing | 2 +- ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha | 4 ++-- ROMFS/px4fmu_common/init.d/4008_ardrone | 4 ++-- ROMFS/px4fmu_common/init.d/4010_dji_f330 | 2 +- ROMFS/px4fmu_common/init.d/4011_dji_f450 | 2 +- ROMFS/px4fmu_common/init.d/4012_quad_x_can | 2 +- ROMFS/px4fmu_common/init.d/4020_hk_micro_pcb | 2 +- ROMFS/px4fmu_common/init.d/rc.fw_defaults | 4 ++-- ROMFS/px4fmu_common/init.d/rc.mc_defaults | 2 +- 15 files changed, 18 insertions(+), 18 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery index 7801bbed9f..c1b366de83 100644 --- a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery +++ b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery @@ -7,7 +7,7 @@ sh /etc/init.d/rc.mc_defaults -if [ $DO_AUTOCONFIG == yes ] +if [ $AUTOCNF == yes ] then # TODO review MC_YAWRATE_I param set MC_ROLL_P 8.0 diff --git a/ROMFS/px4fmu_common/init.d/10016_3dr_iris b/ROMFS/px4fmu_common/init.d/10016_3dr_iris index dc11192f13..3879737a8e 100644 --- a/ROMFS/px4fmu_common/init.d/10016_3dr_iris +++ b/ROMFS/px4fmu_common/init.d/10016_3dr_iris @@ -7,7 +7,7 @@ sh /etc/init.d/rc.mc_defaults -if [ $DO_AUTOCONFIG == yes ] +if [ $AUTOCNF == yes ] then # TODO tune roll/pitch separately param set MC_ROLL_P 7.0 diff --git a/ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d b/ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d index d79a527351..57f77754c8 100644 --- a/ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d +++ b/ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d @@ -7,7 +7,7 @@ sh /etc/init.d/rc.mc_defaults -if [ $DO_AUTOCONFIG == yes ] +if [ $AUTOCNF == yes ] then # TODO tune roll/pitch separately param set MC_ROLL_P 7.0 diff --git a/ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil b/ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil index 00b97d675c..f208b692a0 100644 --- a/ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil +++ b/ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil @@ -7,7 +7,7 @@ sh /etc/init.d/rc.fw_defaults -if [ $DO_AUTOCONFIG == yes ] +if [ $AUTOCNF == yes ] then param set FW_AIRSPD_MIN 12 param set FW_AIRSPD_TRIM 25 diff --git a/ROMFS/px4fmu_common/init.d/3031_phantom b/ROMFS/px4fmu_common/init.d/3031_phantom index d2b2f6c85c..c7dd1dfc5c 100644 --- a/ROMFS/px4fmu_common/init.d/3031_phantom +++ b/ROMFS/px4fmu_common/init.d/3031_phantom @@ -7,7 +7,7 @@ sh /etc/init.d/rc.fw_defaults -if [ $DO_AUTOCONFIG == yes ] +if [ $AUTOCNF == yes ] then param set FW_AIRSPD_MIN 13 param set FW_AIRSPD_TRIM 15 diff --git a/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 b/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 index 26c7c95e67..94363bf6ab 100644 --- a/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 +++ b/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 @@ -7,7 +7,7 @@ sh /etc/init.d/rc.fw_defaults -if [ $DO_AUTOCONFIG == yes ] +if [ $AUTOCNF == yes ] then param set FW_AIRSPD_MIN 15 param set FW_AIRSPD_TRIM 20 diff --git a/ROMFS/px4fmu_common/init.d/3033_wingwing b/ROMFS/px4fmu_common/init.d/3033_wingwing index 64c3fb5fb9..add905b115 100644 --- a/ROMFS/px4fmu_common/init.d/3033_wingwing +++ b/ROMFS/px4fmu_common/init.d/3033_wingwing @@ -7,7 +7,7 @@ sh /etc/init.d/rc.fw_defaults -if [ $DO_AUTOCONFIG == yes ] +if [ $AUTOCNF == yes ] then param set BAT_N_CELLS 2 param set FW_AIRSPD_MAX 15 diff --git a/ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha b/ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha index 9bfd9d9ed5..9eafac1c59 100644 --- a/ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha +++ b/ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha @@ -7,9 +7,9 @@ sh /etc/init.d/rc.fw_defaults -if [ $DO_AUTOCONFIG == yes ] +if [ $AUTOCNF == yes ] then - + # TODO: these are the X5 default parameters, update them to the caipi param set FW_AIRSPD_MIN 15 diff --git a/ROMFS/px4fmu_common/init.d/4008_ardrone b/ROMFS/px4fmu_common/init.d/4008_ardrone index 0488e3928e..c3358ef4c8 100644 --- a/ROMFS/px4fmu_common/init.d/4008_ardrone +++ b/ROMFS/px4fmu_common/init.d/4008_ardrone @@ -8,7 +8,7 @@ sh /etc/init.d/rc.mc_defaults # # Load default params for this platform # -if [ $DO_AUTOCONFIG == yes ] +if [ $AUTOCNF == yes ] then # Set all params here, then disable autoconfig param set MC_ROLL_P 6.0 @@ -24,7 +24,7 @@ then param set MC_YAWRATE_I 0.2 param set MC_YAWRATE_D 0.0 param set MC_YAW_FF 0.8 - + param set BAT_V_SCALING 0.00838095238 fi diff --git a/ROMFS/px4fmu_common/init.d/4010_dji_f330 b/ROMFS/px4fmu_common/init.d/4010_dji_f330 index f0cc052075..8e5dc76b1f 100644 --- a/ROMFS/px4fmu_common/init.d/4010_dji_f330 +++ b/ROMFS/px4fmu_common/init.d/4010_dji_f330 @@ -7,7 +7,7 @@ sh /etc/init.d/4001_quad_x -if [ $DO_AUTOCONFIG == yes ] +if [ $AUTOCNF == yes ] then param set MC_ROLL_P 7.0 param set MC_ROLLRATE_P 0.1 diff --git a/ROMFS/px4fmu_common/init.d/4011_dji_f450 b/ROMFS/px4fmu_common/init.d/4011_dji_f450 index 1ca716a6b3..ea35b3ba9b 100644 --- a/ROMFS/px4fmu_common/init.d/4011_dji_f450 +++ b/ROMFS/px4fmu_common/init.d/4011_dji_f450 @@ -7,7 +7,7 @@ sh /etc/init.d/4001_quad_x -if [ $DO_AUTOCONFIG == yes ] +if [ $AUTOCNF == yes ] then # TODO REVIEW param set MC_ROLL_P 7.0 diff --git a/ROMFS/px4fmu_common/init.d/4012_quad_x_can b/ROMFS/px4fmu_common/init.d/4012_quad_x_can index 5c4a6497a0..b1db1dd9aa 100644 --- a/ROMFS/px4fmu_common/init.d/4012_quad_x_can +++ b/ROMFS/px4fmu_common/init.d/4012_quad_x_can @@ -7,7 +7,7 @@ sh /etc/init.d/4001_quad_x -if [ $DO_AUTOCONFIG == yes ] +if [ $AUTOCNF == yes ] then # TODO REVIEW param set MC_ROLL_P 7.0 diff --git a/ROMFS/px4fmu_common/init.d/4020_hk_micro_pcb b/ROMFS/px4fmu_common/init.d/4020_hk_micro_pcb index 9fe310ddee..a1de19d5df 100644 --- a/ROMFS/px4fmu_common/init.d/4020_hk_micro_pcb +++ b/ROMFS/px4fmu_common/init.d/4020_hk_micro_pcb @@ -9,7 +9,7 @@ echo "HK Micro PCB Quad" sh /etc/init.d/4001_quad_x -if [ $DO_AUTOCONFIG == yes ] +if [ $AUTOCNF == yes ] then param set MC_ROLL_P 7.0 param set MC_ROLLRATE_P 0.1 diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_defaults b/ROMFS/px4fmu_common/init.d/rc.fw_defaults index 3c336f295f..fab2a7f187 100644 --- a/ROMFS/px4fmu_common/init.d/rc.fw_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.fw_defaults @@ -2,7 +2,7 @@ set VEHICLE_TYPE fw -if [ $DO_AUTOCONFIG == yes ] +if [ $AUTOCNF == yes ] then # # Default parameters for FW @@ -15,4 +15,4 @@ then param set FW_T_RLL2THR 15 param set FW_T_SRATE_P 0.01 param set FW_T_TIME_CONST 5 -fi \ No newline at end of file +fi diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_defaults b/ROMFS/px4fmu_common/init.d/rc.mc_defaults index 0df320f49a..307a64c4d5 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.mc_defaults @@ -2,7 +2,7 @@ set VEHICLE_TYPE mc -if [ $DO_AUTOCONFIG == yes ] +if [ $AUTOCNF == yes ] then param set MC_ROLL_P 7.0 param set MC_ROLLRATE_P 0.1 From 30d851284606476642c6cf8a54070bc08ab923c6 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 19 Nov 2014 12:31:04 +0100 Subject: [PATCH 029/216] MC pos control offb: read altitude sp separately --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index d52138522c..f2c650ddd3 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -652,8 +652,6 @@ MulticopterPositionControl::control_offboard(float dt) /* control position */ _pos_sp(0) = _pos_sp_triplet.current.x; _pos_sp(1) = _pos_sp_triplet.current.y; - _pos_sp(2) = _pos_sp_triplet.current.z; - } else if (_control_mode.flag_control_velocity_enabled && _pos_sp_triplet.current.velocity_valid) { /* control velocity */ /* reset position setpoint to current position if needed */ @@ -670,7 +668,10 @@ MulticopterPositionControl::control_offboard(float dt) _att_sp.yaw_body = _att_sp.yaw_body + _pos_sp_triplet.current.yawspeed * dt; } - if (_control_mode.flag_control_altitude_enabled) { + if (_control_mode.flag_control_altitude_enabled && _pos_sp_triplet.current.position_valid) { + /* Control altitude */ + _pos_sp(2) = _pos_sp_triplet.current.z; + } else if (_control_mode.flag_control_climb_rate_enabled && _pos_sp_triplet.current.velocity_valid) { /* reset alt setpoint to current altitude if needed */ reset_alt_sp(); From 5c34f03c4ef1d0b928c8ad32f8038bb15dba7c11 Mon Sep 17 00:00:00 2001 From: philipoe Date: Thu, 20 Nov 2014 17:25:30 +0100 Subject: [PATCH 030/216] commander: Change printing in RC-loss message to integers --- src/modules/commander/commander.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index d8ebee4b6f..1f13aaec4a 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1499,7 +1499,7 @@ int commander_thread_main(int argc, char *argv[]) } else { if (status.rc_signal_lost) { - mavlink_log_critical(mavlink_fd, "RC SIGNAL REGAINED after %.3fs (at t=%.3fs)",(double)(hrt_absolute_time()-status.rc_signal_lost_timestamp)/1.0e6,(double)(hrt_absolute_time())/1.0e6); + mavlink_log_critical(mavlink_fd, "RC SIGNAL REGAINED after %llums (at t=%llums)",(hrt_absolute_time()-status.rc_signal_lost_timestamp)/1000,hrt_absolute_time()/1000); status_changed = true; } } @@ -1592,7 +1592,7 @@ int commander_thread_main(int argc, char *argv[]) } else { if (!status.rc_signal_lost) { - mavlink_log_critical(mavlink_fd, "RC SIGNAL LOST (at t=%.3fs)",(double)(hrt_absolute_time())/1.0e6); + mavlink_log_critical(mavlink_fd, "RC SIGNAL LOST (at t=%llums)",hrt_absolute_time()/1000); status.rc_signal_lost = true; status.rc_signal_lost_timestamp=sp_man.timestamp; status_changed = true; From 9bb0ecf0ca6082355072af190018e0b5298b7e59 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 22 Nov 2014 15:06:32 +0100 Subject: [PATCH 031/216] Airspeed calibration feedback: Improve wording --- src/modules/commander/state_machine_helper.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index e37019d02d..465f9cdc52 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -706,7 +706,7 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) } if (fabsf(airspeed.indicated_airspeed_m_s > 6.0f)) { - mavlink_log_critical(mavlink_fd, "AIRSPEED WARNING: WIND OR CALIBRATION MISSING"); + mavlink_log_critical(mavlink_fd, "AIRSPEED WARNING: WIND OR CALIBRATION ISSUE"); // XXX do not make this fatal yet } } From 6da9063560b900c0ce13bb0e078889eeaf8c71c0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 22 Nov 2014 15:08:54 +0100 Subject: [PATCH 032/216] Fix FD for commander arm operation --- src/modules/commander/commander.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index ec173c12be..e081955d07 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -310,12 +310,16 @@ int commander_main(int argc, char *argv[]) } if (!strcmp(argv[1], "arm")) { - arm_disarm(true, mavlink_fd, "command line"); + int mavlink_fd_local = open(MAVLINK_LOG_DEVICE, 0); + arm_disarm(true, mavlink_fd_local, "command line"); + close(mavlink_fd_local); exit(0); } if (!strcmp(argv[1], "disarm")) { - arm_disarm(false, mavlink_fd, "command line"); + int mavlink_fd_local = open(MAVLINK_LOG_DEVICE, 0); + arm_disarm(false, mavlink_fd_local, "command line"); + close(mavlink_fd_local); exit(0); } From 366c8a9c41e22ff31a8e027c5f931b99b7569a7d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 22 Nov 2014 15:43:21 +0100 Subject: [PATCH 033/216] ROMFS: Do only output necessary information on boot --- ROMFS/px4fmu_common/init.d/rc.interface | 6 +----- ROMFS/px4fmu_common/init.d/rcS | 1 - 2 files changed, 1 insertion(+), 6 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.interface b/ROMFS/px4fmu_common/init.d/rc.interface index e44cd0953d..41e0b149b4 100644 --- a/ROMFS/px4fmu_common/init.d/rc.interface +++ b/ROMFS/px4fmu_common/init.d/rc.interface @@ -10,7 +10,7 @@ then # set MIXERSD /fs/microsd/etc/mixers/$MIXER.mix - #Use the mixer file from the SD-card if it exists + # Use the mixer file from the SD-card if it exists if [ -f $MIXERSD ] then set MIXER_FILE $MIXERSD @@ -54,7 +54,6 @@ then # if [ $PWM_RATE != none ] then - echo "[init] Set PWM rate: $PWM_RATE" pwm rate -c $PWM_OUTPUTS -r $PWM_RATE fi @@ -63,17 +62,14 @@ then # if [ $PWM_DISARMED != none ] then - echo "[init] Set PWM disarmed: $PWM_DISARMED" pwm disarmed -c $PWM_OUTPUTS -p $PWM_DISARMED fi if [ $PWM_MIN != none ] then - echo "[init] Set PWM min: $PWM_MIN" pwm min -c $PWM_OUTPUTS -p $PWM_MIN fi if [ $PWM_MAX != none ] then - echo "[init] Set PWM max: $PWM_MAX" pwm max -c $PWM_OUTPUTS -p $PWM_MAX fi fi diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index f9c8226350..201b997491 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -444,7 +444,6 @@ then if [ $GPS == yes ] then - echo "[init] Start GPS" if [ $GPS_FAKE == yes ] then echo "[init] Faking GPS" From b2671c8f058fae56c468b68d3185bfc1e8626710 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 22 Nov 2014 15:46:28 +0100 Subject: [PATCH 034/216] GPS: be less verbose --- src/drivers/gps/gps.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 5fb4b9ff8d..47c907bd33 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -274,7 +274,6 @@ GPS::task_main_trampoline(void *arg) void GPS::task_main() { - log("starting"); /* open the serial port */ _serial_fd = ::open(_port, O_RDWR); From 7bed194f4a217835b78f89c9e8399e1c946ca579 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 22 Nov 2014 15:46:51 +0100 Subject: [PATCH 035/216] EKF: less verbose --- .../attitude_estimator_ekf/attitude_estimator_ekf_main.cpp | 4 ---- 1 file changed, 4 deletions(-) diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index 35dc39ec6a..e2dbc30dd5 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -206,10 +206,6 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds 0, 0, 1.f }; /**< init: identity matrix */ - // print text - printf("Extended Kalman Filter Attitude Estimator initialized..\n\n"); - fflush(stdout); - int overloadcounter = 19; /* Initialize filter */ From 2fbda61521131f66cd3aab792716f3c40bc1cac8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 22 Nov 2014 15:47:10 +0100 Subject: [PATCH 036/216] mc attitude controller: Less verbose --- src/modules/mc_att_control/mc_att_control_main.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 19c10198c2..81a13edb8f 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -721,8 +721,6 @@ MulticopterAttitudeControl::task_main_trampoline(int argc, char *argv[]) void MulticopterAttitudeControl::task_main() { - warnx("started"); - fflush(stdout); /* * do subscriptions From df37f380cb792570ae07b1cc3e9eb61e3495fc43 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 22 Nov 2014 15:47:54 +0100 Subject: [PATCH 037/216] position controller main: Less verbose --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index d52138522c..e3c94bc46c 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -857,10 +857,8 @@ MulticopterPositionControl::control_auto(float dt) void MulticopterPositionControl::task_main() { - warnx("started"); _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); - mavlink_log_info(_mavlink_fd, "[mpc] started"); /* * do subscriptions From 90f28647539e7c8ae36ac74c7526582f3f18b3e7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 22 Nov 2014 15:49:03 +0100 Subject: [PATCH 038/216] INAV: Less verbose --- .../position_estimator_inav_main.c | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 81bbaa6587..f82a0f989a 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -161,7 +161,7 @@ int position_estimator_inav_main(int argc, char *argv[]) thread_should_exit = true; } else { - warnx("app not started"); + warnx("not started"); } exit(0); @@ -169,10 +169,10 @@ int position_estimator_inav_main(int argc, char *argv[]) if (!strcmp(argv[1], "status")) { if (thread_running) { - warnx("app is running"); + warnx("is running"); } else { - warnx("app not started"); + warnx("not started"); } exit(0); @@ -210,10 +210,8 @@ static void write_debug_log(const char *msg, float dt, float x_est[2], float y_e ****************************************************************************/ int position_estimator_inav_thread_main(int argc, char *argv[]) { - warnx("started"); int mavlink_fd; mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); - mavlink_log_info(mavlink_fd, "[inav] started"); float x_est[2] = { 0.0f, 0.0f }; // pos, vel float y_est[2] = { 0.0f, 0.0f }; // pos, vel From 1709c74f82da510fff5bf546a75c9894a19d8fd2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 22 Nov 2014 15:49:34 +0100 Subject: [PATCH 039/216] dataman: less verbose, fix code style --- src/modules/dataman/dataman.c | 14 +++++--------- 1 file changed, 5 insertions(+), 9 deletions(-) diff --git a/src/modules/dataman/dataman.c b/src/modules/dataman/dataman.c index b2355d4d89..705e54be32 100644 --- a/src/modules/dataman/dataman.c +++ b/src/modules/dataman/dataman.c @@ -629,9 +629,6 @@ task_main(int argc, char *argv[]) { work_q_item_t *work; - /* inform about start */ - warnx("Initializing.."); - /* Initialize global variables */ g_key_offsets[0] = 0; @@ -694,16 +691,15 @@ task_main(int argc, char *argv[]) if (sys_restart_val == DM_INIT_REASON_POWER_ON) { warnx("Power on restart"); _restart(DM_INIT_REASON_POWER_ON); - } - else if (sys_restart_val == DM_INIT_REASON_IN_FLIGHT) { + } else if (sys_restart_val == DM_INIT_REASON_IN_FLIGHT) { warnx("In flight restart"); _restart(DM_INIT_REASON_IN_FLIGHT); - } - else + } else { warnx("Unknown restart"); - } - else + } + } else { warnx("Unknown restart"); + } /* We use two file descriptors, one for the caller context and one for the worker thread */ /* They are actually the same but we need to some way to reject caller request while the */ From b3542bec085a598f34c2e796ee5c78328d0d990c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 22 Nov 2014 16:29:08 +0100 Subject: [PATCH 040/216] INAV: use int for outputs --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 4 ++-- .../position_estimator_inav_main.c | 11 ++++++----- 2 files changed, 8 insertions(+), 7 deletions(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index e3c94bc46c..5918d6bc5f 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -535,7 +535,7 @@ MulticopterPositionControl::reset_pos_sp() - _params.vel_ff(0) * _sp_move_rate(0)) / _params.pos_p(0); _pos_sp(1) = _pos(1) + (_vel(1) - _att_sp.R_body[1][2] * _att_sp.thrust / _params.vel_p(1) - _params.vel_ff(1) * _sp_move_rate(1)) / _params.pos_p(1); - mavlink_log_info(_mavlink_fd, "[mpc] reset pos sp: %.2f, %.2f", (double)_pos_sp(0), (double)_pos_sp(1)); + mavlink_log_info(_mavlink_fd, "[mpc] reset pos sp: %d, %d", (int)_pos_sp(0), (int)_pos_sp(1)); } } @@ -545,7 +545,7 @@ MulticopterPositionControl::reset_alt_sp() if (_reset_alt_sp) { _reset_alt_sp = false; _pos_sp(2) = _pos(2) + (_vel(2) - _params.vel_ff(2) * _sp_move_rate(2)) / _params.pos_p(2); - mavlink_log_info(_mavlink_fd, "[mpc] reset alt sp: %.2f", -(double)_pos_sp(2)); + mavlink_log_info(_mavlink_fd, "[mpc] reset alt sp: %d", -(int)_pos_sp(2)); } } diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index f82a0f989a..e736a86d77 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -387,8 +387,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } else { wait_baro = false; baro_offset /= (float) baro_init_cnt; - warnx("baro offs: %.2f", (double)baro_offset); - mavlink_log_info(mavlink_fd, "[inav] baro offs: %.2f", (double)baro_offset); + warnx("baro offs: %d", (int)baro_offset); + mavlink_log_info(mavlink_fd, "[inav] baro offs: %d", (int)baro_offset); local_pos.z_valid = true; local_pos.v_z_valid = true; } @@ -518,7 +518,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) sonar_valid_time = t; sonar_valid = true; local_pos.surface_bottom_timestamp = t; - mavlink_log_info(mavlink_fd, "[inav] new surface level: %.2f", (double)surface_offset); + mavlink_log_info(mavlink_fd, "[inav] new surface level: %d", (int)surface_offset); } } else { @@ -719,8 +719,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* initialize projection */ map_projection_init(&ref, lat, lon); - warnx("init ref: lat=%.7f, lon=%.7f, alt=%.2f", (double)lat, (double)lon, (double)alt); - mavlink_log_info(mavlink_fd, "[inav] init ref: %.7f, %.7f, %.2f", (double)lat, (double)lon, (double)alt); + // XXX replace this print + warnx("init ref: lat=%.7f, lon=%.7f, alt=%8.4f", (double)lat, (double)lon, (double)alt); + mavlink_log_info(mavlink_fd, "[inav] init ref: %.7f, %.7f, %8.4f", (double)lat, (double)lon, (double)alt); } } From 828163f2f5c6b68d9f262b1dd4f57e641c1d45a6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 22 Nov 2014 16:30:09 +0100 Subject: [PATCH 041/216] Update mixing handling to allow IO safety off updates --- src/modules/px4iofirmware/mixer.cpp | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index c0b8ac358c..66f0969de1 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -353,12 +353,16 @@ static unsigned mixer_text_length = 0; int mixer_handle_text(const void *buffer, size_t length) { - /* do not allow a mixer change while safety off */ - if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)) { + /* do not allow a mixer change while safety off and FMU armed */ + if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) && + (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) { return 1; } - /* abort if we're in the mixer */ + /* disable mixing, will be enabled once load is complete */ + r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_MIXER_OK); + + /* abort if we're in the mixer - the caller is expected to retry */ if (in_mixer) { return 1; } @@ -367,17 +371,16 @@ mixer_handle_text(const void *buffer, size_t length) isr_debug(2, "mix txt %u", length); - if (length < sizeof(px4io_mixdata)) + if (length < sizeof(px4io_mixdata)) { return 0; + } - unsigned text_length = length - sizeof(px4io_mixdata); + unsigned text_length = length - sizeof(px4io_mixdata); switch (msg->action) { case F2I_MIXER_ACTION_RESET: isr_debug(2, "reset"); - /* FIRST mark the mixer as invalid */ - r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK; /* THEN actually delete it */ mixer_group.reset(); mixer_text_length = 0; @@ -386,9 +389,6 @@ mixer_handle_text(const void *buffer, size_t length) case F2I_MIXER_ACTION_APPEND: isr_debug(2, "append %d", length); - /* disable mixing during the update */ - r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK; - /* check for overflow - this would be really fatal */ if ((mixer_text_length + text_length + 1) > sizeof(mixer_text)) { r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK; From 300705321a0ffa79ab3d4d53e3c029fe8238086a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 22 Nov 2014 16:30:59 +0100 Subject: [PATCH 042/216] Allow IO safety off system handling as long as the total system is not live --- src/modules/px4iofirmware/registers.c | 21 +++++++++++++-------- 1 file changed, 13 insertions(+), 8 deletions(-) diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index fbfdd35db0..a1a02965f4 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -407,10 +407,11 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num /* handle text going to the mixer parser */ case PX4IO_PAGE_MIXERLOAD: - if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) || - (r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) { - return mixer_handle_text(values, num_values * sizeof(*values)); - } + /* do not change the mixer if FMU is armed and IO's safety is off + * this state defines an active system. This check is done in the + * text handling function. + */ + return mixer_handle_text(values, num_values * sizeof(*values)); break; default: @@ -583,8 +584,10 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) break; case PX4IO_P_SETUP_REBOOT_BL: - if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) || - (r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) { + // do not reboot if FMU is armed and IO's safety is off + // this state defines an active system. + if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) && + (r_status_flags & PX4IO_P_SETUP_ARMING_FMU_ARMED)) { // don't allow reboot while armed break; } @@ -631,9 +634,11 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) /** * do not allow a RC config change while outputs armed + * = FMU is armed and IO's safety is off + * this state defines an active system. */ - if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) || - (r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) { + if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) && + (r_status_flags & PX4IO_P_SETUP_ARMING_FMU_ARMED)) { break; } From a36088b9c20e52e37d48e7a08aca39d5f8b901f5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 22 Nov 2014 16:29:08 +0100 Subject: [PATCH 043/216] INAV: use int for outputs --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 4 ++-- .../position_estimator_inav_main.c | 11 ++++++----- 2 files changed, 8 insertions(+), 7 deletions(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index d52138522c..769f84cb6c 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -535,7 +535,7 @@ MulticopterPositionControl::reset_pos_sp() - _params.vel_ff(0) * _sp_move_rate(0)) / _params.pos_p(0); _pos_sp(1) = _pos(1) + (_vel(1) - _att_sp.R_body[1][2] * _att_sp.thrust / _params.vel_p(1) - _params.vel_ff(1) * _sp_move_rate(1)) / _params.pos_p(1); - mavlink_log_info(_mavlink_fd, "[mpc] reset pos sp: %.2f, %.2f", (double)_pos_sp(0), (double)_pos_sp(1)); + mavlink_log_info(_mavlink_fd, "[mpc] reset pos sp: %d, %d", (int)_pos_sp(0), (int)_pos_sp(1)); } } @@ -545,7 +545,7 @@ MulticopterPositionControl::reset_alt_sp() if (_reset_alt_sp) { _reset_alt_sp = false; _pos_sp(2) = _pos(2) + (_vel(2) - _params.vel_ff(2) * _sp_move_rate(2)) / _params.pos_p(2); - mavlink_log_info(_mavlink_fd, "[mpc] reset alt sp: %.2f", -(double)_pos_sp(2)); + mavlink_log_info(_mavlink_fd, "[mpc] reset alt sp: %d", -(int)_pos_sp(2)); } } diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 81bbaa6587..c46d22733e 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -389,8 +389,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } else { wait_baro = false; baro_offset /= (float) baro_init_cnt; - warnx("baro offs: %.2f", (double)baro_offset); - mavlink_log_info(mavlink_fd, "[inav] baro offs: %.2f", (double)baro_offset); + warnx("baro offs: %d", (int)baro_offset); + mavlink_log_info(mavlink_fd, "[inav] baro offs: %d", (int)baro_offset); local_pos.z_valid = true; local_pos.v_z_valid = true; } @@ -520,7 +520,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) sonar_valid_time = t; sonar_valid = true; local_pos.surface_bottom_timestamp = t; - mavlink_log_info(mavlink_fd, "[inav] new surface level: %.2f", (double)surface_offset); + mavlink_log_info(mavlink_fd, "[inav] new surface level: %d", (int)surface_offset); } } else { @@ -721,8 +721,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* initialize projection */ map_projection_init(&ref, lat, lon); - warnx("init ref: lat=%.7f, lon=%.7f, alt=%.2f", (double)lat, (double)lon, (double)alt); - mavlink_log_info(mavlink_fd, "[inav] init ref: %.7f, %.7f, %.2f", (double)lat, (double)lon, (double)alt); + // XXX replace this print + warnx("init ref: lat=%.7f, lon=%.7f, alt=%8.4f", (double)lat, (double)lon, (double)alt); + mavlink_log_info(mavlink_fd, "[inav] init ref: %.7f, %.7f, %8.4f", (double)lat, (double)lon, (double)alt); } } From 3c5c1d3c89639d3e67086386d23d10e0dc8f65b0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 22 Nov 2014 15:08:54 +0100 Subject: [PATCH 044/216] Fix FD for commander arm operation --- src/modules/commander/commander.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index ec173c12be..e081955d07 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -310,12 +310,16 @@ int commander_main(int argc, char *argv[]) } if (!strcmp(argv[1], "arm")) { - arm_disarm(true, mavlink_fd, "command line"); + int mavlink_fd_local = open(MAVLINK_LOG_DEVICE, 0); + arm_disarm(true, mavlink_fd_local, "command line"); + close(mavlink_fd_local); exit(0); } if (!strcmp(argv[1], "disarm")) { - arm_disarm(false, mavlink_fd, "command line"); + int mavlink_fd_local = open(MAVLINK_LOG_DEVICE, 0); + arm_disarm(false, mavlink_fd_local, "command line"); + close(mavlink_fd_local); exit(0); } From 00961b55928a949a2dd3b7022fe2cbb4df06ec22 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 22 Nov 2014 15:43:21 +0100 Subject: [PATCH 045/216] ROMFS: Do only output necessary information on boot --- ROMFS/px4fmu_common/init.d/rc.interface | 6 +----- ROMFS/px4fmu_common/init.d/rcS | 1 - 2 files changed, 1 insertion(+), 6 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.interface b/ROMFS/px4fmu_common/init.d/rc.interface index e44cd0953d..41e0b149b4 100644 --- a/ROMFS/px4fmu_common/init.d/rc.interface +++ b/ROMFS/px4fmu_common/init.d/rc.interface @@ -10,7 +10,7 @@ then # set MIXERSD /fs/microsd/etc/mixers/$MIXER.mix - #Use the mixer file from the SD-card if it exists + # Use the mixer file from the SD-card if it exists if [ -f $MIXERSD ] then set MIXER_FILE $MIXERSD @@ -54,7 +54,6 @@ then # if [ $PWM_RATE != none ] then - echo "[init] Set PWM rate: $PWM_RATE" pwm rate -c $PWM_OUTPUTS -r $PWM_RATE fi @@ -63,17 +62,14 @@ then # if [ $PWM_DISARMED != none ] then - echo "[init] Set PWM disarmed: $PWM_DISARMED" pwm disarmed -c $PWM_OUTPUTS -p $PWM_DISARMED fi if [ $PWM_MIN != none ] then - echo "[init] Set PWM min: $PWM_MIN" pwm min -c $PWM_OUTPUTS -p $PWM_MIN fi if [ $PWM_MAX != none ] then - echo "[init] Set PWM max: $PWM_MAX" pwm max -c $PWM_OUTPUTS -p $PWM_MAX fi fi diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index f9c8226350..201b997491 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -444,7 +444,6 @@ then if [ $GPS == yes ] then - echo "[init] Start GPS" if [ $GPS_FAKE == yes ] then echo "[init] Faking GPS" From 54e7ed70e135f20a3470ebc71dae700e8c223102 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 22 Nov 2014 15:46:28 +0100 Subject: [PATCH 046/216] GPS: be less verbose --- src/drivers/gps/gps.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 5fb4b9ff8d..47c907bd33 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -274,7 +274,6 @@ GPS::task_main_trampoline(void *arg) void GPS::task_main() { - log("starting"); /* open the serial port */ _serial_fd = ::open(_port, O_RDWR); From b34b40622ba7bbd08e7ca5bc0571612ba96fc7ad Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 22 Nov 2014 15:46:51 +0100 Subject: [PATCH 047/216] EKF: less verbose --- .../attitude_estimator_ekf/attitude_estimator_ekf_main.cpp | 4 ---- 1 file changed, 4 deletions(-) diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index 35dc39ec6a..e2dbc30dd5 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -206,10 +206,6 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds 0, 0, 1.f }; /**< init: identity matrix */ - // print text - printf("Extended Kalman Filter Attitude Estimator initialized..\n\n"); - fflush(stdout); - int overloadcounter = 19; /* Initialize filter */ From 2a76b10f7a76273ac3b16d5ac917d40ea28a277b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 22 Nov 2014 15:47:10 +0100 Subject: [PATCH 048/216] mc attitude controller: Less verbose --- src/modules/mc_att_control/mc_att_control_main.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 19c10198c2..81a13edb8f 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -721,8 +721,6 @@ MulticopterAttitudeControl::task_main_trampoline(int argc, char *argv[]) void MulticopterAttitudeControl::task_main() { - warnx("started"); - fflush(stdout); /* * do subscriptions From 4c281030bb782f852600daf217d7a75b23489bbc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 22 Nov 2014 15:47:54 +0100 Subject: [PATCH 049/216] position controller main: Less verbose --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 769f84cb6c..5918d6bc5f 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -857,10 +857,8 @@ MulticopterPositionControl::control_auto(float dt) void MulticopterPositionControl::task_main() { - warnx("started"); _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); - mavlink_log_info(_mavlink_fd, "[mpc] started"); /* * do subscriptions From ace6c3fe409f4b70e75bb762e0ba6e0a574a7277 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 22 Nov 2014 15:49:03 +0100 Subject: [PATCH 050/216] INAV: Less verbose --- .../position_estimator_inav_main.c | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index c46d22733e..e736a86d77 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -161,7 +161,7 @@ int position_estimator_inav_main(int argc, char *argv[]) thread_should_exit = true; } else { - warnx("app not started"); + warnx("not started"); } exit(0); @@ -169,10 +169,10 @@ int position_estimator_inav_main(int argc, char *argv[]) if (!strcmp(argv[1], "status")) { if (thread_running) { - warnx("app is running"); + warnx("is running"); } else { - warnx("app not started"); + warnx("not started"); } exit(0); @@ -210,10 +210,8 @@ static void write_debug_log(const char *msg, float dt, float x_est[2], float y_e ****************************************************************************/ int position_estimator_inav_thread_main(int argc, char *argv[]) { - warnx("started"); int mavlink_fd; mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); - mavlink_log_info(mavlink_fd, "[inav] started"); float x_est[2] = { 0.0f, 0.0f }; // pos, vel float y_est[2] = { 0.0f, 0.0f }; // pos, vel From 138c25ec74f08db6f6759053d0200058a794c196 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 22 Nov 2014 15:49:34 +0100 Subject: [PATCH 051/216] dataman: less verbose, fix code style --- src/modules/dataman/dataman.c | 14 +++++--------- 1 file changed, 5 insertions(+), 9 deletions(-) diff --git a/src/modules/dataman/dataman.c b/src/modules/dataman/dataman.c index b2355d4d89..705e54be32 100644 --- a/src/modules/dataman/dataman.c +++ b/src/modules/dataman/dataman.c @@ -629,9 +629,6 @@ task_main(int argc, char *argv[]) { work_q_item_t *work; - /* inform about start */ - warnx("Initializing.."); - /* Initialize global variables */ g_key_offsets[0] = 0; @@ -694,16 +691,15 @@ task_main(int argc, char *argv[]) if (sys_restart_val == DM_INIT_REASON_POWER_ON) { warnx("Power on restart"); _restart(DM_INIT_REASON_POWER_ON); - } - else if (sys_restart_val == DM_INIT_REASON_IN_FLIGHT) { + } else if (sys_restart_val == DM_INIT_REASON_IN_FLIGHT) { warnx("In flight restart"); _restart(DM_INIT_REASON_IN_FLIGHT); - } - else + } else { warnx("Unknown restart"); - } - else + } + } else { warnx("Unknown restart"); + } /* We use two file descriptors, one for the caller context and one for the worker thread */ /* They are actually the same but we need to some way to reject caller request while the */ From 512779907e06f059a15d54c88d71b73aad9aced0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 23 Nov 2014 13:01:00 +0100 Subject: [PATCH 052/216] Update NuttX version, MD5 fix --- NuttX | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/NuttX b/NuttX index 9d06b64579..ec6b670f6d 160000 --- a/NuttX +++ b/NuttX @@ -1 +1 @@ -Subproject commit 9d06b645790e1445f14e3b19c71d40b3088f4e4f +Subproject commit ec6b670f6d1e964700c0b0a50d14db12761e3097 From 2f271888d2ed934c271637c22554b503ce68e535 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 23 Nov 2014 19:22:55 +0100 Subject: [PATCH 053/216] Added performance counter for SD log performance of write() call --- src/modules/sdlog2/sdlog2.c | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index af580f1f79..8638a49045 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -496,6 +496,8 @@ static void *logwriter_thread(void *arg) /* set name */ prctl(PR_SET_NAME, "sdlog2_writer", 0); + perf_counter_t perf_write = perf_alloc(PC_ELAPSED, "sd write"); + int log_fd = open_log_file(); if (log_fd < 0) { @@ -553,7 +555,9 @@ static void *logwriter_thread(void *arg) n = available; } + perf_begin(perf_write); n = write(log_fd, read_ptr, n); + perf_end(perf_write); should_wait = (n == available) && !is_part; @@ -586,6 +590,9 @@ static void *logwriter_thread(void *arg) fsync(log_fd); close(log_fd); + /* free performance counter */ + perf_free(perf_write); + return NULL; } From 08d6cbe6bf0b5b04f63e42c6c60f5b1fe6167547 Mon Sep 17 00:00:00 2001 From: philipoe Date: Sun, 23 Nov 2014 21:23:01 +0100 Subject: [PATCH 054/216] commander: Decrease RC-signal-regained message length to stay within 50 character length limit at all times --- src/modules/commander/commander.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index deb048566b..f77895efb9 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1544,7 +1544,7 @@ int commander_thread_main(int argc, char *argv[]) } else { if (status.rc_signal_lost) { - mavlink_log_critical(mavlink_fd, "RC SIGNAL REGAINED after %llums (at t=%llums)",(hrt_absolute_time()-status.rc_signal_lost_timestamp)/1000,hrt_absolute_time()/1000); + mavlink_log_critical(mavlink_fd, "RC SIGNAL REGAINED after %llums",(hrt_absolute_time()-status.rc_signal_lost_timestamp)/1000); status_changed = true; } } From 2dae1bc542746ed373458b55a8e564607700d17d Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 20 Nov 2014 09:05:10 +1100 Subject: [PATCH 055/216] uavcan: break the link between poll fd indexes and controls this linkage was fragile and makes it harder to add new orb subscriptions to the uavcan code --- src/modules/uavcan/uavcan_main.cpp | 5 ++--- src/modules/uavcan/uavcan_main.hpp | 3 +++ 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 2c543462ec..ccc087920f 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -333,14 +333,12 @@ int UavcanNode::run() } else { // get controls for required topics bool controls_updated = false; - unsigned poll_id = 1; for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) { if (_control_subs[i] > 0) { - if (_poll_fds[poll_id].revents & POLLIN) { + if (_poll_fds[_poll_ids[i]].revents & POLLIN) { controls_updated = true; orb_copy(_control_topics[i], _control_subs[i], &_controls[i]); } - poll_id++; } } @@ -474,6 +472,7 @@ UavcanNode::subscribe() if (_control_subs[i] > 0) { _poll_fds[_poll_fds_num].fd = _control_subs[i]; _poll_fds[_poll_fds_num].events = POLLIN; + _poll_ids[i] = _poll_fds_num; _poll_fds_num++; } } diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index 274321f0db..b791663d40 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -127,4 +127,7 @@ private: orb_id_t _control_topics[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] = {}; pollfd _poll_fds[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN + 1] = {}; ///< +1 for /dev/uavcan/busevent unsigned _poll_fds_num = 0; + + // index into _poll_fds for each _control_subs handle + uint8_t _poll_ids[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN]; }; From f6b0a3e07f27f34af6e6d209aec761374611e968 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 20 Nov 2014 09:35:58 +1100 Subject: [PATCH 056/216] uORB: added actuator_direct topic this topic will be used to allow direct output of actuator values for uavcan, bypassing the mixer. --- src/modules/uORB/Publication.cpp | 2 + src/modules/uORB/objects_common.cpp | 3 + src/modules/uORB/topics/actuator_direct.h | 69 +++++++++++++++++++++++ 3 files changed, 74 insertions(+) create mode 100644 src/modules/uORB/topics/actuator_direct.h diff --git a/src/modules/uORB/Publication.cpp b/src/modules/uORB/Publication.cpp index cd0b30dd61..71757e1f42 100644 --- a/src/modules/uORB/Publication.cpp +++ b/src/modules/uORB/Publication.cpp @@ -46,6 +46,7 @@ #include "topics/vehicle_attitude_setpoint.h" #include "topics/vehicle_rates_setpoint.h" #include "topics/actuator_outputs.h" +#include "topics/actuator_direct.h" #include "topics/encoders.h" #include "topics/tecs_status.h" @@ -76,6 +77,7 @@ template class __EXPORT Publication; template class __EXPORT Publication; template class __EXPORT Publication; template class __EXPORT Publication; +template class __EXPORT Publication; template class __EXPORT Publication; template class __EXPORT Publication; diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index b91a00c1e9..49dfc78349 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -192,6 +192,9 @@ ORB_DEFINE(actuator_outputs_1, struct actuator_outputs_s); ORB_DEFINE(actuator_outputs_2, struct actuator_outputs_s); ORB_DEFINE(actuator_outputs_3, struct actuator_outputs_s); +#include "topics/actuator_direct.h" +ORB_DEFINE(actuator_direct, struct actuator_direct_s); + #include "topics/multirotor_motor_limits.h" ORB_DEFINE(multirotor_motor_limits, struct multirotor_motor_limits_s); diff --git a/src/modules/uORB/topics/actuator_direct.h b/src/modules/uORB/topics/actuator_direct.h new file mode 100644 index 0000000000..5f9d0f56dd --- /dev/null +++ b/src/modules/uORB/topics/actuator_direct.h @@ -0,0 +1,69 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file actuator_direct.h + * + * Actuator direct values. + * + * Values published to this topic are the direct actuator values which + * should be passed to actuators, bypassing mixing + */ + +#ifndef TOPIC_ACTUATOR_DIRECT_H +#define TOPIC_ACTUATOR_DIRECT_H + +#include +#include "../uORB.h" + +#define NUM_ACTUATORS_DIRECT 16 + +/** + * @addtogroup topics + * @{ + */ + +struct actuator_direct_s { + uint64_t timestamp; /**< timestamp in us since system boot */ + float values[NUM_ACTUATORS_DIRECT]; /**< actuator values, from -1 to 1 */ + unsigned nvalues; /**< number of valid values */ +}; + +/** + * @} + */ + +/* actuator direct ORB */ +ORB_DECLARE(actuator_direct); + +#endif // TOPIC_ACTUATOR_DIRECT_H From b830137ec8492dc583b61689b7d56a1b359a5c5b Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 20 Nov 2014 09:37:02 +1100 Subject: [PATCH 057/216] uavcan: added support for actuator_direct ORB topic this watches the actuator_direct topic and uses it to allow for direct output of actuator values, bypassing the mixer --- src/modules/uavcan/uavcan_main.cpp | 23 +++++++++++++++++++++++ src/modules/uavcan/uavcan_main.hpp | 5 +++++ 2 files changed, 28 insertions(+) diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index ccc087920f..cf40f3ea8a 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -280,6 +280,7 @@ int UavcanNode::run() _armed_sub = orb_subscribe(ORB_ID(actuator_armed)); _test_motor_sub = orb_subscribe(ORB_ID(test_motor)); + _actuator_direct_sub = orb_subscribe(ORB_ID(actuator_direct)); actuator_outputs_s outputs; memset(&outputs, 0, sizeof(outputs)); @@ -310,6 +311,18 @@ int UavcanNode::run() _poll_fds[_poll_fds_num].events = POLLIN; _poll_fds_num += 1; + /* + * setup poll to look for actuator direct input if we are + * subscribed to the topic + */ + if (_actuator_direct_sub != -1) { + _poll_fds[_poll_fds_num] = ::pollfd(); + _poll_fds[_poll_fds_num].fd = _actuator_direct_sub; + _poll_fds[_poll_fds_num].events = POLLIN; + _actuator_direct_poll_fd_num = _poll_fds_num; + _poll_fds_num += 1; + } + while (!_task_should_exit) { // update actuator controls subscriptions if needed if (_groups_subscribed != _groups_required) { @@ -342,6 +355,16 @@ int UavcanNode::run() } } + /* + see if we have any direct actuator updates + */ + if (_actuator_direct_sub != -1 && + (_poll_fds[_actuator_direct_poll_fd_num].revents & POLLIN) && + orb_copy(ORB_ID(actuator_direct), _actuator_direct_sub, &_actuator_direct) == OK) { + // Output to the bus + _esc_controller.update_outputs(_actuator_direct.values, _actuator_direct.nvalues); + } + // can we mix? if (_test_in_progress) { float test_outputs[NUM_ACTUATOR_OUTPUTS] = {}; diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index b791663d40..d7eb167642 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -42,6 +42,7 @@ #include #include #include +#include #include "actuators/esc.hpp" #include "sensors/sensor_bridge.hpp" @@ -128,6 +129,10 @@ private: pollfd _poll_fds[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN + 1] = {}; ///< +1 for /dev/uavcan/busevent unsigned _poll_fds_num = 0; + int _actuator_direct_sub = -1; ///< uORB subscription of the actuator_direct topic + uint8_t _actuator_direct_poll_fd_num; + actuator_direct_s _actuator_direct; + // index into _poll_fds for each _control_subs handle uint8_t _poll_ids[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN]; }; From ecc7a3cbb42f6490b43f21b3849738184934a2a0 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 20 Nov 2014 09:52:50 +1100 Subject: [PATCH 058/216] motor_test: prevent use of uninitialised test_motor orb handle stack variables are not initialised to zero --- src/systemcmds/motor_test/motor_test.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/systemcmds/motor_test/motor_test.c b/src/systemcmds/motor_test/motor_test.c index 1b7ff75f7b..77dc2f0d50 100644 --- a/src/systemcmds/motor_test/motor_test.c +++ b/src/systemcmds/motor_test/motor_test.c @@ -59,9 +59,10 @@ __EXPORT int motor_test_main(int argc, char *argv[]); static void motor_test(unsigned channel, float value); static void usage(const char *reason); +static orb_advert_t _test_motor_pub; + void motor_test(unsigned channel, float value) { - orb_advert_t _test_motor_pub; struct test_motor_s _test_motor; _test_motor.motor_number = channel; From ead0458e97d6e19cc0b188124063e90962820e3a Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 20 Nov 2014 16:58:16 +1100 Subject: [PATCH 059/216] uavcan: don't force motors to keep spinning at zero throttle Forcing motors to keep spinning when armed should be a policy decision up at the vehicle type level, not hard coded down in the ESC driver. It isn't appropriate for fixed wing or ground vehicles for example. We could add an ioctl to enable "spin when armed" if just setting a small value in the vehicle code is inconvenient --- src/modules/uavcan/actuators/esc.cpp | 13 +++++++++---- 1 file changed, 9 insertions(+), 4 deletions(-) diff --git a/src/modules/uavcan/actuators/esc.cpp b/src/modules/uavcan/actuators/esc.cpp index 1d23099f3d..7efd505112 100644 --- a/src/modules/uavcan/actuators/esc.cpp +++ b/src/modules/uavcan/actuators/esc.cpp @@ -101,10 +101,15 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs) for (unsigned i = 0; i < num_outputs; i++) { if (_armed_mask & MOTOR_BIT(i)) { float scaled = (outputs[i] + 1.0F) * 0.5F * cmd_max; - if (scaled < 1.0F) { - scaled = 1.0F; // Since we're armed, we don't want to stop it completely - } - + // trim negative values back to 0. Previously + // we set this to 0.1, which meant motors kept + // spinning when armed, but that should be a + // policy decision for a specific vehicle + // type, as it is not appropriate for all + // types of vehicles (eg. fixed wing). + if (scaled < 0.0F) { + scaled = 0.0F; + } if (scaled > cmd_max) { scaled = cmd_max; perf_count(_perfcnt_scaling_error); From 8e44ec2e3bd6852b74e4463c373555dd1bab47d3 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 20 Nov 2014 17:00:02 +1100 Subject: [PATCH 060/216] uavcan: prevent crash in ESC driver passing in more than 8 actuators would crash the ESC driver. We need to check again the array size of the _esc_status.esc, which is CONNECTED_ESC_MAX --- src/modules/uavcan/actuators/esc.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/modules/uavcan/actuators/esc.cpp b/src/modules/uavcan/actuators/esc.cpp index 7efd505112..9f682c7e16 100644 --- a/src/modules/uavcan/actuators/esc.cpp +++ b/src/modules/uavcan/actuators/esc.cpp @@ -76,7 +76,9 @@ int UavcanEscController::init() void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs) { - if ((outputs == nullptr) || (num_outputs > uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize)) { + if ((outputs == nullptr) || + (num_outputs > uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize) || + (num_outputs > CONNECTED_ESC_MAX)) { perf_count(_perfcnt_invalid_input); return; } From 724ec0ec8b0d34c631b2784236a990e535700254 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 20 Nov 2014 17:02:24 +1100 Subject: [PATCH 061/216] uavcan: handle all ESC output in one place moving all the ESC output handling to one place allows the limits on actuator values to apply to all types of inputs, and will make it easier to expand "uavcan status" to show actuator values --- src/modules/uavcan/uavcan_main.cpp | 88 +++++++++++++++++------------- src/modules/uavcan/uavcan_main.hpp | 2 + 2 files changed, 51 insertions(+), 39 deletions(-) diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index cf40f3ea8a..e4f58b3105 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -282,8 +282,7 @@ int UavcanNode::run() _test_motor_sub = orb_subscribe(ORB_ID(test_motor)); _actuator_direct_sub = orb_subscribe(ORB_ID(actuator_direct)); - actuator_outputs_s outputs; - memset(&outputs, 0, sizeof(outputs)); + memset(&_outputs, 0, sizeof(_outputs)); const int busevent_fd = ::open(uavcan_stm32::BusEvent::DevName, 0); if (busevent_fd < 0) @@ -339,6 +338,8 @@ int UavcanNode::run() node_spin_once(); // Non-blocking + bool new_output = false; + // this would be bad... if (poll_ret < 0) { log("poll error %d", errno); @@ -360,18 +361,25 @@ int UavcanNode::run() */ if (_actuator_direct_sub != -1 && (_poll_fds[_actuator_direct_poll_fd_num].revents & POLLIN) && - orb_copy(ORB_ID(actuator_direct), _actuator_direct_sub, &_actuator_direct) == OK) { - // Output to the bus - _esc_controller.update_outputs(_actuator_direct.values, _actuator_direct.nvalues); + orb_copy(ORB_ID(actuator_direct), _actuator_direct_sub, &_actuator_direct) == OK && + !_test_in_progress) { + if (_actuator_direct.nvalues > NUM_ACTUATOR_OUTPUTS) { + _actuator_direct.nvalues = NUM_ACTUATOR_OUTPUTS; + } + memcpy(&_outputs.output[0], &_actuator_direct.values[0], + _actuator_direct.nvalues*sizeof(float)); + _outputs.noutputs = _actuator_direct.nvalues; + new_output = true; } // can we mix? if (_test_in_progress) { - float test_outputs[NUM_ACTUATOR_OUTPUTS] = {}; - test_outputs[_test_motor.motor_number] = _test_motor.value*2.0f-1.0f; - - // Output to the bus - _esc_controller.update_outputs(test_outputs, NUM_ACTUATOR_OUTPUTS); + memset(&_outputs, 0, sizeof(_outputs)); + if (_test_motor.motor_number < NUM_ACTUATOR_OUTPUTS) { + _outputs.output[_test_motor.motor_number] = _test_motor.value*2.0f-1.0f; + _outputs.noutputs = _test_motor.motor_number+1; + } + new_output = true; } else if (controls_updated && (_mixers != nullptr)) { // XXX one output group has 8 outputs max, @@ -379,39 +387,41 @@ int UavcanNode::run() unsigned num_outputs_max = 8; // Do mixing - outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs_max); - outputs.timestamp = hrt_absolute_time(); + _outputs.noutputs = _mixers->mix(&_outputs.output[0], num_outputs_max); - // iterate actuators - for (unsigned i = 0; i < outputs.noutputs; i++) { - // last resort: catch NaN, INF and out-of-band errors - if (!isfinite(outputs.output[i])) { - /* - * Value is NaN, INF or out of band - set to the minimum value. - * This will be clearly visible on the servo status and will limit the risk of accidentally - * spinning motors. It would be deadly in flight. - */ - outputs.output[i] = -1.0f; - } - - // limit outputs to valid range - - // never go below min - if (outputs.output[i] < -1.0f) { - outputs.output[i] = -1.0f; - } - - // never go below max - if (outputs.output[i] > 1.0f) { - outputs.output[i] = 1.0f; - } - } - - // Output to the bus - _esc_controller.update_outputs(outputs.output, outputs.noutputs); + new_output = true; } } + if (new_output) { + // iterate actuators, checking for valid values + for (uint8_t i = 0; i < _outputs.noutputs; i++) { + // last resort: catch NaN, INF and out-of-band errors + if (!isfinite(_outputs.output[i])) { + /* + * Value is NaN, INF or out of band - set to the minimum value. + * This will be clearly visible on the servo status and will limit the risk of accidentally + * spinning motors. It would be deadly in flight. + */ + _outputs.output[i] = -1.0f; + } + + // never go below min + if (_outputs.output[i] < -1.0f) { + _outputs.output[i] = -1.0f; + } + + // never go above max + if (_outputs.output[i] > 1.0f) { + _outputs.output[i] = 1.0f; + } + } + // Output to the bus + _outputs.timestamp = hrt_absolute_time(); + _esc_controller.update_outputs(_outputs.output, _outputs.noutputs); + } + + // Check motor test state bool updated = false; orb_check(_test_motor_sub, &updated); diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index d7eb167642..5a036fcd7f 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -133,6 +133,8 @@ private: uint8_t _actuator_direct_poll_fd_num; actuator_direct_s _actuator_direct; + actuator_outputs_s _outputs; + // index into _poll_fds for each _control_subs handle uint8_t _poll_ids[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN]; }; From a7a68c88a25c2c4cb401fb4f8894de6f4c280ba2 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 20 Nov 2014 17:03:29 +1100 Subject: [PATCH 062/216] uavcan: show ESC output values in uavcan status, and add arm/disarm this makes "uavcan status" show the current output values, which is useful for debugging. It also adds "uavcan arm" and "uavcan disarm" commands, which are very useful for re-arming after a motor test. --- src/modules/uavcan/uavcan_main.cpp | 20 +++++++++++++++++++- 1 file changed, 19 insertions(+), 1 deletion(-) diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index e4f58b3105..8fa7656332 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -604,6 +604,14 @@ UavcanNode::print_info() (unsigned)_groups_subscribed, (unsigned)_groups_required, _poll_fds_num); printf("ESC mixer: %s\n", (_mixers == nullptr) ? "NONE" : "OK"); + if (_outputs.noutputs != 0) { + printf("ESC output: "); + for (uint8_t i=0; i<_outputs.noutputs; i++) { + printf("%d ", (int)(_outputs.output[i]*1000)); + } + printf("\n"); + } + // Sensor bridges auto br = _sensor_bridges.getHead(); while (br != nullptr) { @@ -622,7 +630,7 @@ UavcanNode::print_info() static void print_usage() { warnx("usage: \n" - "\tuavcan {start|status|stop}"); + "\tuavcan {start|status|stop|arm|disarm}"); } extern "C" __EXPORT int uavcan_main(int argc, char *argv[]); @@ -669,6 +677,16 @@ int uavcan_main(int argc, char *argv[]) ::exit(0); } + if (!std::strcmp(argv[1], "arm")) { + inst->arm_actuators(true); + ::exit(0); + } + + if (!std::strcmp(argv[1], "disarm")) { + inst->arm_actuators(false); + ::exit(0); + } + if (!std::strcmp(argv[1], "stop")) { delete inst; ::exit(0); From 7ae4f6d97e398a64aeb99c52880651a2282be5b9 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 21 Nov 2014 08:17:50 +1100 Subject: [PATCH 063/216] uavcan: added add_poll_fd() helper function this makes the code clearer and avoids repeated code --- src/modules/uavcan/uavcan_main.cpp | 36 +++++++++++++++++------------- src/modules/uavcan/uavcan_main.hpp | 7 +++++- 2 files changed, 27 insertions(+), 16 deletions(-) diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 8fa7656332..8147a8b898 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -269,6 +269,24 @@ void UavcanNode::node_spin_once() } } +/* + add a fd to the list of polled events. This assumes you want + POLLIN for now. + */ +int UavcanNode::add_poll_fd(int fd) +{ + int ret = _poll_fds_num; + if (_poll_fds_num >= UAVCAN_NUM_POLL_FDS) { + errx(1, "uavcan: too many poll fds, exiting"); + } + _poll_fds[_poll_fds_num] = ::pollfd(); + _poll_fds[_poll_fds_num].fd = fd; + _poll_fds[_poll_fds_num].events = POLLIN; + _poll_fds_num += 1; + return ret; +} + + int UavcanNode::run() { (void)pthread_mutex_lock(&_node_mutex); @@ -304,22 +322,14 @@ int UavcanNode::run() * the value returned from poll() to detect whether actuator control has timed out or not. * Instead, all ORB events need to be checked individually (see below). */ - _poll_fds_num = 0; - _poll_fds[_poll_fds_num] = ::pollfd(); - _poll_fds[_poll_fds_num].fd = busevent_fd; - _poll_fds[_poll_fds_num].events = POLLIN; - _poll_fds_num += 1; + add_poll_fd(busevent_fd); /* * setup poll to look for actuator direct input if we are * subscribed to the topic */ if (_actuator_direct_sub != -1) { - _poll_fds[_poll_fds_num] = ::pollfd(); - _poll_fds[_poll_fds_num].fd = _actuator_direct_sub; - _poll_fds[_poll_fds_num].events = POLLIN; - _actuator_direct_poll_fd_num = _poll_fds_num; - _poll_fds_num += 1; + _actuator_direct_poll_fd_num = add_poll_fd(_actuator_direct_sub); } while (!_task_should_exit) { @@ -490,7 +500,6 @@ UavcanNode::subscribe() uint32_t sub_groups = _groups_required & ~_groups_subscribed; uint32_t unsub_groups = _groups_subscribed & ~_groups_required; // the first fd used by CAN - _poll_fds_num = 1; for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) { if (sub_groups & (1 << i)) { warnx("subscribe to actuator_controls_%d", i); @@ -503,10 +512,7 @@ UavcanNode::subscribe() } if (_control_subs[i] > 0) { - _poll_fds[_poll_fds_num].fd = _control_subs[i]; - _poll_fds[_poll_fds_num].events = POLLIN; - _poll_ids[i] = _poll_fds_num; - _poll_fds_num++; + _poll_ids[i] = add_poll_fd(_control_subs[i]); } } } diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index 5a036fcd7f..98f2e5ad4b 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -58,6 +58,9 @@ #define NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN 4 #define UAVCAN_DEVICE_PATH "/dev/uavcan/esc" +// we add two to allow for actuator_direct and busevent +#define UAVCAN_NUM_POLL_FDS (NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN+2) + /** * A UAVCAN node. */ @@ -98,6 +101,8 @@ private: int init(uavcan::NodeID node_id); void node_spin_once(); int run(); + int add_poll_fd(int fd); ///< add a fd to poll list, returning index into _poll_fds[] + int _task = -1; ///< handle to the OS task bool _task_should_exit = false; ///< flag to indicate to tear down the CAN driver @@ -126,7 +131,7 @@ private: int _control_subs[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] = {}; actuator_controls_s _controls[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] = {}; orb_id_t _control_topics[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] = {}; - pollfd _poll_fds[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN + 1] = {}; ///< +1 for /dev/uavcan/busevent + pollfd _poll_fds[UAVCAN_NUM_POLL_FDS] = {}; unsigned _poll_fds_num = 0; int _actuator_direct_sub = -1; ///< uORB subscription of the actuator_direct topic From 809500cfcd220e65d7eb301e33533c5fe29f03d8 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 26 Nov 2014 11:37:56 +1100 Subject: [PATCH 064/216] px4io: fixed RC_CONFIG channel limit check number of channels is the right test, not number of actuators --- src/drivers/px4io/px4io.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 6f02ba62c8..e61454b008 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -2592,7 +2592,7 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg) on param_get() */ struct pwm_output_rc_config* config = (struct pwm_output_rc_config*)arg; - if (config->channel >= _max_actuators) { + if (config->channel >= RC_INPUT_MAX_CHANNELS) { /* fail with error */ return E2BIG; } From 29f000dc31f6f18af0130b46d5e143e983549d19 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 26 Nov 2014 11:39:17 +1100 Subject: [PATCH 065/216] px4io: fixed error returns to be negative follow standard conventions --- src/drivers/px4io/px4io.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index e61454b008..b31d7bbfa4 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -2198,7 +2198,7 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg) struct pwm_output_values* pwm = (struct pwm_output_values*)arg; if (pwm->channel_count > _max_actuators) /* fail with error */ - return E2BIG; + return -E2BIG; /* copy values to registers in IO */ ret = io_reg_set(PX4IO_PAGE_FAILSAFE_PWM, 0, pwm->values, pwm->channel_count); @@ -2217,7 +2217,7 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg) struct pwm_output_values* pwm = (struct pwm_output_values*)arg; if (pwm->channel_count > _max_actuators) /* fail with error */ - return E2BIG; + return -E2BIG; /* copy values to registers in IO */ ret = io_reg_set(PX4IO_PAGE_DISARMED_PWM, 0, pwm->values, pwm->channel_count); @@ -2236,7 +2236,7 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg) struct pwm_output_values* pwm = (struct pwm_output_values*)arg; if (pwm->channel_count > _max_actuators) /* fail with error */ - return E2BIG; + return -E2BIG; /* copy values to registers in IO */ ret = io_reg_set(PX4IO_PAGE_CONTROL_MIN_PWM, 0, pwm->values, pwm->channel_count); @@ -2255,7 +2255,7 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg) struct pwm_output_values* pwm = (struct pwm_output_values*)arg; if (pwm->channel_count > _max_actuators) /* fail with error */ - return E2BIG; + return -E2BIG; /* copy values to registers in IO */ ret = io_reg_set(PX4IO_PAGE_CONTROL_MAX_PWM, 0, pwm->values, pwm->channel_count); @@ -2594,7 +2594,7 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg) struct pwm_output_rc_config* config = (struct pwm_output_rc_config*)arg; if (config->channel >= RC_INPUT_MAX_CHANNELS) { /* fail with error */ - return E2BIG; + return -E2BIG; } /* copy values to registers in IO */ From c0b47d6a74197a0dc57c56efbd63803424a9835a Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 8 Nov 2014 15:48:33 +1100 Subject: [PATCH 066/216] px4io: only check SAFETY_OFF for allowing RC config changes and reboot If we check OUTPUTS_ARMED then we can't update trim values and scaling in flight, as there is no way to clear OUTPUTS_ARMED. If safety is on then it should be perfectly safe to update the mixer and RC config or reboot for fw update --- src/modules/px4iofirmware/registers.c | 13 +++---------- 1 file changed, 3 insertions(+), 10 deletions(-) diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index a1a02965f4..f0c2cfd26d 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -412,7 +412,6 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num * text handling function. */ return mixer_handle_text(values, num_values * sizeof(*values)); - break; default: /* avoid offset wrap */ @@ -584,10 +583,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) break; case PX4IO_P_SETUP_REBOOT_BL: - // do not reboot if FMU is armed and IO's safety is off - // this state defines an active system. - if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) && - (r_status_flags & PX4IO_P_SETUP_ARMING_FMU_ARMED)) { + if (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) { // don't allow reboot while armed break; } @@ -633,12 +629,9 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) case PX4IO_PAGE_RC_CONFIG: { /** - * do not allow a RC config change while outputs armed - * = FMU is armed and IO's safety is off - * this state defines an active system. + * do not allow a RC config change while safety is off */ - if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) && - (r_status_flags & PX4IO_P_SETUP_ARMING_FMU_ARMED)) { + if (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) { break; } From c906c2123822ef127026eeaf272b3aceed9f8995 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 26 Nov 2014 09:22:24 +1100 Subject: [PATCH 067/216] px4io: prevent use of uninitialised memory in io_set_arming_state() the vehicle may not have setup a control_mode. We need to check the return of orb_copy() to ensure we are getting initialised values --- src/drivers/px4io/px4io.cpp | 80 +++++++++++++++++++------------------ 1 file changed, 41 insertions(+), 39 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index b31d7bbfa4..58390ba4c3 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1160,52 +1160,54 @@ PX4IO::io_set_arming_state() actuator_armed_s armed; ///< system armed state vehicle_control_mode_s control_mode; ///< vehicle_control_mode - orb_copy(ORB_ID(actuator_armed), _t_actuator_armed, &armed); - orb_copy(ORB_ID(vehicle_control_mode), _t_vehicle_control_mode, &control_mode); + int have_armed = orb_copy(ORB_ID(actuator_armed), _t_actuator_armed, &armed); + int have_control_mode = orb_copy(ORB_ID(vehicle_control_mode), _t_vehicle_control_mode, &control_mode); uint16_t set = 0; uint16_t clear = 0; - if (armed.armed) { - set |= PX4IO_P_SETUP_ARMING_FMU_ARMED; + if (have_armed == OK) { + if (armed.armed) { + set |= PX4IO_P_SETUP_ARMING_FMU_ARMED; + } else { + clear |= PX4IO_P_SETUP_ARMING_FMU_ARMED; + } - } else { - clear |= PX4IO_P_SETUP_ARMING_FMU_ARMED; + if (armed.lockdown && !_lockdown_override) { + set |= PX4IO_P_SETUP_ARMING_LOCKDOWN; + } else { + clear |= PX4IO_P_SETUP_ARMING_LOCKDOWN; + } + + /* Do not set failsafe if circuit breaker is enabled */ + if (armed.force_failsafe && !_cb_flighttermination) { + set |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE; + } else { + clear |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE; + } + + // XXX this is for future support in the commander + // but can be removed if unneeded + // if (armed.termination_failsafe) { + // set |= PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE; + // } else { + // clear |= PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE; + // } + + if (armed.ready_to_arm) { + set |= PX4IO_P_SETUP_ARMING_IO_ARM_OK; + + } else { + clear |= PX4IO_P_SETUP_ARMING_IO_ARM_OK; + } } - if (armed.lockdown && !_lockdown_override) { - set |= PX4IO_P_SETUP_ARMING_LOCKDOWN; - } else { - clear |= PX4IO_P_SETUP_ARMING_LOCKDOWN; - } - - /* Do not set failsafe if circuit breaker is enabled */ - if (armed.force_failsafe && !_cb_flighttermination) { - set |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE; - } else { - clear |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE; - } - - // XXX this is for future support in the commander - // but can be removed if unneeded - // if (armed.termination_failsafe) { - // set |= PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE; - // } else { - // clear |= PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE; - // } - - if (armed.ready_to_arm) { - set |= PX4IO_P_SETUP_ARMING_IO_ARM_OK; - - } else { - clear |= PX4IO_P_SETUP_ARMING_IO_ARM_OK; - } - - if (control_mode.flag_external_manual_override_ok) { - set |= PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK; - - } else { - clear |= PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK; + if (have_control_mode == OK) { + if (control_mode.flag_external_manual_override_ok) { + set |= PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK; + } else { + clear |= PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK; + } } return io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, clear, set); From 8e932cec10aa7e64e4a5cd17a571778f3ff60e9b Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 7 Nov 2014 20:00:40 +1100 Subject: [PATCH 068/216] systemcmds: added reflect command for USB testing --- src/systemcmds/reflect/module.mk | 41 ++++++++++++ src/systemcmds/reflect/reflect.c | 111 +++++++++++++++++++++++++++++++ 2 files changed, 152 insertions(+) create mode 100644 src/systemcmds/reflect/module.mk create mode 100644 src/systemcmds/reflect/reflect.c diff --git a/src/systemcmds/reflect/module.mk b/src/systemcmds/reflect/module.mk new file mode 100644 index 0000000000..973eb775df --- /dev/null +++ b/src/systemcmds/reflect/module.mk @@ -0,0 +1,41 @@ +############################################################################ +# +# Copyright (c) 2014 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# Dump file utility +# + +MODULE_COMMAND = reflect +SRCS = reflect.c + +MAXOPTIMIZATION = -Os diff --git a/src/systemcmds/reflect/reflect.c b/src/systemcmds/reflect/reflect.c new file mode 100644 index 0000000000..6bb53c71a5 --- /dev/null +++ b/src/systemcmds/reflect/reflect.c @@ -0,0 +1,111 @@ +/**************************************************************************** + * + * Copyright (c) 2014 Andrew Tridgell. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file reflect.c + * + * simple data reflector for load testing terminals (especially USB) + * + * @author Andrew Tridgell + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +__EXPORT int reflect_main(int argc, char *argv[]); + +// memory corruption checking +#define MAX_BLOCKS 1000 +static uint32_t nblocks; +struct block { + uint32_t v[256]; +}; +static struct block *blocks[MAX_BLOCKS]; + +#define VALUE(i) ((i*7) ^ 0xDEADBEEF) + +static void allocate_blocks(void) +{ + while (nblocks < MAX_BLOCKS) { + blocks[nblocks] = calloc(1, sizeof(struct block)); + if (blocks[nblocks] == NULL) { + break; + } + for (uint32_t i=0; iv)/sizeof(uint32_t); i++) { + blocks[nblocks]->v[i] = VALUE(i); + } + nblocks++; + } + printf("Allocated %u blocks\n", nblocks); +} + +static void check_blocks(void) +{ + for (uint32_t n=0; nv)/sizeof(uint32_t); i++) { + assert(blocks[n]->v[i] == VALUE(i)); + } + } +} + +int +reflect_main(int argc, char *argv[]) +{ + uint32_t total = 0; + printf("Starting reflector\n"); + + allocate_blocks(); + + while (true) { + char buf[128]; + ssize_t n = read(0, buf, sizeof(buf)); + if (n < 0) { + break; + } + if (n > 0) { + write(1, buf, n); + } + total += n; + if (total > 1024000) { + check_blocks(); + total = 0; + } + } + return OK; +} From 4724c050478900a0b0b760877f1613e43c0aa97c Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 3 Nov 2014 11:24:28 +1100 Subject: [PATCH 069/216] airspeed: use _retries=2 for I2C retries once initialised airspeed sensors often need to be on longer cables due to having to be outside the prop wash. --- src/drivers/airspeed/airspeed.cpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/src/drivers/airspeed/airspeed.cpp b/src/drivers/airspeed/airspeed.cpp index 3a1e1b7b51..6db6713c4e 100644 --- a/src/drivers/airspeed/airspeed.cpp +++ b/src/drivers/airspeed/airspeed.cpp @@ -159,13 +159,15 @@ out: int Airspeed::probe() { - /* on initial power up the device needs more than one retry - for detection. Once it is running then retries aren't - needed + /* on initial power up the device may need more than one retry + for detection. Once it is running the number of retries can + be reduced */ _retries = 4; int ret = measure(); - _retries = 0; + + // drop back to 2 retries once initialised + _retries = 2; return ret; } From 32835757de16c8886373b8299cbcc4f5ffc19476 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 26 Nov 2014 08:24:11 +0100 Subject: [PATCH 070/216] Remove uncommon modules from FMU-v2 build --- makefiles/config_px4fmu-v2_default.mk | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index 1b538f52f8..889e169a17 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -27,9 +27,9 @@ MODULES += drivers/l3gd20 MODULES += drivers/hmc5883 MODULES += drivers/ms5611 MODULES += drivers/mb12xx -MODULES += drivers/sf0x +#MODULES += drivers/sf0x MODULES += drivers/ll40ls -MODULES += drivers/trone +#MODULES += drivers/trone MODULES += drivers/gps MODULES += drivers/hil MODULES += drivers/hott/hott_telemetry From 2a9a649adbf64ba2c8c56d5a934aa8e09838bec6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 26 Nov 2014 18:21:54 +0100 Subject: [PATCH 071/216] Make boot less verbose to not hide the important status messages --- ROMFS/px4fmu_common/init.d/rcS | 5 ----- 1 file changed, 5 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 201b997491..353f448776 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -301,7 +301,6 @@ then if [ $OUTPUT_MODE == io -o $OUTPUT_MODE == uavcan_esc ] then - echo "[init] Use PX4IO PWM as primary output" if px4io start then echo "[init] PX4IO started" @@ -314,7 +313,6 @@ then if [ $OUTPUT_MODE == fmu -o $OUTPUT_MODE == ardrone ] then - echo "[init] Use FMU as primary output" if fmu mode_$FMU_MODE then echo "[init] FMU mode_$FMU_MODE started" @@ -338,7 +336,6 @@ then if [ $OUTPUT_MODE == mkblctrl ] then - echo "[init] Use MKBLCTRL as primary output" set MKBLCTRL_ARG "" if [ $MKBLCTRL_MODE == x ] then @@ -361,7 +358,6 @@ then if [ $OUTPUT_MODE == hil ] then - echo "[init] Use HIL as primary output" if hil mode_port2_pwm8 then echo "[init] HIL output started" @@ -380,7 +376,6 @@ then then if px4io start then - echo "[init] PX4IO started" sh /etc/init.d/rc.io else echo "[init] ERROR: PX4IO start failed" From 9d986f5df3c8da646451fc865c1b1e9e2dbdea9c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 26 Nov 2014 18:22:18 +0100 Subject: [PATCH 072/216] HMC5883: Better status reporting --- src/drivers/hmc5883/hmc5883.cpp | 39 ++++++++++++++++++++++++--------- 1 file changed, 29 insertions(+), 10 deletions(-) diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 81f7679650..d4dbf37782 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -1349,7 +1349,7 @@ HMC5883 *g_dev_ext = nullptr; void start(int bus, enum Rotation rotation); void test(int bus); void reset(int bus); -void info(int bus); +int info(int bus); int calibrate(int bus); void usage(); @@ -1595,17 +1595,23 @@ reset(int bus) /** * Print a little info about the driver. */ -void +int info(int bus) { - HMC5883 *g_dev = (bus == PX4_I2C_BUS_ONBOARD?g_dev_int:g_dev_ext); - if (g_dev == nullptr) - errx(1, "driver not running"); + int ret = 1; - printf("state @ %p\n", g_dev); - g_dev->print_info(); + HMC5883 *g_dev = (bus == PX4_I2C_BUS_ONBOARD ? g_dev_int : g_dev_ext); + if (g_dev == nullptr) { + warnx("not running on bus %d", bus); + } else { - exit(0); + warnx("running on bus: %d (%s)\n", bus, ((PX4_I2C_BUS_ONBOARD) ? "onboard" : "offboard")); + + g_dev->print_info(); + ret = 0; + } + + return ret; } void @@ -1685,8 +1691,21 @@ hmc5883_main(int argc, char *argv[]) /* * Print driver information. */ - if (!strcmp(verb, "info") || !strcmp(verb, "status")) - hmc5883::info(bus); + if (!strcmp(verb, "info") || !strcmp(verb, "status")) { + if (bus == -1) { + int ret = 0; + if (hmc5883::info(PX4_I2C_BUS_ONBOARD)) { + ret = 1; + } + + if (hmc5883::info(PX4_I2C_BUS_EXPANSION)) { + ret = 1; + } + exit(ret); + } else { + exit(hmc5883::info(bus)); + } + } /* * Autocalibrate the scaling From 22a247ca6760996ccb5f583b032253d4af97ed00 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 27 Nov 2014 23:19:53 +0100 Subject: [PATCH 073/216] Disable the BlinkM driver, code style fixes for other disabled drivers --- makefiles/config_px4fmu-v2_default.mk | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index 889e169a17..df6b6d0c5b 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -27,14 +27,14 @@ MODULES += drivers/l3gd20 MODULES += drivers/hmc5883 MODULES += drivers/ms5611 MODULES += drivers/mb12xx -#MODULES += drivers/sf0x +# MODULES += drivers/sf0x MODULES += drivers/ll40ls -#MODULES += drivers/trone +# MODULES += drivers/trone MODULES += drivers/gps MODULES += drivers/hil MODULES += drivers/hott/hott_telemetry MODULES += drivers/hott/hott_sensors -MODULES += drivers/blinkm +# MODULES += drivers/blinkm MODULES += drivers/airspeed MODULES += drivers/ets_airspeed MODULES += drivers/meas_airspeed From 8e8b622f4f5c4737e0f7dad33870f23742449fe9 Mon Sep 17 00:00:00 2001 From: Friedemann Ludwig Date: Fri, 28 Nov 2014 04:11:39 +0100 Subject: [PATCH 074/216] Implemented altitude and velocity hold mode --- .../fw_att_control/fw_att_control_main.cpp | 23 ++++- .../fw_pos_control_l1_main.cpp | 86 ++++++++++++++++++- 2 files changed, 106 insertions(+), 3 deletions(-) diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index e770c11a27..a295d61ac1 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -751,14 +751,33 @@ FixedwingAttitudeControl::task_main() * - manual control is disabled (another app may send the setpoint, but it should * for sure not be set from the remote control values) */ - if (_vcontrol_mode.flag_control_velocity_enabled || - _vcontrol_mode.flag_control_position_enabled || + if (_vcontrol_mode.flag_control_auto_enabled || !_vcontrol_mode.flag_control_manual_enabled) { /* read in attitude setpoint from attitude setpoint uorb topic */ roll_sp = _att_sp.roll_body + _parameters.rollsp_offset_rad; pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad; throttle_sp = _att_sp.thrust; + /* reset integrals where needed */ + if (_att_sp.roll_reset_integral) { + _roll_ctrl.reset_integrator(); + } + if (_att_sp.pitch_reset_integral) { + _pitch_ctrl.reset_integrator(); + } + if (_att_sp.yaw_reset_integral) { + _yaw_ctrl.reset_integrator(); + } + } + else if (_vcontrol_mode.flag_control_velocity_enabled) { + /* + * Velocity should be controlled and manual is enabled. + */ + roll_sp = (_manual.y * _parameters.man_roll_max - _parameters.trim_roll) + + _parameters.rollsp_offset_rad; + pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad; + throttle_sp = _att_sp.thrust; + /* reset integrals where needed */ if (_att_sp.roll_reset_integral) { _roll_ctrl.reset_integrator(); diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 6017369aa5..4f84f088b8 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -163,6 +163,8 @@ private: perf_counter_t _loop_perf; /**< loop performance counter */ + float _hold_alt; /**< hold altitude for velocity mode */ + /* land states */ bool land_noreturn_horizontal; bool land_noreturn_vertical; @@ -198,6 +200,8 @@ private: TECS _tecs; fwPosctrl::mTecs _mTecs; bool _was_pos_control_mode; + bool _was_velocity_control_mode; + bool _was_alt_control_mode; struct { float l1_period; @@ -316,6 +320,11 @@ private: */ void vehicle_status_poll(); + /** + * Check for manual setpoint updates. + */ + bool vehicle_manual_control_setpoint_poll(); + /** * Check for airspeed updates. */ @@ -692,6 +701,22 @@ FixedwingPositionControl::vehicle_airspeed_poll() return airspeed_updated; } +bool +FixedwingPositionControl::vehicle_manual_control_setpoint_poll() +{ + bool manual_updated; + + /* Check if manual setpoint has changed */ + orb_check(_manual_control_sub, &manual_updated); + + if (manual_updated) { + orb_copy(ORB_ID(manual_control_setpoint), _manual_control_sub, &_manual); + } + + return manual_updated; +} + + void FixedwingPositionControl::vehicle_attitude_poll() { @@ -852,6 +877,13 @@ bool FixedwingPositionControl::control_position(const math::Vector<2> ¤t_position, const math::Vector<3> &ground_speed, const struct position_setpoint_triplet_s &pos_sp_triplet) { + static hrt_abstime functionLastCalled = 0; + float dt = 0.0f; + if (functionLastCalled > 0) { + dt = (float)hrt_elapsed_time(&functionLastCalled) * 1e-6f; + } + functionLastCalled = hrt_absolute_time(); + bool setpoint = true; math::Vector<2> ground_speed_2d = {ground_speed(0), ground_speed(1)}; @@ -888,6 +920,10 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } _was_pos_control_mode = true; + _was_velocity_control_mode = false; + + /* reset hold altitude */ + _hold_alt = _global_pos.alt; /* get circle mode */ bool was_circle_mode = _l1_control.circle_mode(); @@ -1209,12 +1245,59 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _att_sp.roll_reset_integral = true; } - } else { + } else if (_control_mode.flag_control_velocity_enabled) { + const float deadBand = (60.0f/1000.0f); + const float factor = 1.0f - deadBand; + if (!_was_velocity_control_mode) { + _hold_alt = _global_pos.alt; + _was_alt_control_mode = false; + } + _was_velocity_control_mode = true; + _was_pos_control_mode = false; + // Get demanded airspeed + float altctrl_airspeed = _parameters.airspeed_min + + (_parameters.airspeed_max - _parameters.airspeed_min) * + _manual.z; + // Get demanded vertical velocity from pitch control + float pitch = 0.0f; + if (_manual.x > deadBand) { + pitch = (_manual.x - deadBand) / factor; + } else if (_manual.x < - deadBand) { + pitch = (_manual.x + deadBand) / factor; + } + if (pitch > 0.0f) { + _hold_alt -= (_parameters.max_climb_rate * dt) * pitch; + _was_alt_control_mode = false; + } else if (pitch < 0.0f) { + _hold_alt -= (_parameters.max_sink_rate * dt) * pitch; + _was_alt_control_mode = false; + } else if (!_was_alt_control_mode) { + _hold_alt = _global_pos.alt; + _was_alt_control_mode = true; + } + tecs_update_pitch_throttle(_hold_alt, + altctrl_airspeed, + eas2tas, + math::radians(_parameters.pitch_limit_min), + math::radians(_parameters.pitch_limit_max), + _parameters.throttle_min, + _parameters.throttle_max, + _parameters.throttle_cruise, + false, + math::radians(_parameters.pitch_limit_min), + _global_pos.alt, + ground_speed, + TECS_MODE_NORMAL); + } else { + _was_velocity_control_mode = false; _was_pos_control_mode = false; /** MANUAL FLIGHT **/ + /* reset hold altitude */ + _hold_alt = _global_pos.alt; + /* no flight mode applies, do not publish an attitude setpoint */ setpoint = false; @@ -1350,6 +1433,7 @@ FixedwingPositionControl::task_main() vehicle_setpoint_poll(); vehicle_sensor_combined_poll(); vehicle_airspeed_poll(); + vehicle_manual_control_setpoint_poll(); // vehicle_baro_poll(); math::Vector<3> ground_speed(_global_pos.vel_n, _global_pos.vel_e, _global_pos.vel_d); From 1da7ca7f78dfa6cd441df5df259b3c191f77fecb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 28 Nov 2014 14:23:08 +0100 Subject: [PATCH 075/216] Updating NuttX version --- NuttX | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/NuttX b/NuttX index 9d06b64579..ae4b05e2c5 160000 --- a/NuttX +++ b/NuttX @@ -1 +1 @@ -Subproject commit 9d06b645790e1445f14e3b19c71d40b3088f4e4f +Subproject commit ae4b05e2c51d07369b5d131052099ac346b0841c From f619dba6f96d22152c1c97216a46a27981ff2472 Mon Sep 17 00:00:00 2001 From: Holger Steinhaus Date: Fri, 28 Nov 2014 15:33:21 +0100 Subject: [PATCH 076/216] Corrected time_gps_usec values description. Fixes #1474 --- src/modules/uORB/topics/vehicle_gps_position.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/uORB/topics/vehicle_gps_position.h b/src/modules/uORB/topics/vehicle_gps_position.h index 31e616f4f7..7888a50f4c 100644 --- a/src/modules/uORB/topics/vehicle_gps_position.h +++ b/src/modules/uORB/topics/vehicle_gps_position.h @@ -79,7 +79,7 @@ struct vehicle_gps_position_s { bool vel_ned_valid; /**< Flag to indicate if NED speed is valid */ uint64_t timestamp_time; /**< Timestamp for time information */ - uint64_t time_gps_usec; /**< Timestamp (microseconds in GPS format), this is the timestamp which comes from the gps module */ + uint64_t time_gps_usec; /**< Timestamp (microseconds in GPS format), this is the timestamp which comes from the gps module. It might be unavailable right after cold start, indicated by a value of 0 */ uint8_t satellites_used; /**< Number of satellites used */ }; From 6efc63d709f5afdbed29528647776d56e7f384a6 Mon Sep 17 00:00:00 2001 From: Friedemann Ludwig Date: Sun, 30 Nov 2014 20:51:01 +0100 Subject: [PATCH 077/216] fixed somereview comments --- src/modules/fw_att_control/fw_att_control_main.cpp | 3 +-- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 13 ++++++++----- 2 files changed, 9 insertions(+), 7 deletions(-) diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index a295d61ac1..2d5744b8a3 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -768,8 +768,7 @@ FixedwingAttitudeControl::task_main() if (_att_sp.yaw_reset_integral) { _yaw_ctrl.reset_integrator(); } - } - else if (_vcontrol_mode.flag_control_velocity_enabled) { + } else if (_vcontrol_mode.flag_control_velocity_enabled) { /* * Velocity should be controlled and manual is enabled. */ diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 4f84f088b8..e7c95cc864 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -164,6 +164,7 @@ private: perf_counter_t _loop_perf; /**< loop performance counter */ float _hold_alt; /**< hold altitude for velocity mode */ + hrt_abstime _control_position_last_called; /** ¤t_position, const math::Vector<3> &ground_speed, const struct position_setpoint_triplet_s &pos_sp_triplet) { - static hrt_abstime functionLastCalled = 0; - float dt = 0.0f; - if (functionLastCalled > 0) { - dt = (float)hrt_elapsed_time(&functionLastCalled) * 1e-6f; + float dt = FLT_MIN; // Using non zero value to a avoid division by zero + if (_control_position_last_called > 0) { + dt = (float)hrt_elapsed_time(&_control_position_last_called) * 1e-6f; } - functionLastCalled = hrt_absolute_time(); + _control_position_last_called = hrt_absolute_time(); bool setpoint = true; From e51f72000bb850f9933a5ba9c024dddb6248440c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 1 Dec 2014 11:27:59 +0100 Subject: [PATCH 078/216] Fix RGB led stop command --- src/drivers/rgbled/rgbled.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/drivers/rgbled/rgbled.cpp b/src/drivers/rgbled/rgbled.cpp index 13cbfdfa8b..d357222441 100644 --- a/src/drivers/rgbled/rgbled.cpp +++ b/src/drivers/rgbled/rgbled.cpp @@ -121,7 +121,7 @@ private: /* for now, we only support one RGBLED */ namespace { -RGBLED *g_rgbled; +RGBLED *g_rgbled = nullptr; } void rgbled_usage(); @@ -680,15 +680,15 @@ rgbled_main(int argc, char *argv[]) ret = ioctl(fd, RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_OFF); close(fd); + /* delete the rgbled object if stop was requested, in addition to turning off the LED. */ + if (!strcmp(verb, "stop")) { + delete g_rgbled; + g_rgbled = nullptr; + exit(0); + } exit(ret); } - if (!strcmp(verb, "stop")) { - delete g_rgbled; - g_rgbled = nullptr; - exit(0); - } - if (!strcmp(verb, "rgb")) { if (argc < 5) { errx(1, "Usage: rgbled rgb "); From 3ce7abe9d84a6c716985339c8165bec98e481847 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Tue, 2 Dec 2014 01:01:35 +0100 Subject: [PATCH 079/216] use consistent orientation naming in messages --- src/modules/commander/accelerometer_calibration.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 0cb41489f8..8eac0a6fdb 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -263,7 +263,7 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float const int samples_num = 2500; float accel_ref[6][3]; bool data_collected[6] = { false, false, false, false, false, false }; - const char *orientation_strs[6] = { "front", "back", "left", "right", "top", "bottom" }; + const char *orientation_strs[6] = { "front", "back", "left", "right", "up", "down" }; int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); From b1bd813978e48d32a66b8a49bccdda7f053ea55c Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Tue, 2 Dec 2014 01:37:54 +0100 Subject: [PATCH 080/216] swap fron/back > the "side" being measured is facing downwards --- src/modules/commander/accelerometer_calibration.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 8eac0a6fdb..49c653ad20 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -263,7 +263,7 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float const int samples_num = 2500; float accel_ref[6][3]; bool data_collected[6] = { false, false, false, false, false, false }; - const char *orientation_strs[6] = { "front", "back", "left", "right", "up", "down" }; + const char *orientation_strs[6] = { "back", "front", "left", "right", "up", "down" }; int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); @@ -294,8 +294,8 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float /* inform user which axes are still needed */ mavlink_log_info(mavlink_fd, "pending: %s%s%s%s%s%s", - (!data_collected[0]) ? "front " : "", - (!data_collected[1]) ? "back " : "", + (!data_collected[0]) ? "back " : "", + (!data_collected[1]) ? "front " : "", (!data_collected[2]) ? "left " : "", (!data_collected[3]) ? "right " : "", (!data_collected[4]) ? "up " : "", From 45c52b51ee84bead9562a51a4d410fcbe921e798 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Tue, 2 Dec 2014 01:42:12 +0100 Subject: [PATCH 081/216] move natural position to the front of the pending list for QGC --- src/modules/commander/accelerometer_calibration.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 49c653ad20..d4cd97be6a 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -294,12 +294,12 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float /* inform user which axes are still needed */ mavlink_log_info(mavlink_fd, "pending: %s%s%s%s%s%s", + (!data_collected[5]) ? "down " : "", (!data_collected[0]) ? "back " : "", (!data_collected[1]) ? "front " : "", (!data_collected[2]) ? "left " : "", (!data_collected[3]) ? "right " : "", - (!data_collected[4]) ? "up " : "", - (!data_collected[5]) ? "down " : ""); + (!data_collected[4]) ? "up " : ""); /* allow user enough time to read the message */ sleep(3); From 39169c35fe00769016cef266ed491c6323fc8630 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Tue, 2 Dec 2014 10:28:30 +0100 Subject: [PATCH 082/216] added configuration file and mixer file for for caipironha VTOL --- ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol | 14 ++++++++++++++ .../px4fmu_common/mixers/FMU_caipirinha_vtol.mix | 16 ++++++++++++++++ 2 files changed, 30 insertions(+) create mode 100644 ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol create mode 100644 ROMFS/px4fmu_common/mixers/FMU_caipirinha_vtol.mix diff --git a/ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol b/ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol new file mode 100644 index 0000000000..c8b095b3c7 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol @@ -0,0 +1,14 @@ +#!nsh +# +# Generic quadshot configuration file +# +# Roman Bapst +# + +sh /etc/init.d/rc.vtol_defaults + +set MIXER FMU_caipirinha_vtol + +set PWM_OUTPUTS 12 +set PWM_MIN 1080 +set PWM_MAX 2000 diff --git a/ROMFS/px4fmu_common/mixers/FMU_caipirinha_vtol.mix b/ROMFS/px4fmu_common/mixers/FMU_caipirinha_vtol.mix new file mode 100644 index 0000000000..5ae0f5588e --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/FMU_caipirinha_vtol.mix @@ -0,0 +1,16 @@ +#!nsh +# Caipirinha vtol mixer for PX4FMU +# +#=========================== +R: 2- 10000 10000 10000 0 + +#mixer for the elevons +M: 2 +O: 10000 10000 0 -10000 10000 +S: 1 0 10000 10000 0 -10000 10000 +S: 1 1 10000 10000 0 -10000 10000 + +M: 2 +O: 10000 10000 0 -10000 10000 +S: 1 0 10000 10000 0 -10000 10000 +S: 1 1 -10000 -10000 0 -10000 10000 From 48ba912f08b328d709965a4dd7acb878b598f961 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Tue, 2 Dec 2014 10:30:26 +0100 Subject: [PATCH 083/216] adapted and added ROMFS-scripts to support VTOL --- ROMFS/px4fmu_common/init.d/rc.autostart | 17 +++++++++ ROMFS/px4fmu_common/init.d/rc.vtol_apps | 15 ++++++++ ROMFS/px4fmu_common/init.d/rc.vtol_defaults | 39 +++++++++++++++++++++ ROMFS/px4fmu_common/init.d/rcS | 39 +++++++++++++++++++++ 4 files changed, 110 insertions(+) create mode 100644 ROMFS/px4fmu_common/init.d/rc.vtol_apps create mode 100644 ROMFS/px4fmu_common/init.d/rc.vtol_defaults diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart b/ROMFS/px4fmu_common/init.d/rc.autostart index 496a52c5fa..331ff4974d 100644 --- a/ROMFS/px4fmu_common/init.d/rc.autostart +++ b/ROMFS/px4fmu_common/init.d/rc.autostart @@ -15,6 +15,7 @@ # 10000 .. 10999 Wide arm / H frame # 11000 .. 11999 Hexa Cox # 12000 .. 12999 Octo Cox +# 13000 .. 13999 VTOL # # Simulation @@ -237,3 +238,19 @@ if param compare SYS_AUTOSTART 12001 then sh /etc/init.d/12001_octo_cox fi + +# +# VTOL quadshot +# + if param compare SYS_AUTOSTART 13000 + then + sh /etc/init.d/13000_quadshot + fi + +# +# VTOL caipririnha +# + if param compare SYS_AUTOSTART 13001 + then + sh /etc/init.d/13001_caipirinha_vtol + fi diff --git a/ROMFS/px4fmu_common/init.d/rc.vtol_apps b/ROMFS/px4fmu_common/init.d/rc.vtol_apps new file mode 100644 index 0000000000..23ade6d78b --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.vtol_apps @@ -0,0 +1,15 @@ +#!nsh +# +# Standard apps for vtol: +# att & pos estimator, att & pos control. +# + +attitude_estimator_ekf start +#ekf_att_pos_estimator start +position_estimator_inav start + +vtol_att_control start +mc_att_control start +mc_pos_control start +fw_att_control start +fw_pos_control_l1 start diff --git a/ROMFS/px4fmu_common/init.d/rc.vtol_defaults b/ROMFS/px4fmu_common/init.d/rc.vtol_defaults new file mode 100644 index 0000000000..376a2e2579 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.vtol_defaults @@ -0,0 +1,39 @@ +#!nsh + +set VEHICLE_TYPE vtol + +if [ $DO_AUTOCONFIG == yes ] +then + # + #Default controller parameters for MC + # + param set MC_ROLL_P 6.0 + param set MC_ROLLRATE_P 0.1 + param set MC_ROLLRATE_I 0.0 + param set MC_ROLLRATE_D 0.003 + param set MC_PITCH_P 6.0 + param set MC_PITCHRATE_P 0.2 + param set MC_PITCHRATE_I 0.0 + param set MC_PITCHRATE_D 0.003 + param set MC_YAW_P 4 + param set MC_YAWRATE_P 0.2 + param set MC_YAWRATE_I 0.0 + param set MC_YAWRATE_D 0.0 + param set MC_YAW_FF 0.3 + + # + # Default parameters for FW + # + param set FW_PR_FF 0.3 + param set FW_PR_I 0.000 + param set FW_PR_IMAX 0.2 + param set FW_PR_P 0.02 + param set FW_RR_FF 0.3 + param set FW_RR_I 0.00 + param set FW_RR_IMAX 0.2 + param set FW_RR_P 0.02 +fi + +set PWM_DISARMED 900 +set PWM_MIN 1000 +set PWM_MAX 2000 diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 353f448776..ffef598c48 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -545,7 +545,46 @@ then sh /etc/init.d/rc.mc_apps fi fi + + # + # VTOL setup + # + if [ $VEHICLE_TYPE == vtol ] + then + echo "[init] Vehicle type: VTOL" + if [ $MIXER == none ] + then + echo "Default mixer for vtol not defined" + fi + + if [ $MAV_TYPE == none ] + then + # Use mixer to detect vehicle type + if [ $MIXER == FMU_quadshot ] + then + set MAV_TYPE 20 + fi + fi + + # Still no MAV_TYPE found + if [ $MAV_TYPE == none ] + then + echo "Unknown MAV_TYPE" + else + param set MAV_TYPE $MAV_TYPE + fi + + # Load mixer and configure outputs + sh /etc/init.d/rc.interface + + # Start standard vtol apps + if [ $LOAD_DEFAULT_APPS == yes ] + then + sh /etc/init.d/rc.vtol_apps + fi + fi + # # Start the navigator # From 35c985e5234f2b2c7cc511a791a1be90bae820bf Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Tue, 2 Dec 2014 10:32:13 +0100 Subject: [PATCH 084/216] extended uORB structs with VTOL specific control topics --- src/modules/uORB/objects_common.cpp | 10 +++ src/modules/uORB/topics/actuator_controls.h | 3 + src/modules/uORB/topics/vehicle_attitude.h | 17 +++++ .../uORB/topics/vehicle_attitude_setpoint.h | 2 + .../uORB/topics/vehicle_rates_setpoint.h | 3 +- src/modules/uORB/topics/vehicle_status.h | 5 +- src/modules/uORB/topics/vtol_vehicle_status.h | 66 +++++++++++++++++++ 7 files changed, 104 insertions(+), 2 deletions(-) create mode 100644 src/modules/uORB/topics/vtol_vehicle_status.h diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 49dfc78349..1141431cc7 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -91,6 +91,9 @@ ORB_DEFINE(home_position, struct home_position_s); #include "topics/vehicle_status.h" ORB_DEFINE(vehicle_status, struct vehicle_status_s); +#include "topics/vtol_vehicle_status.h" +ORB_DEFINE(vtol_vehicle_status, struct vtol_vehicle_status_s); + #include "topics/safety.h" ORB_DEFINE(safety, struct safety_s); @@ -114,6 +117,8 @@ ORB_DEFINE(vehicle_vicon_position, struct vehicle_vicon_position_s); #include "topics/vehicle_rates_setpoint.h" ORB_DEFINE(vehicle_rates_setpoint, struct vehicle_rates_setpoint_s); +ORB_DEFINE(mc_virtual_rates_setpoint, struct vehicle_rates_setpoint_s); +ORB_DEFINE(fw_virtual_rates_setpoint, struct vehicle_rates_setpoint_s); #include "topics/rc_channels.h" ORB_DEFINE(rc_channels, struct rc_channels_s); @@ -148,6 +153,8 @@ ORB_DEFINE(fence, unsigned); #include "topics/vehicle_attitude_setpoint.h" ORB_DEFINE(vehicle_attitude_setpoint, struct vehicle_attitude_setpoint_s); +ORB_DEFINE(mc_virtual_attitude_setpoint, struct vehicle_attitude_setpoint_s); +ORB_DEFINE(fw_virtual_attitude_setpoint, struct vehicle_attitude_setpoint_s); #include "topics/manual_control_setpoint.h" ORB_DEFINE(manual_control_setpoint, struct manual_control_setpoint_s); @@ -182,6 +189,9 @@ ORB_DEFINE(actuator_controls_0, struct actuator_controls_s); ORB_DEFINE(actuator_controls_1, struct actuator_controls_s); ORB_DEFINE(actuator_controls_2, struct actuator_controls_s); ORB_DEFINE(actuator_controls_3, struct actuator_controls_s); +//Virtual control groups, used for VTOL operation +ORB_DEFINE(actuator_controls_virtual_mc, struct actuator_controls_s); +ORB_DEFINE(actuator_controls_virtual_fw, struct actuator_controls_s); #include "topics/actuator_armed.h" ORB_DEFINE(actuator_armed, struct actuator_armed_s); diff --git a/src/modules/uORB/topics/actuator_controls.h b/src/modules/uORB/topics/actuator_controls.h index e768ab2f61..4d1f9a6380 100644 --- a/src/modules/uORB/topics/actuator_controls.h +++ b/src/modules/uORB/topics/actuator_controls.h @@ -74,5 +74,8 @@ ORB_DECLARE(actuator_controls_0); ORB_DECLARE(actuator_controls_1); ORB_DECLARE(actuator_controls_2); ORB_DECLARE(actuator_controls_3); +ORB_DECLARE(actuator_controls_virtual_mc); +ORB_DECLARE(actuator_controls_virtual_fw); + #endif diff --git a/src/modules/uORB/topics/vehicle_attitude.h b/src/modules/uORB/topics/vehicle_attitude.h index 40328af146..77324415a7 100755 --- a/src/modules/uORB/topics/vehicle_attitude.h +++ b/src/modules/uORB/topics/vehicle_attitude.h @@ -80,6 +80,23 @@ struct vehicle_attitude_s { bool R_valid; /**< Rotation matrix valid */ bool q_valid; /**< Quaternion valid */ + // secondary attitude, use for VTOL + float roll_sec; /**< Roll angle (rad, Tait-Bryan, NED) */ + float pitch_sec; /**< Pitch angle (rad, Tait-Bryan, NED) */ + float yaw_sec; /**< Yaw angle (rad, Tait-Bryan, NED) */ + float rollspeed_sec; /**< Roll angular speed (rad/s, Tait-Bryan, NED) */ + float pitchspeed_sec; /**< Pitch angular speed (rad/s, Tait-Bryan, NED) */ + float yawspeed_sec; /**< Yaw angular speed (rad/s, Tait-Bryan, NED) */ + float rollacc_sec; /**< Roll angular accelration (rad/s, Tait-Bryan, NED) */ + float pitchacc_sec; /**< Pitch angular acceleration (rad/s, Tait-Bryan, NED) */ + float yawacc_sec; /**< Yaw angular acceleration (rad/s, Tait-Bryan, NED) */ + float rate_offsets_sec[3]; /**< Offsets of the body angular rates from zero */ + float R_sec[3][3]; /**< Rotation matrix body to world, (Tait-Bryan, NED) */ + float q_sec[4]; /**< Quaternion (NED) */ + float g_comp_sec[3]; /**< Compensated gravity vector */ + bool R_valid_sec; /**< Rotation matrix valid */ + bool q_valid_sec; /**< Quaternion valid */ + }; /** diff --git a/src/modules/uORB/topics/vehicle_attitude_setpoint.h b/src/modules/uORB/topics/vehicle_attitude_setpoint.h index 8446e9c6e2..1cfc37cc6f 100644 --- a/src/modules/uORB/topics/vehicle_attitude_setpoint.h +++ b/src/modules/uORB/topics/vehicle_attitude_setpoint.h @@ -83,5 +83,7 @@ struct vehicle_attitude_setpoint_s { /* register this as object request broker structure */ ORB_DECLARE(vehicle_attitude_setpoint); +ORB_DECLARE(mc_virtual_attitude_setpoint); +ORB_DECLARE(fw_virtual_attitude_setpoint); #endif /* TOPIC_ARDRONE_CONTROL_H_ */ diff --git a/src/modules/uORB/topics/vehicle_rates_setpoint.h b/src/modules/uORB/topics/vehicle_rates_setpoint.h index 9f8b412a7d..47d51f199f 100644 --- a/src/modules/uORB/topics/vehicle_rates_setpoint.h +++ b/src/modules/uORB/topics/vehicle_rates_setpoint.h @@ -63,5 +63,6 @@ struct vehicle_rates_setpoint_s { /* register this as object request broker structure */ ORB_DECLARE(vehicle_rates_setpoint); - +ORB_DECLARE(mc_virtual_rates_setpoint); +ORB_DECLARE(fw_virtual_rates_setpoint); #endif diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 8ec9d98d61..749d00d75b 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -147,7 +147,10 @@ enum VEHICLE_TYPE { VEHICLE_TYPE_TRICOPTER = 15, /* Octorotor | */ VEHICLE_TYPE_FLAPPING_WING = 16, /* Flapping wing | */ VEHICLE_TYPE_KITE = 17, /* Kite | */ - VEHICLE_TYPE_ENUM_END = 18, /* | */ + VEHICLE_TYPE_ONBOARD_CONTROLLER=18, /* Onboard companion controller | */ + VEHICLE_TYPE_VTOL_DUOROTOR = 19, /* Vtol with two engines */ + VEHICLE_TYPE_VTOL_QUADROTOR = 20, /* Vtol with four engines*/ + VEHICLE_TYPE_ENUM_END = 21 /* | */ }; enum VEHICLE_BATTERY_WARNING { diff --git a/src/modules/uORB/topics/vtol_vehicle_status.h b/src/modules/uORB/topics/vtol_vehicle_status.h new file mode 100644 index 0000000000..24ecca9faa --- /dev/null +++ b/src/modules/uORB/topics/vtol_vehicle_status.h @@ -0,0 +1,66 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file vtol_status.h + * + * Vtol status topic + * + */ + +#ifndef TOPIC_VTOL_STATUS_H +#define TOPIC_VTOL_STATUS_H + +#include +#include "../uORB.h" + +/** + * @addtogroup topics + * @{ + */ + +/* Indicates in which mode the vtol aircraft is in */ +struct vtol_vehicle_status_s { + + uint64_t timestamp; /**< Microseconds since system boot */ + bool vtol_in_rw_mode; /*true: vtol vehicle is in rotating wing mode */ +}; + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(vtol_vehicle_status); + +#endif From ad204bbb5d2bab4431b15281fde693e6bf03a0fd Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Tue, 2 Dec 2014 10:33:43 +0100 Subject: [PATCH 085/216] log secondary attitude and fixed wing controls for VTOL --- src/modules/sdlog2/sdlog2.c | 25 +++++++++++++++++++++++++ src/modules/sdlog2/sdlog2_messages.h | 13 ++++++++++++- 2 files changed, 37 insertions(+), 1 deletion(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 0b6861d2a5..8f87d897a1 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -941,6 +941,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct vehicle_rates_setpoint_s rates_sp; struct actuator_outputs_s act_outputs; struct actuator_controls_s act_controls; + struct actuator_controls_s act_controls1; struct vehicle_local_position_s local_pos; struct vehicle_local_position_setpoint_s local_pos_sp; struct vehicle_global_position_s global_pos; @@ -981,6 +982,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_LPSP_s log_LPSP; struct log_GPS_s log_GPS; struct log_ATTC_s log_ATTC; + struct log_FWC_s log_FWC; struct log_STAT_s log_STAT; struct log_RC_s log_RC; struct log_OUT0_s log_OUT0; @@ -1022,6 +1024,7 @@ int sdlog2_thread_main(int argc, char *argv[]) int rates_sp_sub; int act_outputs_sub; int act_controls_sub; + int act_controls_1_sub; int local_pos_sub; int local_pos_sp_sub; int global_pos_sub; @@ -1055,6 +1058,7 @@ int sdlog2_thread_main(int argc, char *argv[]) subs.rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); subs.act_outputs_sub = orb_subscribe(ORB_ID_VEHICLE_CONTROLS); subs.act_controls_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); + subs.act_controls_1_sub = orb_subscribe(ORB_ID(actuator_controls_1)); subs.local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); subs.local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint)); subs.global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); @@ -1375,6 +1379,18 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_ATT.gy = buf.att.g_comp[1]; log_msg.body.log_ATT.gz = buf.att.g_comp[2]; LOGBUFFER_WRITE_AND_COUNT(ATT); + // secondary attitude + log_msg.msg_type = LOG_ATT2_MSG; + log_msg.body.log_ATT.roll = buf.att.roll_sec; + log_msg.body.log_ATT.pitch = buf.att.pitch_sec; + log_msg.body.log_ATT.yaw = buf.att.yaw_sec; + log_msg.body.log_ATT.roll_rate = buf.att.rollspeed_sec; + log_msg.body.log_ATT.pitch_rate = buf.att.pitchspeed_sec; + log_msg.body.log_ATT.yaw_rate = buf.att.yawspeed_sec; + log_msg.body.log_ATT.gx = buf.att.g_comp_sec[0]; + log_msg.body.log_ATT.gy = buf.att.g_comp_sec[1]; + log_msg.body.log_ATT.gz = buf.att.g_comp_sec[2]; + LOGBUFFER_WRITE_AND_COUNT(ATT); } /* --- ATTITUDE SETPOINT --- */ @@ -1413,6 +1429,15 @@ int sdlog2_thread_main(int argc, char *argv[]) LOGBUFFER_WRITE_AND_COUNT(ATTC); } + if(copy_if_updated(ORB_ID(actuator_controls_1),subs.act_controls_1_sub,&buf.act_controls1)) { + log_msg.msg_type = LOG_FWC_MSG; + log_msg.body.log_FWC.roll = buf.act_controls1.control[0]; + log_msg.body.log_FWC.pitch = buf.act_controls1.control[1]; + log_msg.body.log_FWC.yaw = buf.act_controls1.control[2]; + log_msg.body.log_FWC.thrust = buf.act_controls1.control[3]; + LOGBUFFER_WRITE_AND_COUNT(FWC); + } + /* --- LOCAL POSITION --- */ if (copy_if_updated(ORB_ID(vehicle_local_position), subs.local_pos_sub, &buf.local_pos)) { log_msg.msg_type = LOG_LPOS_MSG; diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 30491036a1..447b3fb9cb 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -50,6 +50,7 @@ #pragma pack(push, 1) /* --- ATT - ATTITUDE --- */ #define LOG_ATT_MSG 2 +#define LOG_ATT2_MSG 41 struct log_ATT_s { float roll; float pitch; @@ -422,6 +423,14 @@ struct log_ENCD_s { float vel1; }; +/* ATTITUDE CONTROLS (ACTUATOR_1 CONTROLS) */ +#define LOG_FWC_MSG 40 +struct log_FWC_s { + float roll; + float pitch; + float yaw; + float thrust; +}; /********** SYSTEM MESSAGES, ID > 0x80 **********/ @@ -450,7 +459,8 @@ struct log_PARM_s { /* construct list of all message formats */ static const struct log_format_s log_formats[] = { /* business-level messages, ID < 0x80 */ - LOG_FORMAT(ATT, "fffffffff", "Roll,Pitch,Yaw,RollRate,PitchRate,YawRate,GX,GY,GZ"), + LOG_FORMAT_S(ATT, ATT, "fffffffff", "Roll,Pitch,Yaw,RollRate,PitchRate,YawRate,GX,GY,GZ"), + LOG_FORMAT_S(ATT2, ATT, "fffffffff", "Roll,Pitch,Yaw,RollRate,PitchRate,YawRate,GX,GY,GZ"), LOG_FORMAT(ATSP, "ffff", "RollSP,PitchSP,YawSP,ThrustSP"), LOG_FORMAT_S(IMU, IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"), LOG_FORMAT_S(IMU1, IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"), @@ -460,6 +470,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(LPSP, "ffff", "X,Y,Z,Yaw"), LOG_FORMAT(GPS, "QBffLLfffffBHHH", "GPSTime,Fix,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog,nSat,SNR,N,J"), LOG_FORMAT(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"), + LOG_FORMAT(FWC,"ffff", "Roll,Pitch,Yaw,Thrust"), LOG_FORMAT(STAT, "BBBfBB", "MainState,ArmState,FailsafeState,BatRem,BatWarn,Landed"), LOG_FORMAT(RC, "ffffffffBB", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,Count,SignalLost"), LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"), From ff55652f9a9cdb4cdad216f4c6166681d10f278c Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Tue, 2 Dec 2014 10:34:39 +0100 Subject: [PATCH 086/216] adapted attitude controllers to support VTOL --- .../fw_att_control/fw_att_control_main.cpp | 160 +++++++++++++----- .../mc_att_control/mc_att_control_main.cpp | 89 ++++++++-- 2 files changed, 191 insertions(+), 58 deletions(-) diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index e770c11a27..daf7878638 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -35,8 +35,9 @@ * @file fw_att_control_main.c * Implementation of a generic attitude controller based on classic orthogonal PIDs. * - * @author Lorenz Meier - * @author Thomas Gubler + * @author Lorenz Meier + * @author Thomas Gubler + * @author Roman Bapst * */ @@ -76,6 +77,7 @@ #include #include + /** * Fixedwing attitude control app start / stop handling function * @@ -92,12 +94,12 @@ public: FixedwingAttitudeControl(); /** - * Destructor, also kills the sensors task. + * Destructor, also kills the main task. */ ~FixedwingAttitudeControl(); /** - * Start the sensors task. + * Start the main task. * * @return OK on success. */ @@ -112,9 +114,9 @@ public: private: - bool _task_should_exit; /**< if true, sensor task should exit */ + bool _task_should_exit; /**< if true, attitude control task should exit */ bool _task_running; /**< if true, task is running in its mainloop */ - int _control_task; /**< task handle for sensor task */ + int _control_task; /**< task handle */ int _att_sub; /**< vehicle attitude subscription */ int _accel_sub; /**< accelerometer subscription */ @@ -128,13 +130,16 @@ private: int _vehicle_status_sub; /**< vehicle status subscription */ orb_advert_t _rate_sp_pub; /**< rate setpoint publication */ + orb_advert_t _rate_sp_virtual_pub; /* virtual att rates setpoint topic (vtol) */ orb_advert_t _attitude_sp_pub; /**< attitude setpoint point */ orb_advert_t _actuators_0_pub; /**< actuator control group 0 setpoint */ - orb_advert_t _actuators_1_pub; /**< actuator control group 1 setpoint (Airframe) */ + orb_advert_t _actuators_virtual_fw_pub; /**publisher for VTOL vehicle*/ + orb_advert_t _actuators_2_pub; /**< actuator control group 1 setpoint (Airframe) */ struct vehicle_attitude_s _att; /**< vehicle attitude */ struct accel_report _accel; /**< body frame accelerations */ struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ + struct vehicle_rates_setpoint_s _rates_sp; /* attitude rates setpoint */ struct manual_control_setpoint_s _manual; /**< r/c channel data */ struct airspeed_s _airspeed; /**< airspeed */ struct vehicle_control_mode_s _vcontrol_mode; /**< vehicle control mode */ @@ -189,6 +194,8 @@ private: float man_roll_max; /**< Max Roll in rad */ float man_pitch_max; /**< Max Pitch in rad */ + param_t autostart_id; /* indicates which airframe is used */ + } _parameters; /**< local copies of interesting parameters */ struct { @@ -228,6 +235,8 @@ private: param_t pitchsp_offset_deg; param_t man_roll_max; param_t man_pitch_max; + + param_t autostart_id; /* indicates which airframe is used */ } _parameter_handles; /**< handles for interesting parameters */ @@ -289,7 +298,7 @@ private: static void task_main_trampoline(int argc, char *argv[]); /** - * Main sensor collection task. + * Main attitude controller collection task. */ void task_main(); @@ -325,9 +334,11 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : /* publications */ _rate_sp_pub(-1), + _rate_sp_virtual_pub(-1), _attitude_sp_pub(-1), _actuators_0_pub(-1), - _actuators_1_pub(-1), + _actuators_virtual_fw_pub(-1), + _actuators_2_pub(-1), /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "fw att control")), @@ -341,6 +352,7 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _att = {}; _accel = {}; _att_sp = {}; + _rates_sp = {}; _manual = {}; _airspeed = {}; _vcontrol_mode = {}; @@ -386,6 +398,8 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _parameter_handles.man_roll_max = param_find("FW_MAN_R_MAX"); _parameter_handles.man_pitch_max = param_find("FW_MAN_P_MAX"); + _parameter_handles.autostart_id = param_find("SYS_AUTOSTART"); + /* fetch initial parameter values */ parameters_update(); } @@ -462,6 +476,7 @@ FixedwingAttitudeControl::parameters_update() _parameters.man_roll_max = math::radians(_parameters.man_roll_max); _parameters.man_pitch_max = math::radians(_parameters.man_pitch_max); + param_get(_parameter_handles.autostart_id, &_parameters.autostart_id); /* pitch control parameters */ _pitch_ctrl.set_time_constant(_parameters.tconst); @@ -497,7 +512,7 @@ FixedwingAttitudeControl::vehicle_control_mode_poll() { bool vcontrol_mode_updated; - /* Check HIL state if vehicle status has changed */ + /* Check if vehicle control mode has changed */ orb_check(_vcontrol_mode_sub, &vcontrol_mode_updated); if (vcontrol_mode_updated) { @@ -529,7 +544,6 @@ FixedwingAttitudeControl::vehicle_airspeed_poll() if (airspeed_updated) { orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed); -// warnx("airspeed poll: ind: %.4f, true: %.4f", _airspeed.indicated_airspeed_m_s, _airspeed.true_airspeed_m_s); } } @@ -616,6 +630,18 @@ FixedwingAttitudeControl::task_main() parameters_update(); + /*Publish to correct actuator control topic, depending on what airframe we are using + * If airframe is of type VTOL then we want to publish the actuator controls on the virtual fixed wing + * topic, from which the vtol_att_control module is receiving data and processing it further)*/ + if (_parameters.autostart_id >= 13000 && _parameters.autostart_id <= 13999) { /* VTOL airframe?*/ + _actuators_virtual_fw_pub = orb_advertise(ORB_ID(actuator_controls_virtual_fw), &_actuators); + _rate_sp_virtual_pub = orb_advertise(ORB_ID(fw_virtual_rates_setpoint), &_rates_sp); + + } else { /*airframe is not of type VTOL, use standard topic for controls publication*/ + _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators); + _rate_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_rates_sp); + } + /* get an initial update for all sensor and status data */ vehicle_airspeed_poll(); vehicle_setpoint_poll(); @@ -679,6 +705,65 @@ FixedwingAttitudeControl::task_main() /* load local copies */ orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att); + if (_parameters.autostart_id >= 13000 + && _parameters.autostart_id <= 13999) { //vehicle type is VTOL, need to modify attitude! + /* The following modification to the attitude is vehicle specific and in this case applies + to tail-sitter models !!! + + * Since the VTOL airframe is initialized as a multicopter we need to + * modify the estimated attitude for the fixed wing operation. + * Since the neutral position of the vehicle in fixed wing mode is -90 degrees rotated around + * the pitch axis compared to the neutral position of the vehicle in multicopter mode + * we need to swap the roll and the yaw axis (1st and 3rd column) in the rotation matrix. + * Additionally, in order to get the correct sign of the pitch, we need to multiply + * the new x axis of the rotation matrix with -1 + * + * original: modified: + * + * Rxx Ryx Rzx -Rzx Ryx Rxx + * Rxy Ryy Rzy -Rzy Ryy Rxy + * Rxz Ryz Rzz -Rzz Ryz Rxz + * */ + math::Matrix<3, 3> R; //original rotation matrix + math::Matrix<3, 3> R_adapted; //modified rotation matrix + R.set(_att.R); + R_adapted.set(_att.R); + + //move z to x + R_adapted(0, 0) = R(0, 2); + R_adapted(1, 0) = R(1, 2); + R_adapted(2, 0) = R(2, 2); + //move x to z + R_adapted(0, 2) = R(0, 0); + R_adapted(1, 2) = R(1, 0); + R_adapted(2, 2) = R(2, 0); + + //change direction of pitch (convert to right handed system) + R_adapted(0, 0) = -R_adapted(0, 0); + R_adapted(1, 0) = -R_adapted(1, 0); + R_adapted(2, 0) = -R_adapted(2, 0); + math::Vector<3> euler_angles; //adapted euler angles for fixed wing operation + euler_angles = R_adapted.to_euler(); + //fill in new attitude data + _att.roll = euler_angles(0); + _att.pitch = euler_angles(1); + _att.yaw = euler_angles(2); + _att.R[0][0] = R_adapted(0, 0); + _att.R[0][1] = R_adapted(0, 1); + _att.R[0][2] = R_adapted(0, 2); + _att.R[1][0] = R_adapted(1, 0); + _att.R[1][1] = R_adapted(1, 1); + _att.R[1][2] = R_adapted(1, 2); + _att.R[2][0] = R_adapted(2, 0); + _att.R[2][1] = R_adapted(2, 1); + _att.R[2][2] = R_adapted(2, 2); + + // lastly, roll- and yawspeed have to be swaped + float helper = _att.rollspeed; + _att.rollspeed = -_att.yawspeed; + _att.yawspeed = helper; + } + vehicle_airspeed_poll(); vehicle_setpoint_poll(); @@ -696,7 +781,7 @@ FixedwingAttitudeControl::task_main() /* lock integrator until control is started */ bool lock_integrator; - if (_vcontrol_mode.flag_control_attitude_enabled) { + if (_vcontrol_mode.flag_control_attitude_enabled && !_vehicle_status.is_rotary_wing) { lock_integrator = false; } else { @@ -705,10 +790,10 @@ FixedwingAttitudeControl::task_main() /* Simple handling of failsafe: deploy parachute if failsafe is on */ if (_vcontrol_mode.flag_control_termination_enabled) { - _actuators_airframe.control[1] = 1.0f; + _actuators_airframe.control[7] = 1.0f; // warnx("_actuators_airframe.control[1] = 1.0f;"); } else { - _actuators_airframe.control[1] = 0.0f; + _actuators_airframe.control[7] = 0.0f; // warnx("_actuators_airframe.control[1] = -1.0f;"); } @@ -802,18 +887,18 @@ FixedwingAttitudeControl::task_main() att_sp.thrust = throttle_sp; /* lazily publish the setpoint only once available */ - if (_attitude_sp_pub > 0) { + if (_attitude_sp_pub > 0 && !_vehicle_status.is_rotary_wing) { /* publish the attitude setpoint */ orb_publish(ORB_ID(vehicle_attitude_setpoint), _attitude_sp_pub, &att_sp); - } else { + } else if (_attitude_sp_pub < 0 && !_vehicle_status.is_rotary_wing) { /* advertise and publish */ _attitude_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); } } /* If the aircraft is on ground reset the integrators */ - if (_vehicle_status.condition_landed) { + if (_vehicle_status.condition_landed || _vehicle_status.is_rotary_wing) { _roll_ctrl.reset_integrator(); _pitch_ctrl.reset_integrator(); _yaw_ctrl.reset_integrator(); @@ -915,20 +1000,19 @@ FixedwingAttitudeControl::task_main() * Lazily publish the rate setpoint (for analysis, the actuators are published below) * only once available */ - vehicle_rates_setpoint_s rates_sp; - rates_sp.roll = _roll_ctrl.get_desired_rate(); - rates_sp.pitch = _pitch_ctrl.get_desired_rate(); - rates_sp.yaw = _yaw_ctrl.get_desired_rate(); + _rates_sp.roll = _roll_ctrl.get_desired_rate(); + _rates_sp.pitch = _pitch_ctrl.get_desired_rate(); + _rates_sp.yaw = _yaw_ctrl.get_desired_rate(); - rates_sp.timestamp = hrt_absolute_time(); + _rates_sp.timestamp = hrt_absolute_time(); if (_rate_sp_pub > 0) { - /* publish the attitude setpoint */ - orb_publish(ORB_ID(vehicle_rates_setpoint), _rate_sp_pub, &rates_sp); + /* publish the attitude rates setpoint */ + orb_publish(ORB_ID(vehicle_rates_setpoint), _rate_sp_pub, &_rates_sp); - } else { - /* advertise and publish */ - _rate_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp); + } else if (_rate_sp_virtual_pub > 0) { + /* publish the virtual attitude setpoint */ + orb_publish(ORB_ID(fw_virtual_rates_setpoint), _rate_sp_virtual_pub, &_rates_sp); } } else { @@ -948,28 +1032,22 @@ FixedwingAttitudeControl::task_main() _actuators.timestamp = hrt_absolute_time(); _actuators_airframe.timestamp = hrt_absolute_time(); - if (_actuators_0_pub > 0) { - /* publish the attitude setpoint */ + /* publish the actuator controls */ + if (_actuators_0_pub > 0) { //normal fixed wing airframe orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators); - } else { - /* advertise and publish */ - _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators); + } else if (_actuators_0_pub < 0 && !_vehicle_status.is_rotary_wing) { //VTOL airframe + orb_publish(ORB_ID(actuator_controls_virtual_fw), _actuators_virtual_fw_pub, &_actuators); } - if (_actuators_1_pub > 0) { - /* publish the attitude setpoint */ - orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, &_actuators_airframe); -// warnx("%.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f", -// (double)_actuators_airframe.control[0], (double)_actuators_airframe.control[1], (double)_actuators_airframe.control[2], -// (double)_actuators_airframe.control[3], (double)_actuators_airframe.control[4], (double)_actuators_airframe.control[5], -// (double)_actuators_airframe.control[6], (double)_actuators_airframe.control[7]); + if (_actuators_2_pub > 0) { + /* publish the actuator controls*/ + orb_publish(ORB_ID(actuator_controls_2), _actuators_2_pub, &_actuators_airframe); } else { /* advertise and publish */ - _actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators_airframe); + _actuators_2_pub = orb_advertise(ORB_ID(actuator_controls_2), &_actuators_airframe); } - } loop_counter++; diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 81a13edb8f..c5dccd8c39 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -66,6 +66,7 @@ #include #include #include +#include #include #include #include @@ -96,12 +97,12 @@ public: MulticopterAttitudeControl(); /** - * Destructor, also kills the sensors task. + * Destructor, also kills the main task */ ~MulticopterAttitudeControl(); /** - * Start the sensors task. + * Start the multicopter attitude control task. * * @return OK on success. */ @@ -109,8 +110,8 @@ public: private: - bool _task_should_exit; /**< if true, sensor task should exit */ - int _control_task; /**< task handle for sensor task */ + bool _task_should_exit; /**< if true, task_main() should exit */ + int _control_task; /**< task handle */ int _v_att_sub; /**< vehicle attitude subscription */ int _v_att_sp_sub; /**< vehicle attitude setpoint subscription */ @@ -119,10 +120,13 @@ private: int _params_sub; /**< parameter updates subscription */ int _manual_control_sp_sub; /**< manual control setpoint subscription */ int _armed_sub; /**< arming status subscription */ + int _vehicle_status_sub; /**< vehicle status subscription */ orb_advert_t _att_sp_pub; /**< attitude setpoint publication */ orb_advert_t _v_rates_sp_pub; /**< rate setpoint publication */ + orb_advert_t _v_rates_sp_virtual_pub; /* virtual att rates sp publication */ orb_advert_t _actuators_0_pub; /**< attitude actuator controls publication */ + orb_advert_t _actuators_virtual_mc_pub; /**attitude actuator controls publication if vehicle is VTOL*/ bool _actuators_0_circuit_breaker_enabled; /**< circuit breaker to suppress output */ @@ -133,6 +137,7 @@ private: struct vehicle_control_mode_s _v_control_mode; /**< vehicle control mode */ struct actuator_controls_s _actuators; /**< actuator controls */ struct actuator_armed_s _armed; /**< actuator arming status */ + struct vehicle_status_s _vehicle_status; /**< vehicle status */ perf_counter_t _loop_perf; /**< loop performance counter */ @@ -168,6 +173,8 @@ private: param_t acro_roll_max; param_t acro_pitch_max; param_t acro_yaw_max; + + param_t autostart_id; //what frame are we using? } _params_handles; /**< handles for interesting parameters */ struct { @@ -182,6 +189,8 @@ private: float man_pitch_max; float man_yaw_max; math::Vector<3> acro_rate_max; /**< max attitude rates in acro mode */ + + param_t autostart_id; } _params; /** @@ -229,6 +238,11 @@ private: */ void control_attitude_rates(float dt); + /** + * Check for vehicle status updates. + */ + void vehicle_status_poll(); + /** * Shim for calling task_main from task_create. */ @@ -264,11 +278,14 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _params_sub(-1), _manual_control_sp_sub(-1), _armed_sub(-1), + _vehicle_status_sub(-1), /* publications */ _att_sp_pub(-1), _v_rates_sp_pub(-1), + _v_rates_sp_virtual_pub(-1), _actuators_0_pub(-1), + _actuators_virtual_mc_pub(-1), _actuators_0_circuit_breaker_enabled(false), @@ -283,6 +300,8 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : memset(&_v_control_mode, 0, sizeof(_v_control_mode)); memset(&_actuators, 0, sizeof(_actuators)); memset(&_armed, 0, sizeof(_armed)); + memset(&_vehicle_status, 0, sizeof(_vehicle_status)); + _vehicle_status.is_rotary_wing = true; _params.att_p.zero(); _params.rate_p.zero(); @@ -295,6 +314,8 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _params.man_yaw_max = 0.0f; _params.acro_rate_max.zero(); + _params.autostart_id = 0; //default + _rates_prev.zero(); _rates_sp.zero(); _rates_int.zero(); @@ -324,6 +345,8 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _params_handles.acro_pitch_max = param_find("MC_ACRO_P_MAX"); _params_handles.acro_yaw_max = param_find("MC_ACRO_Y_MAX"); + _params_handles.autostart_id = param_find("SYS_AUTOSTART"); + /* fetch initial parameter values */ parameters_update(); } @@ -409,6 +432,8 @@ MulticopterAttitudeControl::parameters_update() _actuators_0_circuit_breaker_enabled = circuit_breaker_enabled("CBRK_RATE_CTRL", CBRK_RATE_CTRL_KEY); + param_get(_params_handles.autostart_id, &_params.autostart_id); + return OK; } @@ -417,7 +442,7 @@ MulticopterAttitudeControl::parameter_update_poll() { bool updated; - /* Check HIL state if vehicle status has changed */ + /* Check if parameters have changed */ orb_check(_params_sub, &updated); if (updated) { @@ -432,7 +457,7 @@ MulticopterAttitudeControl::vehicle_control_mode_poll() { bool updated; - /* Check HIL state if vehicle status has changed */ + /* Check if vehicle control mode has changed */ orb_check(_v_control_mode_sub, &updated); if (updated) { @@ -489,6 +514,18 @@ MulticopterAttitudeControl::arming_status_poll() } } +void +MulticopterAttitudeControl::vehicle_status_poll() +{ + /* check if there is new status information */ + bool vehicle_status_updated; + orb_check(_vehicle_status_sub, &vehicle_status_updated); + + if (vehicle_status_updated) { + orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status); + } +} + /* * Attitude controller. * Input: 'manual_control_setpoint' and 'vehicle_attitude_setpoint' topics (depending on mode) @@ -585,7 +622,7 @@ MulticopterAttitudeControl::control_attitude(float dt) } /* publish the attitude setpoint if needed */ - if (publish_att_sp) { + if (publish_att_sp && _vehicle_status.is_rotary_wing) { _v_att_sp.timestamp = hrt_absolute_time(); if (_att_sp_pub > 0) { @@ -682,7 +719,7 @@ void MulticopterAttitudeControl::control_attitude_rates(float dt) { /* reset integral if disarmed */ - if (!_armed.armed) { + if (!_armed.armed || !_vehicle_status.is_rotary_wing) { _rates_int.zero(); } @@ -721,6 +758,8 @@ MulticopterAttitudeControl::task_main_trampoline(int argc, char *argv[]) void MulticopterAttitudeControl::task_main() { + warnx("started"); + fflush(stdout); /* * do subscriptions @@ -732,10 +771,24 @@ MulticopterAttitudeControl::task_main() _params_sub = orb_subscribe(ORB_ID(parameter_update)); _manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); _armed_sub = orb_subscribe(ORB_ID(actuator_armed)); + _vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); /* initialize parameters cache */ parameters_update(); + /*Subscribe to correct actuator control topic, depending on what airframe we are using + * If airframe is of type VTOL then we want to publish the actuator controls on the virtual multicopter + * topic, from which the VTOL_att_control module is receiving data and processing it further)*/ + if (_params.autostart_id >= 13000 && _params.autostart_id <= 13999) { /* VTOL airframe?*/ + _actuators_virtual_mc_pub = orb_advertise(ORB_ID(actuator_controls_virtual_mc), &_actuators); + _v_rates_sp_virtual_pub = orb_advertise(ORB_ID(mc_virtual_rates_setpoint), &_v_rates_sp); + + } else { /*airframe is not of type VTOL, use standard topic for controls publication*/ + _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators); + _v_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_v_rates_sp); + } + + /* wakeup source: vehicle attitude */ struct pollfd fds[1]; @@ -783,6 +836,7 @@ MulticopterAttitudeControl::task_main() vehicle_control_mode_poll(); arming_status_poll(); vehicle_manual_poll(); + vehicle_status_poll(); if (_v_control_mode.flag_control_attitude_enabled) { control_attitude(dt); @@ -797,8 +851,8 @@ MulticopterAttitudeControl::task_main() if (_v_rates_sp_pub > 0) { orb_publish(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_pub, &_v_rates_sp); - } else { - _v_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_v_rates_sp); + } else if (_v_rates_sp_virtual_pub > 0) { + _v_rates_sp_pub = orb_publish(ORB_ID(mc_virtual_rates_setpoint), _v_rates_sp_virtual_pub, &_v_rates_sp); } } else { @@ -819,11 +873,11 @@ MulticopterAttitudeControl::task_main() _v_rates_sp.timestamp = hrt_absolute_time(); if (_v_rates_sp_pub > 0) { - orb_publish(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_pub, &_v_rates_sp); + orb_publish(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_pub, &_v_rates_sp); - } else { - _v_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_v_rates_sp); - } + } else if (_v_rates_sp_virtual_pub > 0) { + _v_rates_sp_pub = orb_publish(ORB_ID(mc_virtual_rates_setpoint), _v_rates_sp_virtual_pub, &_v_rates_sp); + } } else { /* attitude controller disabled, poll rates setpoint topic */ @@ -846,12 +900,13 @@ MulticopterAttitudeControl::task_main() _actuators.timestamp = hrt_absolute_time(); if (!_actuators_0_circuit_breaker_enabled) { - if (_actuators_0_pub > 0) { + if (_actuators_0_pub > 0) { //normal mutlicopter airframe orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators); - } else { - _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators); + } else if (_actuators_0_pub < 0 && _vehicle_status.is_rotary_wing) { + orb_publish(ORB_ID(actuator_controls_virtual_mc), _actuators_virtual_mc_pub, &_actuators); } + } } } From 285a0e7be9c6227e0353e356cbbd6f48e54d057e Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Tue, 2 Dec 2014 10:36:30 +0100 Subject: [PATCH 087/216] added more mixer geometries and took v-mixer out of multi_tables script --- src/modules/systemlib/mixer/mixer.h | 1 + .../systemlib/mixer/mixer_multirotor.cpp | 19 +++++++++++++++---- src/modules/systemlib/mixer/multi_tables | 11 ++++------- 3 files changed, 20 insertions(+), 11 deletions(-) diff --git a/src/modules/systemlib/mixer/mixer.h b/src/modules/systemlib/mixer/mixer.h index 17989558e6..f14bdb9fdf 100644 --- a/src/modules/systemlib/mixer/mixer.h +++ b/src/modules/systemlib/mixer/mixer.h @@ -466,6 +466,7 @@ public: OCTA_X, OCTA_PLUS, OCTA_COX, + TWIN_ENGINE, /**< VTOL: one engine on each wing */ MAX_GEOMETRY }; diff --git a/src/modules/systemlib/mixer/mixer_multirotor.cpp b/src/modules/systemlib/mixer/mixer_multirotor.cpp index 57e17b67d2..24187c9bc5 100644 --- a/src/modules/systemlib/mixer/mixer_multirotor.cpp +++ b/src/modules/systemlib/mixer/mixer_multirotor.cpp @@ -76,6 +76,7 @@ float constrain(float val, float min, float max) /* * These tables automatically generated by multi_tables - do not edit. */ + const MultirotorMixer::Rotor _config_quad_x[] = { { -0.707107, 0.707107, 1.00 }, { 0.707107, -0.707107, 1.00 }, @@ -88,11 +89,12 @@ const MultirotorMixer::Rotor _config_quad_plus[] = { { 0.000000, 1.000000, -1.00 }, { -0.000000, -1.000000, -1.00 }, }; +//Add table for quad in V configuration, which is not generated by multi_tables! const MultirotorMixer::Rotor _config_quad_v[] = { - { -0.927184, 0.374607, 1.00 }, - { 0.694658, -0.719340, 1.00 }, - { 0.927184, 0.374607, -1.00 }, - { -0.694658, -0.719340, -1.00 }, + { -0.3223, 0.9466, 0.4242 }, + { 0.3223, -0.9466, 1.0000 }, + { 0.3223, 0.9466, -0.4242 }, + { -0.3223, -0.9466, -1.0000 }, }; const MultirotorMixer::Rotor _config_quad_wide[] = { { -0.927184, 0.374607, 1.00 }, @@ -154,6 +156,11 @@ const MultirotorMixer::Rotor _config_octa_cox[] = { { -0.707107, -0.707107, 1.00 }, { 0.707107, -0.707107, -1.00 }, }; +const MultirotorMixer::Rotor _config_duorotor[] = { + { -1.000000, 0.000000, 0.00 }, + { 1.000000, 0.000000, 0.00 }, +}; + const MultirotorMixer::Rotor *_config_index[MultirotorMixer::MAX_GEOMETRY] = { &_config_quad_x[0], &_config_quad_plus[0], @@ -165,6 +172,7 @@ const MultirotorMixer::Rotor *_config_index[MultirotorMixer::MAX_GEOMETRY] = { &_config_octa_x[0], &_config_octa_plus[0], &_config_octa_cox[0], + &_config_duorotor[0], }; const unsigned _config_rotor_count[MultirotorMixer::MAX_GEOMETRY] = { 4, /* quad_x */ @@ -177,6 +185,7 @@ const unsigned _config_rotor_count[MultirotorMixer::MAX_GEOMETRY] = { 8, /* octa_x */ 8, /* octa_plus */ 8, /* octa_cox */ + 2, /* twin_engine */ }; } @@ -274,6 +283,8 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl } else if (!strcmp(geomname, "8c")) { geometry = MultirotorMixer::OCTA_COX; + } else if (!strcmp(geomname, "2-")) { + geometry = MultirotorMixer::TWIN_ENGINE; } else { debug("unrecognised geometry '%s'", geomname); return nullptr; diff --git a/src/modules/systemlib/mixer/multi_tables b/src/modules/systemlib/mixer/multi_tables index b5698036e1..18c8285786 100755 --- a/src/modules/systemlib/mixer/multi_tables +++ b/src/modules/systemlib/mixer/multi_tables @@ -6,6 +6,7 @@ proc rad {a} { expr ($a / 360.0) * 2 * acos(-1) } proc rcos {a} { expr cos([rad $a])} + set quad_x { 45 CCW -135 CCW @@ -20,12 +21,6 @@ set quad_plus { 180 CW } -set quad_v { - 68 CCW - -136 CCW - -68 CW - 136 CW -} set quad_wide { 68 CCW @@ -94,7 +89,9 @@ set octa_cox { -135 CW } -set tables {quad_x quad_plus quad_v quad_wide hex_x hex_plus hex_cox octa_x octa_plus octa_cox} + +set tables {quad_x quad_plus quad_wide hex_x hex_plus hex_cox octa_x octa_plus octa_cox} + proc factors {a d} { puts [format "\t{ %9.6f, %9.6f, %5.2f }," [rcos [expr $a + 90]] [rcos $a] [expr -$d]]} From 15ad9e441ef87b3e917a057e2f4b24d4bf3d0cee Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Tue, 2 Dec 2014 10:38:39 +0100 Subject: [PATCH 088/216] compute secondary attitude with reference frame rotated -90 degress around pitch axis of original reference frame (used for VTOL) --- .../attitude_estimator_ekf_main.cpp | 38 +++++++++++++++++++ 1 file changed, 38 insertions(+) diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index e2dbc30dd5..6efcbc7f96 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -552,6 +552,44 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds memcpy(&att.R[0][0], &R.data[0][0], sizeof(att.R)); att.R_valid = true; + // compute secondary attitude + math::Matrix<3, 3> R_adapted; //modified rotation matrix + R_adapted = R; + + //move z to x + R_adapted(0, 0) = R(0, 2); + R_adapted(1, 0) = R(1, 2); + R_adapted(2, 0) = R(2, 2); + //move x to z + R_adapted(0, 2) = R(0, 0); + R_adapted(1, 2) = R(1, 0); + R_adapted(2, 2) = R(2, 0); + + //change direction of pitch (convert to right handed system) + R_adapted(0, 0) = -R_adapted(0, 0); + R_adapted(1, 0) = -R_adapted(1, 0); + R_adapted(2, 0) = -R_adapted(2, 0); + math::Vector<3> euler_angles_sec; //adapted euler angles for fixed wing operation + euler_angles_sec = R_adapted.to_euler(); + + att.roll_sec = euler_angles_sec(0); + att.pitch_sec = euler_angles_sec(1); + att.yaw_sec = euler_angles_sec(2); + + memcpy(&att.R_sec[0][0], &R_adapted.data[0][0], sizeof(att.R_sec)); + + att.rollspeed_sec = -x_aposteriori[2]; + att.pitchspeed_sec = x_aposteriori[1]; + att.yawspeed_sec = x_aposteriori[0]; + att.rollacc_sec = -x_aposteriori[5]; + att.pitchacc_sec = x_aposteriori[4]; + att.yawacc_sec = x_aposteriori[3]; + + att.g_comp_sec[0] = -raw.accelerometer_m_s2[2] - (-acc(2)); + att.g_comp_sec[1] = raw.accelerometer_m_s2[1] - acc(1); + att.g_comp_sec[2] = raw.accelerometer_m_s2[0] - acc(0); + + if (isfinite(att.roll) && isfinite(att.pitch) && isfinite(att.yaw)) { // Broadcast orb_publish(ORB_ID(vehicle_attitude), pub_att, &att); From 9a64004d7ff60d0c4ca25877b34183631636112a Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Tue, 2 Dec 2014 10:47:07 +0100 Subject: [PATCH 089/216] let commander know if VTOL is in fw or in mc mode (important because of external_override) --- src/modules/commander/commander.cpp | 41 ++++++++++++++++++++++++++--- 1 file changed, 38 insertions(+), 3 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index c9c8d16b50..bed4676989 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -82,6 +82,7 @@ #include #include #include +#include #include #include @@ -149,6 +150,9 @@ enum MAV_MODE_FLAG { /* Mavlink file descriptors */ static int mavlink_fd = 0; +/* Syste autostart ID */ +static int autostart_id; + /* flags */ static bool commander_initialized = false; static volatile bool thread_should_exit = false; /**< daemon exit flag */ @@ -732,6 +736,7 @@ int commander_thread_main(int argc, char *argv[]) param_t _param_ef_throttle_thres = param_find("COM_EF_THROT"); param_t _param_ef_current2throttle_thres = param_find("COM_EF_C2T"); param_t _param_ef_time_thres = param_find("COM_EF_TIME"); + param_t _param_autostart_id = param_find("SYS_AUTOSTART"); /* welcome user */ warnx("starting"); @@ -1014,6 +1019,13 @@ int commander_thread_main(int argc, char *argv[]) struct actuator_controls_s actuator_controls; memset(&actuator_controls, 0, sizeof(actuator_controls)); + /* Subscribe to vtol vehicle status topic */ + int vtol_vehicle_status_sub = orb_subscribe(ORB_ID(vtol_vehicle_status)); + struct vtol_vehicle_status_s vtol_status; + memset(&vtol_status, 0, sizeof(vtol_status)); + vtol_status.vtol_in_rw_mode = true; //default for vtol is rotary wing + + control_status_leds(&status, &armed, true); /* now initialized */ @@ -1070,7 +1082,10 @@ int commander_thread_main(int argc, char *argv[]) status.system_type == VEHICLE_TYPE_TRICOPTER || status.system_type == VEHICLE_TYPE_QUADROTOR || status.system_type == VEHICLE_TYPE_HEXAROTOR || - status.system_type == VEHICLE_TYPE_OCTOROTOR) { + status.system_type == VEHICLE_TYPE_OCTOROTOR || + (status.system_type == VEHICLE_TYPE_VTOL_DUOROTOR && vtol_status.vtol_in_rw_mode) || + (status.system_type == VEHICLE_TYPE_VTOL_QUADROTOR && vtol_status.vtol_in_rw_mode)) { + status.is_rotary_wing = true; } else { @@ -1106,6 +1121,7 @@ int commander_thread_main(int argc, char *argv[]) param_get(_param_ef_throttle_thres, &ef_throttle_thres); param_get(_param_ef_current2throttle_thres, &ef_current2throttle_thres); param_get(_param_ef_time_thres, &ef_time_thres); + param_get(_param_autostart_id, &autostart_id); } orb_check(sp_man_sub, &updated); @@ -1231,6 +1247,19 @@ int commander_thread_main(int argc, char *argv[]) } } + /* update vtol vehicle status*/ + orb_check(vtol_vehicle_status_sub, &updated); + + if (updated) { + /* vtol status changed */ + orb_copy(ORB_ID(vtol_vehicle_status), vtol_vehicle_status_sub, &vtol_status); + + /* Make sure that this is only adjusted if vehicle realy is of type vtol*/ + if (status.system_type == VEHICLE_TYPE_VTOL_DUOROTOR || VEHICLE_TYPE_VTOL_QUADROTOR) { + status.is_rotary_wing = vtol_status.vtol_in_rw_mode; + } + } + /* update global position estimate */ orb_check(global_position_sub, &updated); @@ -2189,7 +2218,13 @@ set_control_mode() /* set vehicle_control_mode according to set_navigation_state */ control_mode.flag_armed = armed.armed; /* TODO: check this */ - control_mode.flag_external_manual_override_ok = !status.is_rotary_wing; + if (autostart_id < 13000 || autostart_id >= 14000) { + control_mode.flag_external_manual_override_ok = !status.is_rotary_wing; + + } else { + control_mode.flag_external_manual_override_ok = false; + } + control_mode.flag_system_hil_enabled = status.hil_state == HIL_STATE_ON; control_mode.flag_control_offboard_enabled = false; @@ -2205,7 +2240,7 @@ set_control_mode() control_mode.flag_control_velocity_enabled = false; control_mode.flag_control_termination_enabled = false; break; - + case NAVIGATION_STATE_ALTCTL: control_mode.flag_control_manual_enabled = true; control_mode.flag_control_auto_enabled = false; From 0dc202502de8e879f427c94ce0c415f8a558c2fb Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Tue, 2 Dec 2014 10:49:19 +0100 Subject: [PATCH 090/216] added VTOL geometries to determine number of motors --- src/modules/mavlink/mavlink_messages.cpp | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index a8f956ad05..a68381f9f8 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1358,7 +1358,10 @@ protected: /* scale outputs depending on system type */ if (system_type == MAV_TYPE_QUADROTOR || system_type == MAV_TYPE_HEXAROTOR || - system_type == MAV_TYPE_OCTOROTOR) { + system_type == MAV_TYPE_OCTOROTOR || + system_type == MAV_TYPE_VTOL_DUOROTOR || + system_type == MAV_TYPE_VTOL_QUADROTOR) { + /* multirotors: set number of rotor outputs depending on type */ unsigned n; @@ -1372,6 +1375,14 @@ protected: n = 6; break; + case MAV_TYPE_VTOL_DUOROTOR: + n = 2; + break; + + case MAV_TYPE_VTOL_QUADROTOR: + n = 4; + break; + default: n = 8; break; From 887ed69099e43d7f6f3852c11450cd9ef73c2d3b Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Tue, 2 Dec 2014 10:50:25 +0100 Subject: [PATCH 091/216] added VTOL attitude control module --- src/modules/vtol_att_control/module.mk | 40 + .../vtol_att_control_main.cpp | 716 ++++++++++++++++++ 2 files changed, 756 insertions(+) create mode 100644 src/modules/vtol_att_control/module.mk create mode 100644 src/modules/vtol_att_control/vtol_att_control_main.cpp diff --git a/src/modules/vtol_att_control/module.mk b/src/modules/vtol_att_control/module.mk new file mode 100644 index 0000000000..c349c23401 --- /dev/null +++ b/src/modules/vtol_att_control/module.mk @@ -0,0 +1,40 @@ +############################################################################ +# +# Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# VTOL attitude controller +# + +MODULE_COMMAND = vtol_att_control + +SRCS = vtol_att_control_main.cpp diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp new file mode 100644 index 0000000000..62b6a4ed6f --- /dev/null +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -0,0 +1,716 @@ +/**************************************************************************** + * + * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file VTOL_att_control_main.cpp + * Implementation of an attitude controller for VTOL airframes. This module receives data + * from both the fixed wing- and the multicopter attitude controllers and processes it. + * It computes the correct actuator controls depending on which mode the vehicle is in (hover,forward- + * flight or transition). It also publishes the resulting controls on the actuator controls topics. + * + * @author Roman Bapst + * @author Lorenz Meier + * @author Thomas Gubler + * + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "drivers/drv_pwm_output.h" +#include +#include + +#include + +#include + + +extern "C" __EXPORT int vtol_att_control_main(int argc, char *argv[]); + +class VtolAttitudeControl +{ +public: + + VtolAttitudeControl(); + ~VtolAttitudeControl(); + + int start(); /* start the task and return OK on success */ + + +private: +//******************flags & handlers****************************************************** + bool _task_should_exit; + int _control_task; //task handle for VTOL attitude controller + + /* handlers for subscriptions */ + int _v_att_sub; //vehicle attitude subscription + int _v_att_sp_sub; //vehicle attitude setpoint subscription + int _mc_virtual_v_rates_sp_sub; //vehicle rates setpoint subscription + int _fw_virtual_v_rates_sp_sub; //vehicle rates setpoint subscription + int _v_control_mode_sub; //vehicle control mode subscription + int _params_sub; //parameter updates subscription + int _manual_control_sp_sub; //manual control setpoint subscription + int _armed_sub; //arming status subscription + + int _actuator_inputs_mc; //topic on which the mc_att_controller publishes actuator inputs + int _actuator_inputs_fw; //topic on which the fw_att_controller publishes actuator inputs + + //handlers for publishers + orb_advert_t _actuators_0_pub; //input for the mixer (roll,pitch,yaw,thrust) + orb_advert_t _actuators_1_pub; + orb_advert_t _vtol_vehicle_status_pub; + orb_advert_t _v_rates_sp_pub; +//*******************data containers*********************************************************** + struct vehicle_attitude_s _v_att; //vehicle attitude + struct vehicle_attitude_setpoint_s _v_att_sp; //vehicle attitude setpoint + struct vehicle_rates_setpoint_s _v_rates_sp; //vehicle rates setpoint + struct vehicle_rates_setpoint_s _mc_virtual_v_rates_sp; // virtual mc vehicle rates setpoint + struct vehicle_rates_setpoint_s _fw_virtual_v_rates_sp; // virtual fw vehicle rates setpoint + struct manual_control_setpoint_s _manual_control_sp; //manual control setpoint + struct vehicle_control_mode_s _v_control_mode; //vehicle control mode + struct vtol_vehicle_status_s _vtol_vehicle_status; + struct actuator_controls_s _actuators_out_0; //actuator controls going to the mc mixer + struct actuator_controls_s _actuators_out_1; //actuator controls going to the fw mixer (used for elevons) + struct actuator_controls_s _actuators_mc_in; //actuator controls from mc_att_control + struct actuator_controls_s _actuators_fw_in; //actuator controls from fw_att_control + struct actuator_armed_s _armed; //actuator arming status + + struct { + float min_pwm_mc; //pwm value for idle in mc mode + } _params; + + struct { + param_t min_pwm_mc; + } _params_handles; + + perf_counter_t _loop_perf; /**< loop performance counter */ + + /* for multicopters it is usual to have a non-zero idle speed of the engines + * for fixed wings we want to have an idle speed of zero since we do not want + * to waste energy when gliding. */ + bool flag_idle_mc; //false = "idle is set for fixed wing mode"; true = "idle is set for multicopter mode" + +//*****************Member functions*********************************************************************** + + void task_main(); //main task + static void task_main_trampoline(int argc, char *argv[]); //Shim for calling task_main from task_create. + + void vehicle_control_mode_poll(); //Check for changes in vehicle control mode. + void vehicle_manual_poll(); //Check for changes in manual inputs. + void arming_status_poll(); //Check for arming status updates. + void actuator_controls_mc_poll(); //Check for changes in mc_attitude_control output + void actuator_controls_fw_poll(); //Check for changes in fw_attitude_control output + void vehicle_rates_sp_mc_poll(); + void vehicle_rates_sp_fw_poll(); + void parameters_update_poll(); //Check if parameters have changed + int parameters_update(); //Update local paraemter cache + void fill_mc_att_control_output(); //write mc_att_control results to actuator message + void fill_fw_att_control_output(); //write fw_att_control results to actuator message + void fill_mc_att_rates_sp(); + void fill_fw_att_rates_sp(); + void set_idle_fw(); + void set_idle_mc(); +}; + +namespace VTOL_att_control +{ +VtolAttitudeControl *g_control; +} + +/** +* Constructor +*/ +VtolAttitudeControl::VtolAttitudeControl() : + _task_should_exit(false), + _control_task(-1), + + //init subscription handlers + _v_att_sub(-1), + _v_att_sp_sub(-1), + _mc_virtual_v_rates_sp_sub(-1), + _fw_virtual_v_rates_sp_sub(-1), + _v_control_mode_sub(-1), + _params_sub(-1), + _manual_control_sp_sub(-1), + _armed_sub(-1), + + //init publication handlers + _actuators_0_pub(-1), + _actuators_1_pub(-1), + _vtol_vehicle_status_pub(-1), + _v_rates_sp_pub(-1), + + _loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control")) +{ + + flag_idle_mc = true; /*assume we always start in mc mode for a VTOL airframe */ + + memset(& _vtol_vehicle_status, 0, sizeof(_vtol_vehicle_status)); + _vtol_vehicle_status.vtol_in_rw_mode = true; /* start vtol in rotary wing mode*/ + memset(&_v_att, 0, sizeof(_v_att)); + memset(&_v_att_sp, 0, sizeof(_v_att_sp)); + memset(&_v_rates_sp, 0, sizeof(_v_rates_sp)); + memset(&_mc_virtual_v_rates_sp, 0, sizeof(_mc_virtual_v_rates_sp)); + memset(&_fw_virtual_v_rates_sp, 0, sizeof(_fw_virtual_v_rates_sp)); + memset(&_manual_control_sp, 0, sizeof(_manual_control_sp)); + memset(&_v_control_mode, 0, sizeof(_v_control_mode)); + memset(&_vtol_vehicle_status, 0, sizeof(_vtol_vehicle_status)); + memset(&_actuators_out_0, 0, sizeof(_actuators_out_0)); + memset(&_actuators_out_1, 0, sizeof(_actuators_out_1)); + memset(&_actuators_mc_in, 0, sizeof(_actuators_mc_in)); + memset(&_actuators_fw_in, 0, sizeof(_actuators_fw_in)); + memset(&_armed, 0, sizeof(_armed)); + + _params_handles.min_pwm_mc = param_find("PWM_MIN"); + + /* fetch initial parameter values */ + parameters_update(); +} + +/** +* Destructor +*/ +VtolAttitudeControl::~VtolAttitudeControl() +{ + if (_control_task != -1) { + /* task wakes up every 100ms or so at the longest */ + _task_should_exit = true; + + /* wait for a second for the task to quit at our request */ + unsigned i = 0; + + do { + /* wait 20ms */ + usleep(20000); + + /* if we have given up, kill it */ + if (++i > 50) { + task_delete(_control_task); + break; + } + } while (_control_task != -1); + } + + VTOL_att_control::g_control = nullptr; +} + +/** +* Check for changes in vehicle control mode. +*/ +void VtolAttitudeControl::vehicle_control_mode_poll() +{ + bool updated; + + /* Check if vehicle control mode has changed */ + orb_check(_v_control_mode_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(vehicle_control_mode), _v_control_mode_sub, &_v_control_mode); + } +} + +/** +* Check for changes in manual inputs. +*/ +void VtolAttitudeControl::vehicle_manual_poll() +{ + bool updated; + + /* get pilots inputs */ + orb_check(_manual_control_sp_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(manual_control_setpoint), _manual_control_sp_sub, &_manual_control_sp); + } +} +/** +* Check for arming status updates. +*/ +void VtolAttitudeControl::arming_status_poll() +{ + /* check if there is a new setpoint */ + bool updated; + orb_check(_armed_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed); + } +} + +/** +* Check for inputs from mc attitude controller. +*/ +void VtolAttitudeControl::actuator_controls_mc_poll() +{ + bool updated; + orb_check(_actuator_inputs_mc, &updated); + + if (updated) { + orb_copy(ORB_ID(actuator_controls_virtual_mc), _actuator_inputs_mc , &_actuators_mc_in); + } +} + +/** +* Check for inputs from fw attitude controller. +*/ +void VtolAttitudeControl::actuator_controls_fw_poll() +{ + bool updated; + orb_check(_actuator_inputs_fw, &updated); + + if (updated) { + orb_copy(ORB_ID(actuator_controls_virtual_fw), _actuator_inputs_fw , &_actuators_fw_in); + } +} + +/** +* Check for attitude rates setpoint from mc attitude controller +*/ +void VtolAttitudeControl::vehicle_rates_sp_mc_poll() +{ + bool updated; + orb_check(_mc_virtual_v_rates_sp_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(mc_virtual_rates_setpoint), _mc_virtual_v_rates_sp_sub , &_mc_virtual_v_rates_sp); + } +} + +/** +* Check for attitude rates setpoint from fw attitude controller +*/ +void VtolAttitudeControl::vehicle_rates_sp_fw_poll() +{ + bool updated; + orb_check(_fw_virtual_v_rates_sp_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(fw_virtual_rates_setpoint), _fw_virtual_v_rates_sp_sub , &_fw_virtual_v_rates_sp); + } +} + +/** +* Check for parameter updates. +*/ +void +VtolAttitudeControl::parameters_update_poll() +{ + bool updated; + + /* Check if parameters have changed */ + orb_check(_params_sub, &updated); + + if (updated) { + struct parameter_update_s param_update; + orb_copy(ORB_ID(parameter_update), _params_sub, ¶m_update); + parameters_update(); + } +} + +/** +* Update parameters. +*/ +int +VtolAttitudeControl::parameters_update() +{ + /* idle pwm */ + float v; + param_get(_params_handles.min_pwm_mc, &v); + _params.min_pwm_mc = v; + + return OK; +} + +/** +* Prepare message to acutators with data from mc attitude controller. +*/ +void VtolAttitudeControl::fill_mc_att_control_output() +{ + _actuators_out_0.control[0] = _actuators_mc_in.control[0]; + _actuators_out_0.control[1] = _actuators_mc_in.control[1]; + _actuators_out_0.control[2] = _actuators_mc_in.control[2]; + _actuators_out_0.control[3] = _actuators_mc_in.control[3]; + //set neutral position for elevons + _actuators_out_1.control[0] = _actuators_mc_in.control[2]; //roll elevon + _actuators_out_1.control[1] = _actuators_mc_in.control[1];; //pitch elevon +} + +/** +* Prepare message to acutators with data from fw attitude controller. +*/ +void VtolAttitudeControl::fill_fw_att_control_output() +{ + /*For the first test in fw mode, only use engines for thrust!!!*/ + _actuators_out_0.control[0] = 0; + _actuators_out_0.control[1] = 0; + _actuators_out_0.control[2] = 0; + _actuators_out_0.control[3] = _actuators_fw_in.control[3]; + /*controls for the elevons */ + _actuators_out_1.control[0] = -_actuators_fw_in.control[0]; /*roll elevon*/ + _actuators_out_1.control[1] = _actuators_fw_in.control[1]; /*pitch elevon */ +} + +/** +* Prepare message for mc attitude rates setpoint topic +*/ +void VtolAttitudeControl::fill_mc_att_rates_sp() +{ + _v_rates_sp.roll = _mc_virtual_v_rates_sp.roll; + _v_rates_sp.pitch = _mc_virtual_v_rates_sp.pitch; + _v_rates_sp.yaw = _mc_virtual_v_rates_sp.yaw; + _v_rates_sp.thrust = _mc_virtual_v_rates_sp.thrust; +} + +/** +* Prepare message for fw attitude rates setpoint topic +*/ +void VtolAttitudeControl::fill_fw_att_rates_sp() +{ + _v_rates_sp.roll = _fw_virtual_v_rates_sp.roll; + _v_rates_sp.pitch = _fw_virtual_v_rates_sp.pitch; + _v_rates_sp.yaw = _fw_virtual_v_rates_sp.yaw; + _v_rates_sp.thrust = _fw_virtual_v_rates_sp.thrust; +} + +/** +* Adjust idle speed for fw mode. +*/ +void VtolAttitudeControl::set_idle_fw() +{ + int ret; + char *dev = PWM_OUTPUT_DEVICE_PATH; + int fd = open(dev, 0); + + if (fd < 0) {err(1, "can't open %s", dev);} + + unsigned pwm_value = PWM_LOWEST_MIN; + struct pwm_output_values pwm_values; + memset(&pwm_values, 0, sizeof(pwm_values)); + + for (unsigned i = 0; i < 4; i++) { + + pwm_values.values[i] = pwm_value; + pwm_values.channel_count++; + } + + ret = ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values); + + if (ret != OK) {errx(ret, "failed setting min values");} + + close(fd); +} + +/** +* Adjust idle speed for mc mode. +*/ +void VtolAttitudeControl::set_idle_mc() +{ + int ret; + unsigned servo_count; + char *dev = PWM_OUTPUT_DEVICE_PATH; + int fd = open(dev, 0); + + if (fd < 0) {err(1, "can't open %s", dev);} + + ret = ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count); + unsigned pwm_value = 1100; + struct pwm_output_values pwm_values; + memset(&pwm_values, 0, sizeof(pwm_values)); + + for (unsigned i = 0; i < 4; i++) { + pwm_values.values[i] = pwm_value; + pwm_values.channel_count++; + } + + ret = ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values); + + if (ret != OK) {errx(ret, "failed setting min values");} + + close(fd); +} + +void +VtolAttitudeControl::task_main_trampoline(int argc, char *argv[]) +{ + VTOL_att_control::g_control->task_main(); +} + +void VtolAttitudeControl::task_main() +{ + warnx("started"); + fflush(stdout); + + /* do subscriptions */ + _v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); + _mc_virtual_v_rates_sp_sub = orb_subscribe(ORB_ID(mc_virtual_rates_setpoint)); + _fw_virtual_v_rates_sp_sub = orb_subscribe(ORB_ID(fw_virtual_rates_setpoint)); + _v_att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + _v_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); + _params_sub = orb_subscribe(ORB_ID(parameter_update)); + _manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); + _armed_sub = orb_subscribe(ORB_ID(actuator_armed)); + + _actuator_inputs_mc = orb_subscribe(ORB_ID(actuator_controls_virtual_mc)); + _actuator_inputs_fw = orb_subscribe(ORB_ID(actuator_controls_virtual_fw)); + + parameters_update(); /*initialize parameter cache/* + + /* wakeup source*/ + struct pollfd fds[3]; /*input_mc, input_fw, parameters*/ + + fds[0].fd = _actuator_inputs_mc; + fds[0].events = POLLIN; + fds[1].fd = _actuator_inputs_fw; + fds[1].events = POLLIN; + fds[2].fd = _params_sub; + fds[2].events = POLLIN; + + while (!_task_should_exit) { + /*Advertise/Publish vtol vehicle status*/ + if (_vtol_vehicle_status_pub > 0) { + orb_publish(ORB_ID(vtol_vehicle_status), _vtol_vehicle_status_pub, &_vtol_vehicle_status); + + } else { + _vtol_vehicle_status.timestamp = hrt_absolute_time(); + _vtol_vehicle_status_pub = orb_advertise(ORB_ID(vtol_vehicle_status), &_vtol_vehicle_status); + } + + /* wait for up to 100ms for data */ + int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); + + + /* timed out - periodic check for _task_should_exit */ + if (pret == 0) { + continue; + } + + /* this is undesirable but not much we can do - might want to flag unhappy status */ + if (pret < 0) { + warn("poll error %d, %d", pret, errno); + /* sleep a bit before next try */ + usleep(100000); + continue; + } + + if (fds[2].revents & POLLIN) { //parameters were updated, read them now + /* read from param to clear updated flag */ + struct parameter_update_s update; + orb_copy(ORB_ID(parameter_update), _params_sub, &update); + + /* update parameters from storage */ + parameters_update(); + } + + vehicle_control_mode_poll(); //Check for changes in vehicle control mode. + vehicle_manual_poll(); //Check for changes in manual inputs. + arming_status_poll(); //Check for arming status updates. + actuator_controls_mc_poll(); //Check for changes in mc_attitude_control output + actuator_controls_fw_poll(); //Check for changes in fw_attitude_control output + vehicle_rates_sp_mc_poll(); + vehicle_rates_sp_fw_poll(); + parameters_update_poll(); + + if (_manual_control_sp.aux1 <= 0.0f) { /* vehicle is in mc mode */ + _vtol_vehicle_status.vtol_in_rw_mode = true; + + if (!flag_idle_mc) { /* we want to adjust idle speed for mc mode */ + set_idle_mc(); + flag_idle_mc = true; + } + + /* got data from mc_att_controller */ + if (fds[0].revents & POLLIN) { + vehicle_manual_poll(); /* update remote input */ + orb_copy(ORB_ID(actuator_controls_virtual_mc), _actuator_inputs_mc, &_actuators_mc_in); + + fill_mc_att_control_output(); + fill_mc_att_rates_sp(); + + if (_actuators_0_pub > 0) { + orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators_out_0); + + } else { + _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators_out_0); + } + + if (_actuators_1_pub > 0) { + orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, &_actuators_out_1); + + } else { + _actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators_out_1); + } + } + } + + if (_manual_control_sp.aux1 >= 0.0f) { /* vehicle is in fw mode */ + _vtol_vehicle_status.vtol_in_rw_mode = false; + + if (flag_idle_mc) { /* we want to adjust idle speed for fixed wing mode */ + set_idle_fw(); + flag_idle_mc = false; + } + + if (fds[1].revents & POLLIN) { /* got data from fw_att_controller */ + orb_copy(ORB_ID(actuator_controls_virtual_fw), _actuator_inputs_fw, &_actuators_fw_in); + vehicle_manual_poll(); //update remote input + + fill_fw_att_control_output(); + fill_fw_att_rates_sp(); + + if (_actuators_0_pub > 0) { + orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators_out_0); + + } else { + _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators_out_0); + } + + if (_actuators_1_pub > 0) { + orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, &_actuators_out_1); + + } else { + _actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators_out_1); + } + } + } + + // publish the attitude rates setpoint + if(_v_rates_sp_pub > 0) { + orb_publish(ORB_ID(vehicle_rates_setpoint),_v_rates_sp_pub,&_v_rates_sp); + } + else { + _v_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint),&_v_rates_sp); + } + } + + warnx("exit"); + _control_task = -1; + _exit(0); +} + +int +VtolAttitudeControl::start() +{ + ASSERT(_control_task == -1); + + /* start the task */ + _control_task = task_spawn_cmd("vtol_att_control", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 10, + 2048, + (main_t)&VtolAttitudeControl::task_main_trampoline, + nullptr); + + if (_control_task < 0) { + warn("task start failed"); + return -errno; + } + + return OK; +} + + +int vtol_att_control_main(int argc, char *argv[]) +{ + if (argc < 1) { + errx(1, "usage: vtol_att_control {start|stop|status}"); + } + + if (!strcmp(argv[1], "start")) { + + if (VTOL_att_control::g_control != nullptr) { + errx(1, "already running"); + } + + VTOL_att_control::g_control = new VtolAttitudeControl; + + if (VTOL_att_control::g_control == nullptr) { + errx(1, "alloc failed"); + } + + if (OK != VTOL_att_control::g_control->start()) { + delete VTOL_att_control::g_control; + VTOL_att_control::g_control = nullptr; + err(1, "start failed"); + } + + exit(0); + } + + if (!strcmp(argv[1], "stop")) { + if (VTOL_att_control::g_control == nullptr) { + errx(1, "not running"); + } + + delete VTOL_att_control::g_control; + VTOL_att_control::g_control = nullptr; + exit(0); + } + + if (!strcmp(argv[1], "status")) { + if (VTOL_att_control::g_control) { + errx(0, "running"); + + } else { + errx(1, "not running"); + } + } + + warnx("unrecognized command"); + return 1; +} From e027bf22133ff20fdb2a877880e1b9ec5c7d8533 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Tue, 2 Dec 2014 11:02:23 +0100 Subject: [PATCH 092/216] removed remainings of quadshot for now --- ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol | 4 ++-- ROMFS/px4fmu_common/init.d/rc.autostart | 8 +------- ROMFS/px4fmu_common/init.d/rcS | 4 ++-- 3 files changed, 5 insertions(+), 11 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol b/ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol index c8b095b3c7..3e1aa1643d 100644 --- a/ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol +++ b/ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol @@ -1,6 +1,6 @@ #!nsh # -# Generic quadshot configuration file +# Generic configuration file for caipirinha VTOL version # # Roman Bapst # @@ -9,6 +9,6 @@ sh /etc/init.d/rc.vtol_defaults set MIXER FMU_caipirinha_vtol -set PWM_OUTPUTS 12 +set PWM_OUTPUTS 2 set PWM_MIN 1080 set PWM_MAX 2000 diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart b/ROMFS/px4fmu_common/init.d/rc.autostart index 331ff4974d..19fce6ea56 100644 --- a/ROMFS/px4fmu_common/init.d/rc.autostart +++ b/ROMFS/px4fmu_common/init.d/rc.autostart @@ -239,13 +239,7 @@ then sh /etc/init.d/12001_octo_cox fi -# -# VTOL quadshot -# - if param compare SYS_AUTOSTART 13000 - then - sh /etc/init.d/13000_quadshot - fi +# 13000 is historically reserved for the quadshot # # VTOL caipririnha diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index ffef598c48..8741f5ee0c 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -561,9 +561,9 @@ then if [ $MAV_TYPE == none ] then # Use mixer to detect vehicle type - if [ $MIXER == FMU_quadshot ] + if [ $MIXER == FMU_caipirinha_vtol ] then - set MAV_TYPE 20 + set MAV_TYPE 19 fi fi From fa8ca2fc107ccd0a8e74dc3798bc27f8422f0a56 Mon Sep 17 00:00:00 2001 From: tumbili Date: Wed, 3 Dec 2014 09:42:12 +0100 Subject: [PATCH 093/216] set correct number of PWM outputs and PWM Rate for caipirinha VTOL --- ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol b/ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol index 3e1aa1643d..c1461d9c42 100644 --- a/ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol +++ b/ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol @@ -9,6 +9,7 @@ sh /etc/init.d/rc.vtol_defaults set MIXER FMU_caipirinha_vtol -set PWM_OUTPUTS 2 +set PWM_OUTPUTS 12 set PWM_MIN 1080 set PWM_MAX 2000 +set PWM_RATE 400 \ No newline at end of file From 0e33e52d4ae2df06467f5b862bb0734c3ba23015 Mon Sep 17 00:00:00 2001 From: tumbili Date: Wed, 3 Dec 2014 09:44:34 +0100 Subject: [PATCH 094/216] use uORB ID to determine the correct rate_sp- and actuator topic to publish on --- .../fw_att_control/fw_att_control_main.cpp | 48 ++++++++--------- .../mc_att_control/mc_att_control_main.cpp | 51 +++++++++---------- 2 files changed, 44 insertions(+), 55 deletions(-) diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index daf7878638..679ef1d296 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -130,12 +130,13 @@ private: int _vehicle_status_sub; /**< vehicle status subscription */ orb_advert_t _rate_sp_pub; /**< rate setpoint publication */ - orb_advert_t _rate_sp_virtual_pub; /* virtual att rates setpoint topic (vtol) */ orb_advert_t _attitude_sp_pub; /**< attitude setpoint point */ orb_advert_t _actuators_0_pub; /**< actuator control group 0 setpoint */ - orb_advert_t _actuators_virtual_fw_pub; /**publisher for VTOL vehicle*/ orb_advert_t _actuators_2_pub; /**< actuator control group 1 setpoint (Airframe) */ + orb_id_t _rates_sp_id; // pointer to correct rates setpoint uORB metadata structure + orb_id_t _actuators_id; // pointer to correct actuator controls0 uORB metadata structure + struct vehicle_attitude_s _att; /**< vehicle attitude */ struct accel_report _accel; /**< body frame accelerations */ struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ @@ -334,10 +335,8 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : /* publications */ _rate_sp_pub(-1), - _rate_sp_virtual_pub(-1), _attitude_sp_pub(-1), _actuators_0_pub(-1), - _actuators_virtual_fw_pub(-1), _actuators_2_pub(-1), /* performance counters */ @@ -402,6 +401,15 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : /* fetch initial parameter values */ parameters_update(); + // set correct uORB ID, depending on if vehicle is VTOL or not + if (_parameters.autostart_id >= 13000 && _parameters.autostart_id <= 13999) { /* VTOL airframe?*/ + _rates_sp_id = ORB_ID(fw_virtual_rates_setpoint); + _actuators_id = ORB_ID(actuator_controls_virtual_fw); + } + else { + _rates_sp_id = ORB_ID(vehicle_rates_setpoint); + _actuators_id = ORB_ID(actuator_controls_0); + } } FixedwingAttitudeControl::~FixedwingAttitudeControl() @@ -629,19 +637,7 @@ FixedwingAttitudeControl::task_main() orb_set_interval(_att_sub, 17); parameters_update(); - - /*Publish to correct actuator control topic, depending on what airframe we are using - * If airframe is of type VTOL then we want to publish the actuator controls on the virtual fixed wing - * topic, from which the vtol_att_control module is receiving data and processing it further)*/ - if (_parameters.autostart_id >= 13000 && _parameters.autostart_id <= 13999) { /* VTOL airframe?*/ - _actuators_virtual_fw_pub = orb_advertise(ORB_ID(actuator_controls_virtual_fw), &_actuators); - _rate_sp_virtual_pub = orb_advertise(ORB_ID(fw_virtual_rates_setpoint), &_rates_sp); - - } else { /*airframe is not of type VTOL, use standard topic for controls publication*/ - _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators); - _rate_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_rates_sp); - } - + /* get an initial update for all sensor and status data */ vehicle_airspeed_poll(); vehicle_setpoint_poll(); @@ -1008,11 +1004,10 @@ FixedwingAttitudeControl::task_main() if (_rate_sp_pub > 0) { /* publish the attitude rates setpoint */ - orb_publish(ORB_ID(vehicle_rates_setpoint), _rate_sp_pub, &_rates_sp); - - } else if (_rate_sp_virtual_pub > 0) { - /* publish the virtual attitude setpoint */ - orb_publish(ORB_ID(fw_virtual_rates_setpoint), _rate_sp_virtual_pub, &_rates_sp); + orb_publish(_rates_sp_id, _rate_sp_pub, &_rates_sp); + } else { + /* advertise the attitude rates setpoint */ + _rate_sp_pub = orb_advertise(_rates_sp_id, &_rates_sp); } } else { @@ -1033,11 +1028,10 @@ FixedwingAttitudeControl::task_main() _actuators_airframe.timestamp = hrt_absolute_time(); /* publish the actuator controls */ - if (_actuators_0_pub > 0) { //normal fixed wing airframe - orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators); - - } else if (_actuators_0_pub < 0 && !_vehicle_status.is_rotary_wing) { //VTOL airframe - orb_publish(ORB_ID(actuator_controls_virtual_fw), _actuators_virtual_fw_pub, &_actuators); + if (_actuators_0_pub > 0) { + orb_publish(_actuators_id, _actuators_0_pub, &_actuators); + } else { + _actuators_0_pub= orb_advertise(_actuators_id, &_actuators); } if (_actuators_2_pub > 0) { diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index c5dccd8c39..2431f07f4d 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -124,9 +124,10 @@ private: orb_advert_t _att_sp_pub; /**< attitude setpoint publication */ orb_advert_t _v_rates_sp_pub; /**< rate setpoint publication */ - orb_advert_t _v_rates_sp_virtual_pub; /* virtual att rates sp publication */ orb_advert_t _actuators_0_pub; /**< attitude actuator controls publication */ - orb_advert_t _actuators_virtual_mc_pub; /**attitude actuator controls publication if vehicle is VTOL*/ + + orb_id_t _rates_sp_id; // pointer to correct rates setpoint uORB metadata structure + orb_id_t _actuators_id; // pointer to correct actuator controls0 uORB metadata structure bool _actuators_0_circuit_breaker_enabled; /**< circuit breaker to suppress output */ @@ -283,9 +284,7 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : /* publications */ _att_sp_pub(-1), _v_rates_sp_pub(-1), - _v_rates_sp_virtual_pub(-1), _actuators_0_pub(-1), - _actuators_virtual_mc_pub(-1), _actuators_0_circuit_breaker_enabled(false), @@ -349,6 +348,15 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : /* fetch initial parameter values */ parameters_update(); + // set correct uORB ID, depending on if vehicle is VTOL or not + if (_params.autostart_id >= 13000 && _params.autostart_id <= 13999) { /* VTOL airframe?*/ + _rates_sp_id = ORB_ID(mc_virtual_rates_setpoint); + _actuators_id = ORB_ID(actuator_controls_virtual_mc); + } + else { + _rates_sp_id = ORB_ID(vehicle_rates_setpoint); + _actuators_id = ORB_ID(actuator_controls_0); + } } MulticopterAttitudeControl::~MulticopterAttitudeControl() @@ -776,19 +784,6 @@ MulticopterAttitudeControl::task_main() /* initialize parameters cache */ parameters_update(); - /*Subscribe to correct actuator control topic, depending on what airframe we are using - * If airframe is of type VTOL then we want to publish the actuator controls on the virtual multicopter - * topic, from which the VTOL_att_control module is receiving data and processing it further)*/ - if (_params.autostart_id >= 13000 && _params.autostart_id <= 13999) { /* VTOL airframe?*/ - _actuators_virtual_mc_pub = orb_advertise(ORB_ID(actuator_controls_virtual_mc), &_actuators); - _v_rates_sp_virtual_pub = orb_advertise(ORB_ID(mc_virtual_rates_setpoint), &_v_rates_sp); - - } else { /*airframe is not of type VTOL, use standard topic for controls publication*/ - _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators); - _v_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_v_rates_sp); - } - - /* wakeup source: vehicle attitude */ struct pollfd fds[1]; @@ -849,10 +844,10 @@ MulticopterAttitudeControl::task_main() _v_rates_sp.timestamp = hrt_absolute_time(); if (_v_rates_sp_pub > 0) { - orb_publish(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_pub, &_v_rates_sp); + orb_publish(_rates_sp_id, _v_rates_sp_pub, &_v_rates_sp); - } else if (_v_rates_sp_virtual_pub > 0) { - _v_rates_sp_pub = orb_publish(ORB_ID(mc_virtual_rates_setpoint), _v_rates_sp_virtual_pub, &_v_rates_sp); + } else { + _v_rates_sp_pub = orb_advertise(_rates_sp_id, &_v_rates_sp); } } else { @@ -873,11 +868,11 @@ MulticopterAttitudeControl::task_main() _v_rates_sp.timestamp = hrt_absolute_time(); if (_v_rates_sp_pub > 0) { - orb_publish(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_pub, &_v_rates_sp); + orb_publish(_rates_sp_id, _v_rates_sp_pub, &_v_rates_sp); - } else if (_v_rates_sp_virtual_pub > 0) { - _v_rates_sp_pub = orb_publish(ORB_ID(mc_virtual_rates_setpoint), _v_rates_sp_virtual_pub, &_v_rates_sp); - } + } else { + _v_rates_sp_pub = orb_advertise(_rates_sp_id, &_v_rates_sp); + } } else { /* attitude controller disabled, poll rates setpoint topic */ @@ -900,11 +895,11 @@ MulticopterAttitudeControl::task_main() _actuators.timestamp = hrt_absolute_time(); if (!_actuators_0_circuit_breaker_enabled) { - if (_actuators_0_pub > 0) { //normal mutlicopter airframe - orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators); + if (_actuators_0_pub > 0) { + orb_publish(_actuators_id, _actuators_0_pub, &_actuators); - } else if (_actuators_0_pub < 0 && _vehicle_status.is_rotary_wing) { - orb_publish(ORB_ID(actuator_controls_virtual_mc), _actuators_virtual_mc_pub, &_actuators); + } else { + _actuators_0_pub = orb_advertise(_actuators_id, &_actuators); } } From f7ca1d36d2a35b9781a41ffc40acfb2eaf84544f Mon Sep 17 00:00:00 2001 From: tumbili Date: Wed, 3 Dec 2014 09:46:52 +0100 Subject: [PATCH 095/216] renamed FWC to ATC1 --- src/modules/sdlog2/sdlog2.c | 16 ++++++++-------- src/modules/sdlog2/sdlog2_messages.h | 14 +++----------- 2 files changed, 11 insertions(+), 19 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 8f87d897a1..6df677338e 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -982,7 +982,6 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_LPSP_s log_LPSP; struct log_GPS_s log_GPS; struct log_ATTC_s log_ATTC; - struct log_FWC_s log_FWC; struct log_STAT_s log_STAT; struct log_RC_s log_RC; struct log_OUT0_s log_OUT0; @@ -1429,13 +1428,14 @@ int sdlog2_thread_main(int argc, char *argv[]) LOGBUFFER_WRITE_AND_COUNT(ATTC); } - if(copy_if_updated(ORB_ID(actuator_controls_1),subs.act_controls_1_sub,&buf.act_controls1)) { - log_msg.msg_type = LOG_FWC_MSG; - log_msg.body.log_FWC.roll = buf.act_controls1.control[0]; - log_msg.body.log_FWC.pitch = buf.act_controls1.control[1]; - log_msg.body.log_FWC.yaw = buf.act_controls1.control[2]; - log_msg.body.log_FWC.thrust = buf.act_controls1.control[3]; - LOGBUFFER_WRITE_AND_COUNT(FWC); + /* --- ACTUATOR CONTROL FW VTOL --- */ + if(copy_if_updated(ORB_ID(actuator_controls_1),subs.act_controls_1_sub,&buf.act_controls)) { + log_msg.msg_type = LOG_ATC1_MSG; + log_msg.body.log_ATTC.roll = buf.act_controls.control[0]; + log_msg.body.log_ATTC.pitch = buf.act_controls.control[1]; + log_msg.body.log_ATTC.yaw = buf.act_controls.control[2]; + log_msg.body.log_ATTC.thrust = buf.act_controls.control[3]; + LOGBUFFER_WRITE_AND_COUNT(ATTC); } /* --- LOCAL POSITION --- */ diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 447b3fb9cb..b78b430aad 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -150,6 +150,7 @@ struct log_GPS_s { /* --- ATTC - ATTITUDE CONTROLS (ACTUATOR_0 CONTROLS)--- */ #define LOG_ATTC_MSG 9 +#define LOG_ATC1_MSG 40 struct log_ATTC_s { float roll; float pitch; @@ -423,15 +424,6 @@ struct log_ENCD_s { float vel1; }; -/* ATTITUDE CONTROLS (ACTUATOR_1 CONTROLS) */ -#define LOG_FWC_MSG 40 -struct log_FWC_s { - float roll; - float pitch; - float yaw; - float thrust; -}; - /********** SYSTEM MESSAGES, ID > 0x80 **********/ /* --- TIME - TIME STAMP --- */ @@ -469,8 +461,8 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(LPOS, "ffffffffLLfBBBff", "X,Y,Z,Dist,DistR,VX,VY,VZ,RLat,RLon,RAlt,PFlg,LFlg,GFlg,EPH,EPV"), LOG_FORMAT(LPSP, "ffff", "X,Y,Z,Yaw"), LOG_FORMAT(GPS, "QBffLLfffffBHHH", "GPSTime,Fix,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog,nSat,SNR,N,J"), - LOG_FORMAT(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"), - LOG_FORMAT(FWC,"ffff", "Roll,Pitch,Yaw,Thrust"), + LOG_FORMAT_S(ATTC, ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"), + LOG_FORMAT_S(ATC1, ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"), LOG_FORMAT(STAT, "BBBfBB", "MainState,ArmState,FailsafeState,BatRem,BatWarn,Landed"), LOG_FORMAT(RC, "ffffffffBB", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,Count,SignalLost"), LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"), From 563de3b49c7e12c9745744b859b5aeedb0535090 Mon Sep 17 00:00:00 2001 From: tumbili Date: Wed, 3 Dec 2014 09:47:56 +0100 Subject: [PATCH 096/216] in fw mode, publish also yaw and throttle on control group 1 for logging --- src/modules/vtol_att_control/vtol_att_control_main.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index 62b6a4ed6f..cc3a3665a3 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -402,8 +402,11 @@ void VtolAttitudeControl::fill_fw_att_control_output() _actuators_out_0.control[2] = 0; _actuators_out_0.control[3] = _actuators_fw_in.control[3]; /*controls for the elevons */ - _actuators_out_1.control[0] = -_actuators_fw_in.control[0]; /*roll elevon*/ - _actuators_out_1.control[1] = _actuators_fw_in.control[1]; /*pitch elevon */ + _actuators_out_1.control[0] = -_actuators_fw_in.control[0]; // roll elevon + _actuators_out_1.control[1] = _actuators_fw_in.control[1]; // pitch elevon + // unused now but still logged + _actuators_out_1.control[2] = _actuators_fw_in.control[2]; // yaw + _actuators_out_1.control[3] = _actuators_fw_in.control[3]; // throttle } /** From 34cf39e1ec106fe976e82bddee482cb27448e812 Mon Sep 17 00:00:00 2001 From: tumbili Date: Wed, 3 Dec 2014 09:52:29 +0100 Subject: [PATCH 097/216] added vtol attitude control module to FMU makefile --- makefiles/config_px4fmu-v2_default.mk | 1 + 1 file changed, 1 insertion(+) diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index df6b6d0c5b..335055cb17 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -86,6 +86,7 @@ MODULES += modules/fw_pos_control_l1 MODULES += modules/fw_att_control MODULES += modules/mc_att_control MODULES += modules/mc_pos_control +MODULES += modules/vtol_att_control # # Logging From 21465bc2872eba86ee123224f5132631d715a161 Mon Sep 17 00:00:00 2001 From: tumbili Date: Wed, 3 Dec 2014 09:59:18 +0100 Subject: [PATCH 098/216] added missing newline --- ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol b/ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol index c1461d9c42..4e14dd0d15 100644 --- a/ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol +++ b/ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol @@ -12,4 +12,4 @@ set MIXER FMU_caipirinha_vtol set PWM_OUTPUTS 12 set PWM_MIN 1080 set PWM_MAX 2000 -set PWM_RATE 400 \ No newline at end of file +set PWM_RATE 400 From 7a344b933738d9caedc0fe177cc4615c294eaf51 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Wed, 3 Dec 2014 10:30:49 +0100 Subject: [PATCH 099/216] Display ESC data in the status output --- src/modules/uavcan/uavcan_main.cpp | 22 ++++++++++++++++++++++ 1 file changed, 22 insertions(+) diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 8147a8b898..7213c72c61 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -46,6 +46,8 @@ #include #include +#include + #include #include @@ -612,10 +614,30 @@ UavcanNode::print_info() if (_outputs.noutputs != 0) { printf("ESC output: "); + for (uint8_t i=0; i<_outputs.noutputs; i++) { printf("%d ", (int)(_outputs.output[i]*1000)); } printf("\n"); + + // ESC status + int esc_sub = orb_subscribe(ORB_ID(esc_status)); + struct esc_status_s esc; + memset(&esc, 0, sizeof(esc)); + orb_copy(ORB_ID(esc_status), esc_sub, &esc); + + printf("ESC Status:\n"); + printf("Addr\tV\tA\tTemp\tSetpt\tRPM\tErr\n"); + for (uint8_t i=0; i<_outputs.noutputs; i++) { + printf("%d\t", esc.esc[i].esc_address); + printf("%3.2f\t", (double)esc.esc[i].esc_voltage); + printf("%3.2f\t", (double)esc.esc[i].esc_current); + printf("%3.2f\t", (double)esc.esc[i].esc_temperature); + printf("%3.2f\t", (double)esc.esc[i].esc_setpoint); + printf("%d\t", esc.esc[i].esc_rpm); + printf("%d", esc.esc[i].esc_errorcount); + printf("\n"); + } } // Sensor bridges From f3fb32bc4790673e4a643f341b16d2434423dbea Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Wed, 3 Dec 2014 10:43:17 +0100 Subject: [PATCH 100/216] Unsubscribe from the topic. --- src/modules/uavcan/uavcan_main.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 7213c72c61..60901e0c75 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -638,6 +638,8 @@ UavcanNode::print_info() printf("%d", esc.esc[i].esc_errorcount); printf("\n"); } + + orb_unsubscribe(esc_sub); } // Sensor bridges From fbedd3ed206531704f24a0b76657c4583a49efe8 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Thu, 4 Dec 2014 14:23:19 +0100 Subject: [PATCH 101/216] removed whitespaces removed small unused code block --- src/modules/commander/commander.cpp | 2 +- src/modules/fw_att_control/fw_att_control_main.cpp | 3 +-- src/modules/mc_att_control/mc_att_control_main.cpp | 2 -- 3 files changed, 2 insertions(+), 5 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index bed4676989..4c7664cd0c 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -2240,7 +2240,7 @@ set_control_mode() control_mode.flag_control_velocity_enabled = false; control_mode.flag_control_termination_enabled = false; break; - + case NAVIGATION_STATE_ALTCTL: control_mode.flag_control_manual_enabled = true; control_mode.flag_control_auto_enabled = false; diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 679ef1d296..956fc94de4 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -77,7 +77,6 @@ #include #include - /** * Fixedwing attitude control app start / stop handling function * @@ -637,7 +636,7 @@ FixedwingAttitudeControl::task_main() orb_set_interval(_att_sub, 17); parameters_update(); - + /* get an initial update for all sensor and status data */ vehicle_airspeed_poll(); vehicle_setpoint_poll(); diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 2431f07f4d..0702e63783 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -766,8 +766,6 @@ MulticopterAttitudeControl::task_main_trampoline(int argc, char *argv[]) void MulticopterAttitudeControl::task_main() { - warnx("started"); - fflush(stdout); /* * do subscriptions From 4fd3e0720f37db472a689b869d3c4019f90acd25 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Thu, 4 Dec 2014 16:55:12 +0100 Subject: [PATCH 102/216] added parameter file for vtol attitude control module --- src/modules/vtol_att_control/vtol_att_control_params.c | 6 ++++++ 1 file changed, 6 insertions(+) create mode 100644 src/modules/vtol_att_control/vtol_att_control_params.c diff --git a/src/modules/vtol_att_control/vtol_att_control_params.c b/src/modules/vtol_att_control/vtol_att_control_params.c new file mode 100644 index 0000000000..b9baa20466 --- /dev/null +++ b/src/modules/vtol_att_control/vtol_att_control_params.c @@ -0,0 +1,6 @@ +#include + +// number of engines +PARAM_DEFINE_INT32(VTOL_MOT_COUNT,0); +// idle pwm in multicopter mode +PARAM_DEFINE_INT32(IDLE_PWM_MC,900); \ No newline at end of file From 7ea4d10bc9802a0d2a167f65e1bb843ca5643519 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Thu, 4 Dec 2014 16:56:08 +0100 Subject: [PATCH 103/216] added parameter file to makefile --- src/modules/vtol_att_control/module.mk | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/modules/vtol_att_control/module.mk b/src/modules/vtol_att_control/module.mk index c349c23401..0d5155e901 100644 --- a/src/modules/vtol_att_control/module.mk +++ b/src/modules/vtol_att_control/module.mk @@ -37,4 +37,5 @@ MODULE_COMMAND = vtol_att_control -SRCS = vtol_att_control_main.cpp +SRCS = vtol_att_control_main.cpp \ + vtol_att_control_params.c From bed53e810b106f5f9206a89676d493e1e50edc1d Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Thu, 4 Dec 2014 16:57:06 +0100 Subject: [PATCH 104/216] introduced vtol_motor_count and idle_pwm_mc parameter --- .../vtol_att_control_main.cpp | 28 ++++++++++++------- 1 file changed, 18 insertions(+), 10 deletions(-) diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index cc3a3665a3..c9c6368b2e 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -132,11 +132,13 @@ private: struct actuator_armed_s _armed; //actuator arming status struct { - float min_pwm_mc; //pwm value for idle in mc mode + param_t idle_pwm_mc; //pwm value for idle in mc mode + param_t vtol_motor_count; } _params; struct { - param_t min_pwm_mc; + param_t idle_pwm_mc; + param_t vtol_motor_count; } _params_handles; perf_counter_t _loop_perf; /**< loop performance counter */ @@ -145,6 +147,7 @@ private: * for fixed wings we want to have an idle speed of zero since we do not want * to waste energy when gliding. */ bool flag_idle_mc; //false = "idle is set for fixed wing mode"; true = "idle is set for multicopter mode" + unsigned _motor_count; // number of motors //*****************Member functions*********************************************************************** @@ -217,7 +220,11 @@ VtolAttitudeControl::VtolAttitudeControl() : memset(&_actuators_fw_in, 0, sizeof(_actuators_fw_in)); memset(&_armed, 0, sizeof(_armed)); - _params_handles.min_pwm_mc = param_find("PWM_MIN"); + _params.idle_pwm_mc = PWM_LOWEST_MIN; + _params.vtol_motor_count = 0; + + _params_handles.idle_pwm_mc = param_find("IDLE_PWM_MC"); + _params_handles.vtol_motor_count = param_find("VTOL_MOT_COUNT"); /* fetch initial parameter values */ parameters_update(); @@ -369,10 +376,11 @@ VtolAttitudeControl::parameters_update_poll() int VtolAttitudeControl::parameters_update() { - /* idle pwm */ - float v; - param_get(_params_handles.min_pwm_mc, &v); - _params.min_pwm_mc = v; + /* idle pwm for mc mode */ + param_get(_params_handles.idle_pwm_mc, &_params.idle_pwm_mc); + + /* vtol motor count */ + param_get(_params_handles.vtol_motor_count, &_params.vtol_motor_count); return OK; } @@ -446,7 +454,7 @@ void VtolAttitudeControl::set_idle_fw() struct pwm_output_values pwm_values; memset(&pwm_values, 0, sizeof(pwm_values)); - for (unsigned i = 0; i < 4; i++) { + for (unsigned i = 0; i < _params.vtol_motor_count; i++) { pwm_values.values[i] = pwm_value; pwm_values.channel_count++; @@ -472,11 +480,11 @@ void VtolAttitudeControl::set_idle_mc() if (fd < 0) {err(1, "can't open %s", dev);} ret = ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count); - unsigned pwm_value = 1100; + unsigned pwm_value = _params.idle_pwm_mc; struct pwm_output_values pwm_values; memset(&pwm_values, 0, sizeof(pwm_values)); - for (unsigned i = 0; i < 4; i++) { + for (unsigned i = 0; i < _params.vtol_motor_count; i++) { pwm_values.values[i] = pwm_value; pwm_values.channel_count++; } From 692af62885e42fcaa09a5960c763549868090a6e Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Thu, 4 Dec 2014 16:58:43 +0100 Subject: [PATCH 105/216] set vtol motor count- and idle pwm parameter --- ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol b/ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol index 4e14dd0d15..bff971c0ff 100644 --- a/ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol +++ b/ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol @@ -10,6 +10,7 @@ sh /etc/init.d/rc.vtol_defaults set MIXER FMU_caipirinha_vtol set PWM_OUTPUTS 12 -set PWM_MIN 1080 set PWM_MAX 2000 set PWM_RATE 400 +param set VTOL_MOT_COUNT 2 +param set IDLE_PWM_MC 1080 From 590489d49859c77ef2e919f2ef45e9dc81e28f9f Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Thu, 4 Dec 2014 17:36:16 +0100 Subject: [PATCH 106/216] make sure vtol attitude control module is started with idle speed set for multicopter mode --- src/modules/vtol_att_control/vtol_att_control_main.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index c9c6368b2e..e69c51142b 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -202,7 +202,7 @@ VtolAttitudeControl::VtolAttitudeControl() : _loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control")) { - flag_idle_mc = true; /*assume we always start in mc mode for a VTOL airframe */ + flag_idle_mc = true; memset(& _vtol_vehicle_status, 0, sizeof(_vtol_vehicle_status)); _vtol_vehicle_status.vtol_in_rw_mode = true; /* start vtol in rotary wing mode*/ @@ -520,7 +520,11 @@ void VtolAttitudeControl::task_main() _actuator_inputs_mc = orb_subscribe(ORB_ID(actuator_controls_virtual_mc)); _actuator_inputs_fw = orb_subscribe(ORB_ID(actuator_controls_virtual_fw)); - parameters_update(); /*initialize parameter cache/* + parameters_update(); // initialize parameter cache + + // make sure we start with idle in mc mode + set_idle_mc(); + flag_idle_mc = true; /* wakeup source*/ struct pollfd fds[3]; /*input_mc, input_fw, parameters*/ From 945c8df18bfc25650bd01d9358295f1c2102a61f Mon Sep 17 00:00:00 2001 From: tumbili Date: Sat, 6 Dec 2014 17:37:46 +0100 Subject: [PATCH 107/216] added scaling of mc pitch control with airspeed --- .../vtol_att_control_main.cpp | 78 ++++++++++++++++++- .../vtol_att_control_params.c | 9 ++- 2 files changed, 85 insertions(+), 2 deletions(-) diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index e69c51142b..2bd4d0c25d 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -63,6 +63,7 @@ #include #include #include +#include #include #include #include @@ -107,6 +108,7 @@ private: int _params_sub; //parameter updates subscription int _manual_control_sp_sub; //manual control setpoint subscription int _armed_sub; //arming status subscription + int _airspeed_sub; // airspeed subscription int _actuator_inputs_mc; //topic on which the mc_att_controller publishes actuator inputs int _actuator_inputs_fw; //topic on which the fw_att_controller publishes actuator inputs @@ -130,18 +132,26 @@ private: struct actuator_controls_s _actuators_mc_in; //actuator controls from mc_att_control struct actuator_controls_s _actuators_fw_in; //actuator controls from fw_att_control struct actuator_armed_s _armed; //actuator arming status + struct airspeed_s _airspeed; // airspeed struct { param_t idle_pwm_mc; //pwm value for idle in mc mode param_t vtol_motor_count; + float mc_airspeed_min; // min airspeed in multicoper mode (including prop-wash) + float mc_airspeed_trim; // trim airspeed in multicopter mode + float mc_airspeed_max; // max airpseed in multicopter mode } _params; struct { param_t idle_pwm_mc; param_t vtol_motor_count; + param_t mc_airspeed_min; + param_t mc_airspeed_trim; + param_t mc_airspeed_max; } _params_handles; perf_counter_t _loop_perf; /**< loop performance counter */ + perf_counter_t _nonfinite_input_perf; /**< performance counter for non finite input */ /* for multicopters it is usual to have a non-zero idle speed of the engines * for fixed wings we want to have an idle speed of zero since we do not want @@ -161,6 +171,7 @@ private: void actuator_controls_fw_poll(); //Check for changes in fw_attitude_control output void vehicle_rates_sp_mc_poll(); void vehicle_rates_sp_fw_poll(); + void vehicle_airspeed_poll(); // Check for changes in airspeed void parameters_update_poll(); //Check if parameters have changed int parameters_update(); //Update local paraemter cache void fill_mc_att_control_output(); //write mc_att_control results to actuator message @@ -169,6 +180,7 @@ private: void fill_fw_att_rates_sp(); void set_idle_fw(); void set_idle_mc(); + void scale_mc_output(); }; namespace VTOL_att_control @@ -192,6 +204,7 @@ VtolAttitudeControl::VtolAttitudeControl() : _params_sub(-1), _manual_control_sp_sub(-1), _armed_sub(-1), + _airspeed_sub(-1), //init publication handlers _actuators_0_pub(-1), @@ -199,7 +212,8 @@ VtolAttitudeControl::VtolAttitudeControl() : _vtol_vehicle_status_pub(-1), _v_rates_sp_pub(-1), - _loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control")) + _loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control")), + _nonfinite_input_perf(perf_alloc(PC_COUNT, "vtol att control nonfinite input")) { flag_idle_mc = true; @@ -219,12 +233,16 @@ VtolAttitudeControl::VtolAttitudeControl() : memset(&_actuators_mc_in, 0, sizeof(_actuators_mc_in)); memset(&_actuators_fw_in, 0, sizeof(_actuators_fw_in)); memset(&_armed, 0, sizeof(_armed)); + memset(&_airspeed,0,sizeof(_airspeed)); _params.idle_pwm_mc = PWM_LOWEST_MIN; _params.vtol_motor_count = 0; _params_handles.idle_pwm_mc = param_find("IDLE_PWM_MC"); _params_handles.vtol_motor_count = param_find("VTOL_MOT_COUNT"); + _params_handles.mc_airspeed_min = param_find("VTOL_MC_AIRSPEED_MIN"); + _params_handles.mc_airspeed_max = param_find("VTOL_MC_AIRSPEED_MAX"); + _params_handles.mc_airspeed_trim = param_find("VTOL_MC_AIRSPEED_TRIM"); /* fetch initial parameter values */ parameters_update(); @@ -352,6 +370,19 @@ void VtolAttitudeControl::vehicle_rates_sp_fw_poll() } } +/** +* Check for airspeed updates. +*/ +void +VtolAttitudeControl::vehicle_airspeed_poll() { + bool updated; + orb_check(_airspeed_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(airspeed), _airspeed_sub , &_airspeed); + } +} + /** * Check for parameter updates. */ @@ -376,12 +407,25 @@ VtolAttitudeControl::parameters_update_poll() int VtolAttitudeControl::parameters_update() { + float v; /* idle pwm for mc mode */ param_get(_params_handles.idle_pwm_mc, &_params.idle_pwm_mc); /* vtol motor count */ param_get(_params_handles.vtol_motor_count, &_params.vtol_motor_count); + /* vtol mc mode min airspeed */ + param_get(_params_handles.mc_airspeed_min, &v); + _params.mc_airspeed_min = v; + + /* vtol mc mode max airspeed */ + param_get(_params_handles.mc_airspeed_max, &v); + _params.mc_airspeed_max = v; + + /* vtol mc mode trim airspeed */ + param_get(_params_handles.mc_airspeed_trim, &v); + _params.mc_airspeed_trim = v; + return OK; } @@ -496,6 +540,34 @@ void VtolAttitudeControl::set_idle_mc() close(fd); } +void +VtolAttitudeControl::scale_mc_output() { + // scale around tuning airspeed + float airspeed; + + // if airspeed is not updating, we assume the normal average speed + if (bool nonfinite = !isfinite(_airspeed.true_airspeed_m_s) || + hrt_elapsed_time(&_airspeed.timestamp) > 1e6) { + airspeed = _params.mc_airspeed_trim; + if (nonfinite) { + perf_count(_nonfinite_input_perf); + } + } else { + // prevent numerical drama by requiring 0.5 m/s minimal speed + airspeed = math::max(0.5f, _airspeed.true_airspeed_m_s); + } + + /* + * For scaling our actuators using anything less than the min (close to stall) + * speed doesn't make any sense - its the strongest reasonable deflection we + * want to do in flight and its the baseline a human pilot would choose. + * + * Forcing the scaling to this value allows reasonable handheld tests. + */ + float airspeed_scaling = _params.mc_airspeed_trim / ((airspeed < _params.mc_airspeed_min) ? _params.mc_airspeed_min : airspeed); + _actuators_mc_in.control[1] = math::constrain(_actuators_mc_in.control[1]*airspeed_scaling*airspeed_scaling,-1.0f,1.0f); +} + void VtolAttitudeControl::task_main_trampoline(int argc, char *argv[]) { @@ -516,6 +588,7 @@ void VtolAttitudeControl::task_main() _params_sub = orb_subscribe(ORB_ID(parameter_update)); _manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); _armed_sub = orb_subscribe(ORB_ID(actuator_armed)); + _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); _actuator_inputs_mc = orb_subscribe(ORB_ID(actuator_controls_virtual_mc)); _actuator_inputs_fw = orb_subscribe(ORB_ID(actuator_controls_virtual_fw)); @@ -580,6 +653,7 @@ void VtolAttitudeControl::task_main() vehicle_rates_sp_mc_poll(); vehicle_rates_sp_fw_poll(); parameters_update_poll(); + vehicle_airspeed_poll(); if (_manual_control_sp.aux1 <= 0.0f) { /* vehicle is in mc mode */ _vtol_vehicle_status.vtol_in_rw_mode = true; @@ -593,6 +667,8 @@ void VtolAttitudeControl::task_main() if (fds[0].revents & POLLIN) { vehicle_manual_poll(); /* update remote input */ orb_copy(ORB_ID(actuator_controls_virtual_mc), _actuator_inputs_mc, &_actuators_mc_in); + // scale pitch control with airspeed + scale_mc_output(); fill_mc_att_control_output(); fill_mc_att_rates_sp(); diff --git a/src/modules/vtol_att_control/vtol_att_control_params.c b/src/modules/vtol_att_control/vtol_att_control_params.c index b9baa20466..44157acba6 100644 --- a/src/modules/vtol_att_control/vtol_att_control_params.c +++ b/src/modules/vtol_att_control/vtol_att_control_params.c @@ -3,4 +3,11 @@ // number of engines PARAM_DEFINE_INT32(VTOL_MOT_COUNT,0); // idle pwm in multicopter mode -PARAM_DEFINE_INT32(IDLE_PWM_MC,900); \ No newline at end of file +PARAM_DEFINE_INT32(IDLE_PWM_MC,900); +// min airspeed in multicopter mode +PARAM_DEFINE_FLOAT(VTOL_MC_AIRSPEED_MIN,2); +// max airspeed in multicopter mode +PARAM_DEFINE_FLOAT(VTOL_MC_AIRSPEED_MAX,30); +// trim airspeed in multicopter mode +PARAM_DEFINE_FLOAT(VTOL_MC_AIRSPEED_TRIM,10); + From 8af6ab27132563865a7ac0be2f7fa1105d614ce3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 6 Dec 2014 18:17:37 +0100 Subject: [PATCH 108/216] Fix vehicle command docs to AMSL --- src/modules/uORB/topics/vehicle_command.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/uORB/topics/vehicle_command.h b/src/modules/uORB/topics/vehicle_command.h index f264accbba..6b4cb483bd 100644 --- a/src/modules/uORB/topics/vehicle_command.h +++ b/src/modules/uORB/topics/vehicle_command.h @@ -65,7 +65,7 @@ enum VEHICLE_CMD { VEHICLE_CMD_NAV_TAKEOFF = 22, /* Takeoff from ground / hand |Minimum pitch (if airspeed sensor present), desired pitch without sensor| Empty| Empty| Yaw angle (if magnetometer present), ignored without magnetometer| Latitude| Longitude| Altitude| */ VEHICLE_CMD_NAV_ROI = 80, /* Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of intereset mode. (see MAV_ROI enum)| MISSION index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z| */ VEHICLE_CMD_NAV_PATHPLANNING = 81, /* Control autonomous path planning on the MAV. |0: Disable local obstacle avoidance / local path planning (without resetting map), 1: Enable local path planning, 2: Enable and reset local path planning| 0: Disable full path planning (without resetting map), 1: Enable, 2: Enable and reset map/occupancy grid, 3: Enable and reset planned route, but not occupancy grid| Empty| Yaw angle at goal, in compass degrees, [0..360]| Latitude/X of goal| Longitude/Y of goal| Altitude/Z of goal| */ - VEHICLE_CMD_NAV_GUIDED_LIMITS=90, /* set limits for external control |timeout - maximum time (in seconds) that external controller will be allowed to control vehicle. 0 means no timeout| absolute altitude min (in meters, WGS84) - if vehicle moves below this alt, the command will be aborted and the mission will continue. 0 means no lower altitude limit| absolute altitude max (in meters)- if vehicle moves above this alt, the command will be aborted and the mission will continue. 0 means no upper altitude limit| horizontal move limit (in meters, WGS84) - if vehicle moves more than this distance from it's location at the moment the command was executed, the command will be aborted and the mission will continue. 0 means no horizontal altitude limit| Empty| Empty| Empty| */ + VEHICLE_CMD_NAV_GUIDED_LIMITS=90, /* set limits for external control |timeout - maximum time (in seconds) that external controller will be allowed to control vehicle. 0 means no timeout| absolute altitude min (in meters, AMSL) - if vehicle moves below this alt, the command will be aborted and the mission will continue. 0 means no lower altitude limit| absolute altitude max (in meters)- if vehicle moves above this alt, the command will be aborted and the mission will continue. 0 means no upper altitude limit| horizontal move limit (in meters, AMSL) - if vehicle moves more than this distance from it's location at the moment the command was executed, the command will be aborted and the mission will continue. 0 means no horizontal altitude limit| Empty| Empty| Empty| */ VEHICLE_CMD_NAV_GUIDED_MASTER=91, /* set id of master controller |System ID| Component ID| Empty| Empty| Empty| Empty| Empty| */ VEHICLE_CMD_NAV_GUIDED_ENABLE=92, /* hand control over to an external controller |On / Off (> 0.5f on)| Empty| Empty| Empty| Empty| Empty| Empty| */ VEHICLE_CMD_NAV_LAST = 95, /* NOP - This command is only used to mark the upper limit of the NAV/ACTION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ From 350e2549df6d3bb9ec6c9787c1e6112aa403417a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 6 Dec 2014 18:18:25 +0100 Subject: [PATCH 109/216] uORB mission topic: Add AMSL as clarificaiton in docs --- src/modules/uORB/topics/mission.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/uORB/topics/mission.h b/src/modules/uORB/topics/mission.h index e4b72f87c9..22a8f3ecb8 100644 --- a/src/modules/uORB/topics/mission.h +++ b/src/modules/uORB/topics/mission.h @@ -83,7 +83,7 @@ struct mission_item_s { bool altitude_is_relative; /**< true if altitude is relative from start point */ double lat; /**< latitude in degrees */ double lon; /**< longitude in degrees */ - float altitude; /**< altitude in meters */ + float altitude; /**< altitude in meters (AMSL) */ float yaw; /**< in radians NED -PI..+PI, NAN means don't change yaw */ float loiter_radius; /**< loiter radius in meters, 0 for a VTOL to hover */ int8_t loiter_direction; /**< 1: positive / clockwise, -1, negative. */ From 33725ecc6ad383d43cfd6f533159e931052516c1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 6 Dec 2014 18:18:47 +0100 Subject: [PATCH 110/216] uORB home position: Add AMSL as clarificaiton in docs --- src/modules/uORB/topics/home_position.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/uORB/topics/home_position.h b/src/modules/uORB/topics/home_position.h index 70071130d6..d747b5f435 100644 --- a/src/modules/uORB/topics/home_position.h +++ b/src/modules/uORB/topics/home_position.h @@ -61,7 +61,7 @@ struct home_position_s double lat; /**< Latitude in degrees */ double lon; /**< Longitude in degrees */ - float alt; /**< Altitude in meters */ + float alt; /**< Altitude in meters (AMSL) */ float x; /**< X coordinate in meters */ float y; /**< Y coordinate in meters */ From d0fb862a01fc14a83f8862b86e03c8033b481a5c Mon Sep 17 00:00:00 2001 From: tumbili Date: Sat, 6 Dec 2014 20:13:42 +0100 Subject: [PATCH 111/216] fixed _loop_perf message --- src/modules/vtol_att_control/vtol_att_control_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index e69c51142b..329e2ef94e 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -199,7 +199,7 @@ VtolAttitudeControl::VtolAttitudeControl() : _vtol_vehicle_status_pub(-1), _v_rates_sp_pub(-1), - _loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control")) + _loop_perf(perf_alloc(PC_ELAPSED, "vtol_att_control")) { flag_idle_mc = true; From 1c0f850fac3f056a9c52a3258877e10c4ee0c8a0 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 10 Dec 2014 15:06:45 +0100 Subject: [PATCH 112/216] add sched.h to systemlib includes SCHED_RR and SCHED_FIFO are defined in sched.h --- src/modules/systemlib/systemlib.h | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/systemlib/systemlib.h b/src/modules/systemlib/systemlib.h index 3728f20672..6e22a557ea 100644 --- a/src/modules/systemlib/systemlib.h +++ b/src/modules/systemlib/systemlib.h @@ -41,6 +41,7 @@ #define SYSTEMLIB_H_ #include #include +#include __BEGIN_DECLS From 42c8f6b48bc7ff761eea749fdff338ee88275848 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 12 Dec 2014 12:10:50 +0100 Subject: [PATCH 113/216] Update UAVCAN version, delete outdated example --- .../flow_speed_control_main.c | 387 ------------------ .../flow_speed_control_params.c | 70 ---- .../flow_speed_control_params.h | 70 ---- src/examples/flow_speed_control/module.mk | 41 -- uavcan | 2 +- 5 files changed, 1 insertion(+), 569 deletions(-) delete mode 100644 src/examples/flow_speed_control/flow_speed_control_main.c delete mode 100644 src/examples/flow_speed_control/flow_speed_control_params.c delete mode 100644 src/examples/flow_speed_control/flow_speed_control_params.h delete mode 100644 src/examples/flow_speed_control/module.mk diff --git a/src/examples/flow_speed_control/flow_speed_control_main.c b/src/examples/flow_speed_control/flow_speed_control_main.c deleted file mode 100644 index feed0d23cd..0000000000 --- a/src/examples/flow_speed_control/flow_speed_control_main.c +++ /dev/null @@ -1,387 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. - * Author: Samuel Zihlmann - * Lorenz Meier - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file flow_speed_control.c - * - * Optical flow speed controller - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "flow_speed_control_params.h" - - -static bool thread_should_exit = false; /**< Deamon exit flag */ -static bool thread_running = false; /**< Deamon status flag */ -static int deamon_task; /**< Handle of deamon task / thread */ - -__EXPORT int flow_speed_control_main(int argc, char *argv[]); - -/** - * Mainloop of position controller. - */ -static int flow_speed_control_thread_main(int argc, char *argv[]); - -/** - * Print the correct usage. - */ -static void usage(const char *reason); - -static void -usage(const char *reason) -{ - if (reason) - fprintf(stderr, "%s\n", reason); - fprintf(stderr, "usage: deamon {start|stop|status} [-p ]\n\n"); - exit(1); -} - -/** - * The deamon app only briefly exists to start - * the background job. The stack size assigned in the - * Makefile does only apply to this management task. - * - * The actual stack size should be set in the call - * to task_spawn_cmd(). - */ -int flow_speed_control_main(int argc, char *argv[]) -{ - if (argc < 1) - usage("missing command"); - - if (!strcmp(argv[1], "start")) - { - if (thread_running) - { - printf("flow speed control already running\n"); - /* this is not an error */ - exit(0); - } - - thread_should_exit = false; - deamon_task = task_spawn_cmd("flow_speed_control", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 6, - 4096, - flow_speed_control_thread_main, - (argv) ? (const char **)&argv[2] : (const char **)NULL); - exit(0); - } - - if (!strcmp(argv[1], "stop")) - { - thread_should_exit = true; - exit(0); - } - - if (!strcmp(argv[1], "status")) - { - if (thread_running) - printf("\tflow speed control app is running\n"); - else - printf("\tflow speed control app not started\n"); - - exit(0); - } - - usage("unrecognized command"); - exit(1); -} - -static int -flow_speed_control_thread_main(int argc, char *argv[]) -{ - /* welcome user */ - thread_running = true; - static int mavlink_fd; - mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); - mavlink_log_info(mavlink_fd,"[fsc] started"); - - uint32_t counter = 0; - - /* structures */ - struct actuator_armed_s armed; - memset(&armed, 0, sizeof(armed)); - struct vehicle_control_mode_s control_mode; - memset(&control_mode, 0, sizeof(control_mode)); - struct filtered_bottom_flow_s filtered_flow; - memset(&filtered_flow, 0, sizeof(filtered_flow)); - struct vehicle_bodyframe_speed_setpoint_s speed_sp; - memset(&speed_sp, 0, sizeof(speed_sp)); - struct vehicle_attitude_setpoint_s att_sp; - memset(&att_sp, 0, sizeof(att_sp)); - - /* subscribe to attitude, motor setpoints and system state */ - int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update)); - int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - int armed_sub = orb_subscribe(ORB_ID(actuator_armed)); - int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); - int filtered_bottom_flow_sub = orb_subscribe(ORB_ID(filtered_bottom_flow)); - int vehicle_bodyframe_speed_setpoint_sub = orb_subscribe(ORB_ID(vehicle_bodyframe_speed_setpoint)); - - orb_advert_t att_sp_pub; - bool attitude_setpoint_adverted = false; - - /* parameters init*/ - struct flow_speed_control_params params; - struct flow_speed_control_param_handles param_handles; - parameters_init(¶m_handles); - parameters_update(¶m_handles, ¶ms); - - /* register the perf counter */ - perf_counter_t mc_loop_perf = perf_alloc(PC_ELAPSED, "flow_speed_control_runtime"); - perf_counter_t mc_interval_perf = perf_alloc(PC_INTERVAL, "flow_speed_control_interval"); - perf_counter_t mc_err_perf = perf_alloc(PC_COUNT, "flow_speed_control_err"); - - static bool sensors_ready = false; - static bool status_changed = false; - - while (!thread_should_exit) - { - /* wait for first attitude msg to be sure all data are available */ - if (sensors_ready) - { - /* polling */ - struct pollfd fds[2] = { - { .fd = vehicle_bodyframe_speed_setpoint_sub, .events = POLLIN }, // speed setpoint from pos controller - { .fd = parameter_update_sub, .events = POLLIN } - }; - - /* wait for a position update, check for exit condition every 5000 ms */ - int ret = poll(fds, 2, 500); - - if (ret < 0) - { - /* poll error, count it in perf */ - perf_count(mc_err_perf); - } - else if (ret == 0) - { - /* no return value, ignore */ -// printf("[flow speed control] no bodyframe speed setpoints updates\n"); - } - else - { - /* parameter update available? */ - if (fds[1].revents & POLLIN) - { - /* read from param to clear updated flag */ - struct parameter_update_s update; - orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update); - - parameters_update(¶m_handles, ¶ms); - mavlink_log_info(mavlink_fd,"[fsp] parameters updated."); - } - - /* only run controller if position/speed changed */ - if (fds[0].revents & POLLIN) - { - perf_begin(mc_loop_perf); - - /* get a local copy of the armed topic */ - orb_copy(ORB_ID(actuator_armed), armed_sub, &armed); - /* get a local copy of the control mode */ - orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode); - /* get a local copy of filtered bottom flow */ - orb_copy(ORB_ID(filtered_bottom_flow), filtered_bottom_flow_sub, &filtered_flow); - /* get a local copy of bodyframe speed setpoint */ - orb_copy(ORB_ID(vehicle_bodyframe_speed_setpoint), vehicle_bodyframe_speed_setpoint_sub, &speed_sp); - /* get a local copy of control mode */ - orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode); - - if (control_mode.flag_control_velocity_enabled) - { - /* calc new roll/pitch */ - float pitch_body = -(speed_sp.vx - filtered_flow.vx) * params.speed_p; - float roll_body = (speed_sp.vy - filtered_flow.vy) * params.speed_p; - - if(status_changed == false) - mavlink_log_info(mavlink_fd,"[fsc] flow SPEED control engaged"); - - status_changed = true; - - /* limit roll and pitch corrections */ - if((pitch_body <= params.limit_pitch) && (pitch_body >= -params.limit_pitch)) - { - att_sp.pitch_body = pitch_body; - } - else - { - if(pitch_body > params.limit_pitch) - att_sp.pitch_body = params.limit_pitch; - if(pitch_body < -params.limit_pitch) - att_sp.pitch_body = -params.limit_pitch; - } - - if((roll_body <= params.limit_roll) && (roll_body >= -params.limit_roll)) - { - att_sp.roll_body = roll_body; - } - else - { - if(roll_body > params.limit_roll) - att_sp.roll_body = params.limit_roll; - if(roll_body < -params.limit_roll) - att_sp.roll_body = -params.limit_roll; - } - - /* set yaw setpoint forward*/ - att_sp.yaw_body = speed_sp.yaw_sp; - - /* add trim from parameters */ - att_sp.roll_body = att_sp.roll_body + params.trim_roll; - att_sp.pitch_body = att_sp.pitch_body + params.trim_pitch; - - att_sp.thrust = speed_sp.thrust_sp; - att_sp.timestamp = hrt_absolute_time(); - - /* publish new attitude setpoint */ - if(isfinite(att_sp.pitch_body) && isfinite(att_sp.roll_body) && isfinite(att_sp.yaw_body) && isfinite(att_sp.thrust)) - { - if (attitude_setpoint_adverted) - { - orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); - } - else - { - att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); - attitude_setpoint_adverted = true; - } - } - else - { - warnx("NaN in flow speed controller!"); - } - } - else - { - if(status_changed == true) - mavlink_log_info(mavlink_fd,"[fsc] flow SPEED controller disengaged."); - - status_changed = false; - - /* reset attitude setpoint */ - att_sp.roll_body = 0.0f; - att_sp.pitch_body = 0.0f; - att_sp.thrust = 0.0f; - att_sp.yaw_body = 0.0f; - } - - /* measure in what intervals the controller runs */ - perf_count(mc_interval_perf); - perf_end(mc_loop_perf); - } - } - - counter++; - } - else - { - /* sensors not ready waiting for first attitude msg */ - - /* polling */ - struct pollfd fds[1] = { - { .fd = vehicle_attitude_sub, .events = POLLIN }, - }; - - /* wait for a flow msg, check for exit condition every 5 s */ - int ret = poll(fds, 1, 5000); - - if (ret < 0) - { - /* poll error, count it in perf */ - perf_count(mc_err_perf); - } - else if (ret == 0) - { - /* no return value, ignore */ - mavlink_log_info(mavlink_fd,"[fsc] no attitude received."); - } - else - { - if (fds[0].revents & POLLIN) - { - sensors_ready = true; - mavlink_log_info(mavlink_fd,"[fsp] initialized."); - } - } - } - } - - mavlink_log_info(mavlink_fd,"[fsc] ending now..."); - - thread_running = false; - - close(parameter_update_sub); - close(vehicle_attitude_sub); - close(vehicle_bodyframe_speed_setpoint_sub); - close(filtered_bottom_flow_sub); - close(armed_sub); - close(control_mode_sub); - close(att_sp_pub); - - perf_print_counter(mc_loop_perf); - perf_free(mc_loop_perf); - - fflush(stdout); - return 0; -} diff --git a/src/examples/flow_speed_control/flow_speed_control_params.c b/src/examples/flow_speed_control/flow_speed_control_params.c deleted file mode 100644 index 8dfe541736..0000000000 --- a/src/examples/flow_speed_control/flow_speed_control_params.c +++ /dev/null @@ -1,70 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. - * Author: Samuel Zihlmann - * Lorenz Meier - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/* - * @file flow_speed_control_params.c - * - */ - -#include "flow_speed_control_params.h" - -/* controller parameters */ -PARAM_DEFINE_FLOAT(FSC_S_P, 0.1f); -PARAM_DEFINE_FLOAT(FSC_L_PITCH, 0.4f); -PARAM_DEFINE_FLOAT(FSC_L_ROLL, 0.4f); - -int parameters_init(struct flow_speed_control_param_handles *h) -{ - /* PID parameters */ - h->speed_p = param_find("FSC_S_P"); - h->limit_pitch = param_find("FSC_L_PITCH"); - h->limit_roll = param_find("FSC_L_ROLL"); - h->trim_roll = param_find("TRIM_ROLL"); - h->trim_pitch = param_find("TRIM_PITCH"); - - - return OK; -} - -int parameters_update(const struct flow_speed_control_param_handles *h, struct flow_speed_control_params *p) -{ - param_get(h->speed_p, &(p->speed_p)); - param_get(h->limit_pitch, &(p->limit_pitch)); - param_get(h->limit_roll, &(p->limit_roll)); - param_get(h->trim_roll, &(p->trim_roll)); - param_get(h->trim_pitch, &(p->trim_pitch)); - - return OK; -} diff --git a/src/examples/flow_speed_control/flow_speed_control_params.h b/src/examples/flow_speed_control/flow_speed_control_params.h deleted file mode 100644 index eec27a2bf7..0000000000 --- a/src/examples/flow_speed_control/flow_speed_control_params.h +++ /dev/null @@ -1,70 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. - * Author: Samuel Zihlmann - * Lorenz Meier - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/* - * @file flow_speed_control_params.h - * - * Parameters for speed controller - */ - -#include - -struct flow_speed_control_params { - float speed_p; - float limit_pitch; - float limit_roll; - float trim_roll; - float trim_pitch; -}; - -struct flow_speed_control_param_handles { - param_t speed_p; - param_t limit_pitch; - param_t limit_roll; - param_t trim_roll; - param_t trim_pitch; -}; - -/** - * Initialize all parameter handles and values - * - */ -int parameters_init(struct flow_speed_control_param_handles *h); - -/** - * Update all parameters - * - */ -int parameters_update(const struct flow_speed_control_param_handles *h, struct flow_speed_control_params *p); diff --git a/src/examples/flow_speed_control/module.mk b/src/examples/flow_speed_control/module.mk deleted file mode 100644 index 5a41821460..0000000000 --- a/src/examples/flow_speed_control/module.mk +++ /dev/null @@ -1,41 +0,0 @@ -############################################################################ -# -# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - -# -# Build flow speed control -# - -MODULE_COMMAND = flow_speed_control - -SRCS = flow_speed_control_main.c \ - flow_speed_control_params.c diff --git a/uavcan b/uavcan index 4de0338824..1efd244275 160000 --- a/uavcan +++ b/uavcan @@ -1 +1 @@ -Subproject commit 4de0338824972de4d3a8e29697ea1a2d95a926c0 +Subproject commit 1efd24427539fa332a15151143466ec760fa5fff From be5bd6f97850307c82351184a52ca49b38cf49a4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 12 Dec 2014 14:07:24 +0100 Subject: [PATCH 114/216] Remove unmaintained math demo --- src/examples/math_demo/math_demo.cpp | 106 --------------------------- src/examples/math_demo/module.mk | 41 ----------- 2 files changed, 147 deletions(-) delete mode 100644 src/examples/math_demo/math_demo.cpp delete mode 100644 src/examples/math_demo/module.mk diff --git a/src/examples/math_demo/math_demo.cpp b/src/examples/math_demo/math_demo.cpp deleted file mode 100644 index 36d3c2e849..0000000000 --- a/src/examples/math_demo/math_demo.cpp +++ /dev/null @@ -1,106 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. - * Author: James Goppert - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file math_demo.cpp - * @author James Goppert - * Demonstration of math library - */ - -#include -#include -#include -#include -#include -#include -#include - -/** - * Management function. - */ -extern "C" __EXPORT int math_demo_main(int argc, char *argv[]); - -/** - * Test function - */ -void test(); - -/** - * Print the correct usage. - */ -static void usage(const char *reason); - -static void -usage(const char *reason) -{ - if (reason) - fprintf(stderr, "%s\n", reason); - fprintf(stderr, "usage: math_demo {test}\n\n"); - exit(1); -} - -/** - * The deamon app only briefly exists to start - * the background job. The stack size assigned in the - * Makefile does only apply to this management task. - * - * The actual stack size should be set in the call - * to task_create(). - */ -int math_demo_main(int argc, char *argv[]) -{ - - if (argc < 1) - usage("missing command"); - - if (!strcmp(argv[1], "test")) { - test(); - exit(0); - } - - usage("unrecognized command"); - exit(1); -} - -void test() -{ - printf("beginning math lib test\n"); - using namespace math; - vectorTest(); - matrixTest(); - vector3Test(); - eulerAnglesTest(); - quaternionTest(); - dcmTest(); -} diff --git a/src/examples/math_demo/module.mk b/src/examples/math_demo/module.mk deleted file mode 100644 index deba13fd08..0000000000 --- a/src/examples/math_demo/module.mk +++ /dev/null @@ -1,41 +0,0 @@ -############################################################################ -# -# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - -# -# Mathlib / operations demo application -# - -MODULE_COMMAND = math_demo -MODULE_STACKSIZE = 12000 - -SRCS = math_demo.cpp From 691e42324cc9d29f9a7dc57dc61316ebf6b092a1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 12 Dec 2014 14:07:42 +0100 Subject: [PATCH 115/216] Fix build breakage in FW control example --- src/examples/fixedwing_control/main.c | 153 ++++---------------------- 1 file changed, 22 insertions(+), 131 deletions(-) diff --git a/src/examples/fixedwing_control/main.c b/src/examples/fixedwing_control/main.c index 067d773649..6a5cbcd307 100644 --- a/src/examples/fixedwing_control/main.c +++ b/src/examples/fixedwing_control/main.c @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier + * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -31,6 +30,7 @@ * POSSIBILITY OF SUCH DAMAGE. * ****************************************************************************/ + /** * @file main.c * @@ -55,7 +55,7 @@ #include #include #include -#include +#include #include #include #include @@ -106,11 +106,9 @@ static void usage(const char *reason); * * @param att_sp The current attitude setpoint - the values the system would like to reach. * @param att The current attitude. The controller should make the attitude match the setpoint - * @param speed_body The velocity of the system. Currently unused. * @param rates_sp The angular rate setpoint. This is the output of the controller. */ -void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att, - float speed_body[], struct vehicle_rates_setpoint_s *rates_sp, +void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, struct actuator_controls_s *actuators); /** @@ -125,7 +123,7 @@ void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const st * @param att The current attitude * @param att_sp The attitude setpoint. This is the output of the controller */ -void control_heading(const struct vehicle_global_position_s *pos, const struct vehicle_global_position_setpoint_s *sp, +void control_heading(const struct vehicle_global_position_s *pos, const struct position_setpoint_s *sp, const struct vehicle_attitude_s *att, struct vehicle_attitude_setpoint_s *att_sp); /* Variables */ @@ -135,8 +133,7 @@ static int deamon_task; /**< Handle of deamon task / thread */ static struct params p; static struct param_handles ph; -void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att, - float speed_body[], struct vehicle_rates_setpoint_s *rates_sp, +void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, struct actuator_controls_s *actuators) { @@ -173,7 +170,7 @@ void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const st actuators->control[1] = pitch_err * p.pitch_p; } -void control_heading(const struct vehicle_global_position_s *pos, const struct vehicle_global_position_setpoint_s *sp, +void control_heading(const struct vehicle_global_position_s *pos, const struct position_setpoint_s *sp, const struct vehicle_attitude_s *att, struct vehicle_attitude_setpoint_s *att_sp) { @@ -186,7 +183,7 @@ void control_heading(const struct vehicle_global_position_s *pos, const struct v /* calculate heading error */ float yaw_err = att->yaw - bearing; /* apply control gain */ - float roll_command = yaw_err * p.hdng_p; + att_sp->roll_body = yaw_err * p.hdng_p; /* limit output, this commonly is a tuning parameter, too */ if (att_sp->roll_body < -0.6f) { @@ -253,7 +250,7 @@ int fixedwing_control_thread_main(int argc, char *argv[]) memset(&manual_sp, 0, sizeof(manual_sp)); struct vehicle_status_s vstatus; memset(&vstatus, 0, sizeof(vstatus)); - struct vehicle_global_position_setpoint_s global_sp; + struct position_setpoint_s global_sp; memset(&global_sp, 0, sizeof(global_sp)); /* output structs - this is what is sent to the mixer */ @@ -275,17 +272,14 @@ int fixedwing_control_thread_main(int argc, char *argv[]) /* subscribe to topics. */ int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - int att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); int global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); int manual_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); int vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); - int global_sp_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint)); + int global_sp_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); int param_sub = orb_subscribe(ORB_ID(parameter_update)); /* Setup of loop */ - float speed_body[3] = {0.0f, 0.0f, 0.0f}; - /* RC failsafe check */ - bool throttle_half_once = false; + struct pollfd fds[2] = {{ .fd = param_sub, .events = POLLIN }, { .fd = att_sub, .events = POLLIN }}; @@ -339,25 +333,10 @@ int fixedwing_control_thread_main(int argc, char *argv[]) /* get a local copy of attitude */ orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); - if (global_sp_updated) - orb_copy(ORB_ID(vehicle_global_position_setpoint), global_sp_sub, &global_sp); - - /* currently speed in body frame is not used, but here for reference */ - if (pos_updated) { - orb_copy(ORB_ID(vehicle_global_position), global_pos_sub, &global_pos); - - if (att.R_valid) { - speed_body[0] = att.R[0][0] * global_pos.vx + att.R[0][1] * global_pos.vy + att.R[0][2] * global_pos.vz; - speed_body[1] = att.R[1][0] * global_pos.vx + att.R[1][1] * global_pos.vy + att.R[1][2] * global_pos.vz; - speed_body[2] = att.R[2][0] * global_pos.vx + att.R[2][1] * global_pos.vy + att.R[2][2] * global_pos.vz; - - } else { - speed_body[0] = 0; - speed_body[1] = 0; - speed_body[2] = 0; - - warnx("Did not get a valid R\n"); - } + if (global_sp_updated) { + struct position_setpoint_triplet_s triplet; + orb_copy(ORB_ID(position_setpoint_triplet), global_sp_sub, &triplet); + memcpy(&global_sp, &triplet.current, sizeof(global_sp)); } if (manual_sp_updated) @@ -365,106 +344,14 @@ int fixedwing_control_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(manual_control_setpoint), manual_sp_sub, &manual_sp); /* check if the throttle was ever more than 50% - go later only to failsafe if yes */ - if (isfinite(manual_sp.throttle) && - (manual_sp.throttle >= 0.6f) && - (manual_sp.throttle <= 1.0f)) { - throttle_half_once = true; + if (isfinite(manual_sp.z) && + (manual_sp.z >= 0.6f) && + (manual_sp.z <= 1.0f)) { } /* get the system status and the flight mode we're in */ orb_copy(ORB_ID(vehicle_status), vstatus_sub, &vstatus); - /* control */ - -#warning fix this -#if 0 - if (vstatus.navigation_state == NAVIGATION_STATE_AUTO_ || - vstatus.navigation_state == NAVIGATION_STATE_STABILIZED) { - - /* simple heading control */ - control_heading(&global_pos, &global_sp, &att, &att_sp); - - /* nail pitch and yaw (rudder) to zero. This example only controls roll (index 0) */ - actuators.control[1] = 0.0f; - actuators.control[2] = 0.0f; - - /* simple attitude control */ - control_attitude(&att_sp, &att, speed_body, &rates_sp, &actuators); - - /* pass through throttle */ - actuators.control[3] = att_sp.thrust; - - /* set flaps to zero */ - actuators.control[4] = 0.0f; - - } else if (vstatus.navigation_state == NAVIGATION_STATE_MANUAL) { - /* if in manual mode, decide between attitude stabilization (SAS) and full manual pass-through */ - } else if (vstatus.state_machine == SYSTEM_STATE_MANUAL) { - if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) { - - /* if the RC signal is lost, try to stay level and go slowly back down to ground */ - if (vstatus.rc_signal_lost && throttle_half_once) { - - /* put plane into loiter */ - att_sp.roll_body = 0.3f; - att_sp.pitch_body = 0.0f; - - /* limit throttle to 60 % of last value if sane */ - if (isfinite(manual_sp.throttle) && - (manual_sp.throttle >= 0.0f) && - (manual_sp.throttle <= 1.0f)) { - att_sp.thrust = 0.6f * manual_sp.throttle; - - } else { - att_sp.thrust = 0.0f; - } - - att_sp.yaw_body = 0; - - // XXX disable yaw control, loiter - - } else { - - att_sp.roll_body = manual_sp.roll; - att_sp.pitch_body = manual_sp.pitch; - att_sp.yaw_body = 0; - att_sp.thrust = manual_sp.throttle; - } - - att_sp.timestamp = hrt_absolute_time(); - - /* attitude control */ - control_attitude(&att_sp, &att, speed_body, &rates_sp, &actuators); - - /* pass through throttle */ - actuators.control[3] = att_sp.thrust; - - /* pass through flaps */ - if (isfinite(manual_sp.flaps)) { - actuators.control[4] = manual_sp.flaps; - - } else { - actuators.control[4] = 0.0f; - } - - } else if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) { - /* directly pass through values */ - actuators.control[0] = manual_sp.roll; - /* positive pitch means negative actuator -> pull up */ - actuators.control[1] = manual_sp.pitch; - actuators.control[2] = manual_sp.yaw; - actuators.control[3] = manual_sp.throttle; - - if (isfinite(manual_sp.flaps)) { - actuators.control[4] = manual_sp.flaps; - - } else { - actuators.control[4] = 0.0f; - } - } - } -#endif - /* publish rates */ orb_publish(ORB_ID(vehicle_rates_setpoint), rates_pub, &rates_sp); @@ -474,6 +361,10 @@ int fixedwing_control_thread_main(int argc, char *argv[]) isfinite(actuators.control[2]) && isfinite(actuators.control[3])) { orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); + + if (verbose) { + warnx("published"); + } } } } From cc6224de6500072a8a9523dce19ccc3a369eca1a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 12 Dec 2014 14:08:02 +0100 Subject: [PATCH 116/216] Fix / update HW test example --- src/examples/hwtest/hwtest.c | 92 ++++++++++++++++++++++++++++-------- 1 file changed, 71 insertions(+), 21 deletions(-) diff --git a/src/examples/hwtest/hwtest.c b/src/examples/hwtest/hwtest.c index 06337be32d..d3b10f46ed 100644 --- a/src/examples/hwtest/hwtest.c +++ b/src/examples/hwtest/hwtest.c @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier + * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -34,7 +33,8 @@ /** * @file hwtest.c * - * Simple functional hardware test. + * Simple output test. + * @ref Documentation https://pixhawk.org/dev/examples/write_output * * @author Lorenz Meier */ @@ -45,30 +45,80 @@ #include #include #include +#include __EXPORT int ex_hwtest_main(int argc, char *argv[]); int ex_hwtest_main(int argc, char *argv[]) { - struct actuator_controls_s actuators; - memset(&actuators, 0, sizeof(actuators)); - orb_advert_t actuator_pub_fd = orb_advertise(ORB_ID(actuator_controls_0), &actuators); + warnx("DO NOT FORGET TO STOP THE COMMANDER APP!"); + warnx("(run to do so)"); + warnx("usage: http://px4.io/dev/examples/write_output"); - int i; - float rcvalue = -1.0f; - hrt_abstime stime; + struct actuator_controls_s actuators; + memset(&actuators, 0, sizeof(actuators)); + orb_advert_t actuator_pub_fd = orb_advertise(ORB_ID(actuator_controls_0), &actuators); - while (true) { - stime = hrt_absolute_time(); - while (hrt_absolute_time() - stime < 1000000) { - for (i=0; i<8; i++) - actuators.control[i] = rcvalue; - actuators.timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(actuator_controls_0), actuator_pub_fd, &actuators); - } - warnx("servos set to %.1f", rcvalue); - rcvalue *= -1.0f; - } + struct actuator_armed_s arm; + memset(&arm, 0 , sizeof(arm)); - return OK; + arm.timestamp = hrt_absolute_time(); + arm.ready_to_arm = true; + arm.armed = true; + orb_advert_t arm_pub_fd = orb_advertise(ORB_ID(actuator_armed), &arm); + orb_publish(ORB_ID(actuator_armed), arm_pub_fd, &arm); + + /* read back values to validate */ + int arm_sub_fd = orb_subscribe(ORB_ID(actuator_armed)); + orb_copy(ORB_ID(actuator_armed), arm_sub_fd, &arm); + + if (arm.ready_to_arm && arm.armed) { + warnx("Actuator armed"); + + } else { + errx(1, "Arming actuators failed"); + } + + hrt_abstime stime; + + int count = 0; + + while (count != 36) { + stime = hrt_absolute_time(); + + while (hrt_absolute_time() - stime < 1000000) { + for (int i = 0; i != 2; i++) { + if (count <= 5) { + actuators.control[i] = -1.0f; + + } else if (count <= 10) { + actuators.control[i] = -0.7f; + + } else if (count <= 15) { + actuators.control[i] = -0.5f; + + } else if (count <= 20) { + actuators.control[i] = -0.3f; + + } else if (count <= 25) { + actuators.control[i] = 0.0f; + + } else if (count <= 30) { + actuators.control[i] = 0.3f; + + } else { + actuators.control[i] = 0.5f; + } + } + + actuators.timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(actuator_controls_0), actuator_pub_fd, &actuators); + usleep(10000); + } + + warnx("count %i", count); + count++; + } + + return OK; } From ef450cc447a93a5bf4537738fda75d85374e570d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 12 Dec 2014 14:08:41 +0100 Subject: [PATCH 117/216] Enable examples by default to ensure they get maintained on API changes --- makefiles/config_px4fmu-v2_test.mk | 17 +++++++++++++---- 1 file changed, 13 insertions(+), 4 deletions(-) diff --git a/makefiles/config_px4fmu-v2_test.mk b/makefiles/config_px4fmu-v2_test.mk index 6f54b960c1..bc6723e5f5 100644 --- a/makefiles/config_px4fmu-v2_test.mk +++ b/makefiles/config_px4fmu-v2_test.mk @@ -50,16 +50,25 @@ MODULES += lib/mathlib/math/filter MODULES += lib/conversion # -# Modules to test-build, but not useful for test environment +# Example modules to test-build +# +MODULES += examples/flow_position_estimator +MODULES += examples/fixedwing_control +MODULES += examples/hwtest +MODULES += examples/matlab_csv_serial +MODULES += examples/px4_daemon_app +MODULES += examples/px4_mavlink_debug +MODULES += examples/px4_simple_app + +# +# Drivers / modules to test build, but not useful for test environment # MODULES += modules/attitude_estimator_so3 MODULES += drivers/pca8574 -MODULES += examples/flow_position_estimator # -# Libraries +# Tests # -LIBRARIES += lib/mathlib/CMSIS MODULES += modules/unit_test MODULES += modules/mavlink/mavlink_tests From cd3e9186292586a158405ab75dd0e194e81c0fff Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 12 Dec 2014 14:36:02 +0100 Subject: [PATCH 118/216] Update NuttX with critical bugfix from Greg --- NuttX | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/NuttX b/NuttX index ae4b05e2c5..c7b0638592 160000 --- a/NuttX +++ b/NuttX @@ -1 +1 @@ -Subproject commit ae4b05e2c51d07369b5d131052099ac346b0841c +Subproject commit c7b06385926349e10b3739314d1d56eec7efb8be From 4c25051edc4495f82927441501598c28a6834d1f Mon Sep 17 00:00:00 2001 From: Friedemann Ludwig Date: Fri, 12 Dec 2014 22:36:01 +0100 Subject: [PATCH 119/216] changed telementry connection lost warnings to info in order to avoid audio message flooding in case of instable connections. --- src/modules/commander/commander.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index c9c8d16b50..6d6b3a36ce 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1668,7 +1668,7 @@ int commander_thread_main(int argc, char *argv[]) if (telemetry_lost[i] && hrt_elapsed_time(&telemetry_last_dl_loss[i]) > datalink_regain_timeout * 1e6) { - mavlink_log_critical(mavlink_fd, "data link %i regained", i); + mavlink_log_info(mavlink_fd, "data link %i regained", i); telemetry_lost[i] = false; have_link = true; @@ -1682,7 +1682,7 @@ int commander_thread_main(int argc, char *argv[]) telemetry_last_dl_loss[i] = hrt_absolute_time(); if (!telemetry_lost[i]) { - mavlink_log_critical(mavlink_fd, "data link %i lost", i); + mavlink_log_info(mavlink_fd, "data link %i lost", i); telemetry_lost[i] = true; } } @@ -1697,7 +1697,7 @@ int commander_thread_main(int argc, char *argv[]) } else { if (!status.data_link_lost) { - mavlink_log_critical(mavlink_fd, "ALL DATA LINKS LOST"); + mavlink_log_info(mavlink_fd, "ALL DATA LINKS LOST"); status.data_link_lost = true; status.data_link_lost_counter++; status_changed = true; From 4bb07514c86d020bc2b16058e75de3074f89aa4b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 12 Dec 2014 23:22:21 +0100 Subject: [PATCH 120/216] Give Skywalker its own mixer --- ROMFS/px4fmu_common/init.d/2102_3dr_skywalker | 2 +- ROMFS/px4fmu_common/mixers/skywalker.mix | 64 +++++++++++++++++++ 2 files changed, 65 insertions(+), 1 deletion(-) create mode 100644 ROMFS/px4fmu_common/mixers/skywalker.mix diff --git a/ROMFS/px4fmu_common/init.d/2102_3dr_skywalker b/ROMFS/px4fmu_common/init.d/2102_3dr_skywalker index dcc5db824a..906f4b1ccf 100644 --- a/ROMFS/px4fmu_common/init.d/2102_3dr_skywalker +++ b/ROMFS/px4fmu_common/init.d/2102_3dr_skywalker @@ -2,4 +2,4 @@ sh /etc/init.d/rc.fw_defaults -set MIXER FMU_AERT \ No newline at end of file +set MIXER skywalker \ No newline at end of file diff --git a/ROMFS/px4fmu_common/mixers/skywalker.mix b/ROMFS/px4fmu_common/mixers/skywalker.mix new file mode 100644 index 0000000000..04d677e566 --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/skywalker.mix @@ -0,0 +1,64 @@ +Mixer for Skywalker Airframe +================================================== + +This file defines mixers suitable for controlling a fixed wing aircraft with +aileron, rudder, elevator and throttle controls using PX4FMU. The configuration +assumes the aileron servo(s) are connected to PX4FMU servo output 0, the +elevator to output 1, the rudder to output 2 and the throttle to output 3. + +Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0 +(roll), 1 (pitch) and 3 (thrust). + +Aileron mixer +------------- +Two scalers total (output, roll). + +This mixer assumes that the aileron servos are set up correctly mechanically; +depending on the actual configuration it may be necessary to reverse the scaling +factors (to reverse the servo movement) and adjust the offset, scaling and +endpoints to suit. + +As there is only one output, if using two servos adjustments to compensate for +differences between the servos must be made mechanically. To obtain the correct +motion using a Y cable, the servos can be positioned reversed from one another. + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 0 -10000 -10000 0 -10000 10000 + +Elevator mixer +------------ +Two scalers total (output, roll). + +This mixer assumes that the elevator servo is set up correctly mechanically; +depending on the actual configuration it may be necessary to reverse the scaling +factors (to reverse the servo movement) and adjust the offset, scaling and +endpoints to suit. + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 1 10000 10000 0 -10000 10000 + +Rudder mixer +------------ +Two scalers total (output, yaw). + +This mixer assumes that the rudder servo is set up correctly mechanically; +depending on the actual configuration it may be necessary to reverse the scaling +factors (to reverse the servo movement) and adjust the offset, scaling and +endpoints to suit. + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 2 10000 10000 0 -10000 10000 + +Motor speed mixer +----------------- +Two scalers total (output, thrust). + +This mixer generates a full-range output (-1 to 1) from an input in the (0 - 1) +range. Inputs below zero are treated as zero. + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 3 0 20000 -10000 -10000 10000 From 59ec2401b6f8e6714d515d3d0f1cf2e0ee14b8bc Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 13 Dec 2014 00:48:27 +0100 Subject: [PATCH 121/216] fw pos control: better check for control mode --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index e7c95cc864..1bb27168e0 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -909,10 +909,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi float throttle_max = 1.0f; /* AUTONOMOUS FLIGHT */ - - // XXX this should only execute if auto AND safety off (actuators active), - // else integrators should be constantly reset. - if (pos_sp_triplet.current.valid) { + if (_control_mode.flag_control_auto_enabled && + pos_sp_triplet.current.valid) { if (!_was_pos_control_mode) { /* reset integrators */ @@ -1248,7 +1246,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _att_sp.roll_reset_integral = true; } - } else if (_control_mode.flag_control_velocity_enabled) { + } else if (_control_mode.flag_control_velocity_enabled && + _control_mode.flag_control_altitude_enabled) { const float deadBand = (60.0f/1000.0f); const float factor = 1.0f - deadBand; if (!_was_velocity_control_mode) { From 5cd2ee83421e79c9f283d46b788343a479418d8a Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 13 Dec 2014 12:36:33 +0100 Subject: [PATCH 122/216] fw pos control: improve mode logic slightly --- .../fw_pos_control_l1_main.cpp | 62 +++++++++++-------- 1 file changed, 36 insertions(+), 26 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 1bb27168e0..1592e70d84 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -200,9 +200,11 @@ private: ECL_L1_Pos_Controller _l1_control; TECS _tecs; fwPosctrl::mTecs _mTecs; - bool _was_pos_control_mode; - bool _was_velocity_control_mode; - bool _was_alt_control_mode; + enum FW_POSCTRL_MODE { + FW_POSCTRL_MODE_AUTO, + FW_POSCTRL_MODE_POSITION, + FW_POSCTRL_MODE_OTHER + } _control_mode_current; ///< used to check the mode in the last control loop iteration. Use to check if the last iteration was in the same mode. struct { float l1_period; @@ -471,7 +473,7 @@ FixedwingPositionControl::FixedwingPositionControl() : _global_pos_valid(false), _l1_control(), _mTecs(), - _was_pos_control_mode(false) + _control_mode_current(FW_POSCTRL_MODE_OTHER) { _nav_capabilities.turn_distance = 0.0f; @@ -908,20 +910,19 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* no throttle limit as default */ float throttle_max = 1.0f; - /* AUTONOMOUS FLIGHT */ if (_control_mode.flag_control_auto_enabled && pos_sp_triplet.current.valid) { + /* AUTONOMOUS FLIGHT */ - if (!_was_pos_control_mode) { + /* Reset integrators if switching to this mode from a other mode in which posctl was not active */ + if (_control_mode_current == FW_POSCTRL_MODE_OTHER) { /* reset integrators */ if (_mTecs.getEnabled()) { _mTecs.resetIntegrators(); _mTecs.resetDerivatives(_airspeed.true_airspeed_m_s); } } - - _was_pos_control_mode = true; - _was_velocity_control_mode = false; + _control_mode_current = FW_POSCTRL_MODE_AUTO; /* reset hold altitude */ _hold_alt = _global_pos.alt; @@ -1248,35 +1249,45 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } else if (_control_mode.flag_control_velocity_enabled && _control_mode.flag_control_altitude_enabled) { + /* POSITION CONTROL: pitch stick moves altitude setpoint, throttle stick sets airspeed */ + const float deadBand = (60.0f/1000.0f); const float factor = 1.0f - deadBand; - if (!_was_velocity_control_mode) { + if (_control_mode_current != FW_POSCTRL_MODE_POSITION) { + /* Need to init because last loop iteration was in a different mode */ _hold_alt = _global_pos.alt; - _was_alt_control_mode = false; } - _was_velocity_control_mode = true; - _was_pos_control_mode = false; + /* Reset integrators if switching to this mode from a other mode in which posctl was not active */ + if (_control_mode_current == FW_POSCTRL_MODE_OTHER) { + /* reset integrators */ + if (_mTecs.getEnabled()) { + _mTecs.resetIntegrators(); + _mTecs.resetDerivatives(_airspeed.true_airspeed_m_s); + } + } + _control_mode_current = FW_POSCTRL_MODE_POSITION; + // Get demanded airspeed float altctrl_airspeed = _parameters.airspeed_min + (_parameters.airspeed_max - _parameters.airspeed_min) * _manual.z; // Get demanded vertical velocity from pitch control - float pitch = 0.0f; + static bool was_in_deadband = false; if (_manual.x > deadBand) { - pitch = (_manual.x - deadBand) / factor; - } else if (_manual.x < - deadBand) { - pitch = (_manual.x + deadBand) / factor; - } - if (pitch > 0.0f) { + float pitch = (_manual.x - deadBand) / factor; _hold_alt -= (_parameters.max_climb_rate * dt) * pitch; - _was_alt_control_mode = false; - } else if (pitch < 0.0f) { + was_in_deadband = false; + } else if (_manual.x < - deadBand) { + float pitch = (_manual.x + deadBand) / factor; _hold_alt -= (_parameters.max_sink_rate * dt) * pitch; - _was_alt_control_mode = false; - } else if (!_was_alt_control_mode) { + was_in_deadband = false; + } else if (!was_in_deadband) { + /* store altitude at which manual.x was inside deadBand + * The aircraft should immediately try to fly at this altitude + * as this is what the pilot expects when he moves the stick to the center */ _hold_alt = _global_pos.alt; - _was_alt_control_mode = true; + was_in_deadband = true; } tecs_update_pitch_throttle(_hold_alt, altctrl_airspeed, @@ -1292,8 +1303,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi ground_speed, TECS_MODE_NORMAL); } else { - _was_velocity_control_mode = false; - _was_pos_control_mode = false; + _control_mode_current = FW_POSCTRL_MODE_OTHER; /** MANUAL FLIGHT **/ From e1c12bf67cbe8a628fd5319541d7df9f318f5942 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 13 Dec 2014 15:33:42 +0100 Subject: [PATCH 123/216] ROMFS: rc.interface: Make output less verbose to clutter boot log less --- ROMFS/px4fmu_common/init.d/rc.interface | 4 ---- 1 file changed, 4 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.interface b/ROMFS/px4fmu_common/init.d/rc.interface index 0f703c3b38..bab71be930 100644 --- a/ROMFS/px4fmu_common/init.d/rc.interface +++ b/ROMFS/px4fmu_common/init.d/rc.interface @@ -55,7 +55,6 @@ then # if [ $PWM_RATE != none ] then - echo "[i] PWM rate: $PWM_RATE" pwm rate -c $PWM_OUT -r $PWM_RATE fi @@ -64,17 +63,14 @@ then # if [ $PWM_DISARMED != none ] then - echo "[i] PWM disarmed: $PWM_DISARMED" pwm disarmed -c $PWM_OUT -p $PWM_DISARMED fi if [ $PWM_MIN != none ] then - echo "[i] PWM min: $PWM_MIN" pwm min -c $PWM_OUT -p $PWM_MIN fi if [ $PWM_MAX != none ] then - echo "[i] PWM max: $PWM_MAX" pwm max -c $PWM_OUT -p $PWM_MAX fi fi From 24fd099e58029dd275d2f30fc65bb9209bb26983 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 13 Dec 2014 15:57:38 +0100 Subject: [PATCH 124/216] ROMFS: Better commenting, save some more RAM --- ROMFS/px4fmu_common/init.d/rcS | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 7532d38115..94c462439a 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -1,6 +1,9 @@ #!nsh # # PX4FMU startup script. +# +# NOTE: COMMENT LINES ARE REMOVED BEFORE STORED IN ROMFS. +# # # Default to auto-start mode. @@ -480,7 +483,7 @@ then set MAV_TYPE 1 fi - #param set MAV_TYPE $MAV_TYPE + param set MAV_TYPE $MAV_TYPE # Load mixer and configure outputs sh /etc/init.d/rc.interface @@ -580,6 +583,11 @@ then echo "Exit from nsh" exit fi + unset EXIT_ON_END # End of autostart fi + +# There is no further processing, so we can free some RAM +# XXX potentially unset all script variables. +unset TUNE_ERR From d0325f2b125a3573231443b7b4bac04b65081426 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 13 Dec 2014 17:13:20 +0100 Subject: [PATCH 125/216] fw pos ctrl: takeoff special case only in takeoff --- .../fw_pos_control_l1_main.cpp | 51 ++++++++++--------- 1 file changed, 27 insertions(+), 24 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 1592e70d84..eec37f80c2 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -1207,15 +1207,15 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim), eas2tas, - math::radians(_parameters.pitch_limit_min), - math::radians(_parameters.pitch_limit_max), - _parameters.throttle_min, - takeoff_throttle, - _parameters.throttle_cruise, - false, - math::radians(_parameters.pitch_limit_min), - _global_pos.alt, - ground_speed); + math::radians(_parameters.pitch_limit_min), + math::radians(_parameters.pitch_limit_max), + _parameters.throttle_min, + takeoff_throttle, + _parameters.throttle_cruise, + false, + math::radians(_parameters.pitch_limit_min), + _global_pos.alt, + ground_speed); } } else { /* Tell the attitude controller to stop integrating while we are waiting @@ -1290,18 +1290,18 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi was_in_deadband = true; } tecs_update_pitch_throttle(_hold_alt, - altctrl_airspeed, - eas2tas, - math::radians(_parameters.pitch_limit_min), - math::radians(_parameters.pitch_limit_max), - _parameters.throttle_min, - _parameters.throttle_max, - _parameters.throttle_cruise, - false, - math::radians(_parameters.pitch_limit_min), - _global_pos.alt, - ground_speed, - TECS_MODE_NORMAL); + altctrl_airspeed, + eas2tas, + math::radians(_parameters.pitch_limit_min), + math::radians(_parameters.pitch_limit_max), + _parameters.throttle_min, + _parameters.throttle_max, + _parameters.throttle_cruise, + false, + math::radians(_parameters.pitch_limit_min), + _global_pos.alt, + ground_speed, + TECS_MODE_NORMAL); } else { _control_mode_current = FW_POSCTRL_MODE_OTHER; @@ -1320,10 +1320,12 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } } + /* Copy thrust output for publication */ if (_vehicle_status.engine_failure || _vehicle_status.engine_failure_cmd) { /* Set thrust to 0 to minimize damage */ _att_sp.thrust = 0.0f; - } else if (pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF && + } else if (_control_mode_current == FW_POSCTRL_MODE_AUTO && // launchdetector only available in auot + pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF && launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) { /* making sure again that the correct thrust is used, * without depending on library calls for safety reasons */ @@ -1336,8 +1338,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* During a takeoff waypoint while waiting for launch the pitch sp is set * already (not by tecs) */ - if (!(pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF && - launch_detection_state == LAUNCHDETECTION_RES_NONE)) { + if (!(_control_mode_current == FW_POSCTRL_MODE_AUTO && + pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF && + launch_detection_state == LAUNCHDETECTION_RES_NONE)) { _att_sp.pitch_body = _mTecs.getEnabled() ? _mTecs.getPitchSetpoint() : _tecs.get_pitch_demand(); } From 6f89514beb09c5db3f9c71da22ceed7fe19e09c9 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 13 Dec 2014 17:17:57 +0100 Subject: [PATCH 126/216] fix comment style and type --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index eec37f80c2..167126a2b9 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -1267,12 +1267,12 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } _control_mode_current = FW_POSCTRL_MODE_POSITION; - // Get demanded airspeed + /* Get demanded airspeed */ float altctrl_airspeed = _parameters.airspeed_min + (_parameters.airspeed_max - _parameters.airspeed_min) * _manual.z; - // Get demanded vertical velocity from pitch control + /* Get demanded vertical velocity from pitch control */ static bool was_in_deadband = false; if (_manual.x > deadBand) { float pitch = (_manual.x - deadBand) / factor; @@ -1324,7 +1324,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi if (_vehicle_status.engine_failure || _vehicle_status.engine_failure_cmd) { /* Set thrust to 0 to minimize damage */ _att_sp.thrust = 0.0f; - } else if (_control_mode_current == FW_POSCTRL_MODE_AUTO && // launchdetector only available in auot + } else if (_control_mode_current == FW_POSCTRL_MODE_AUTO && // launchdetector only available in auto pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF && launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) { /* making sure again that the correct thrust is used, From ea4876da38deaecced36dc3d8e79afe85d1c0798 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 13 Dec 2014 18:19:12 +0100 Subject: [PATCH 127/216] mavlink: distance sensor: fix max value --- src/modules/mavlink/mavlink_messages.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 96aa484992..5908279d54 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -2162,7 +2162,7 @@ protected: msg.id = 0; msg.orientation = 0; msg.min_distance = range.minimum_distance * 100; - msg.max_distance = range.minimum_distance * 100; + msg.max_distance = range.maximum_distance * 100; msg.current_distance = range.distance * 100; msg.covariance = 20; From 046ca1f8e3f2f511a00eb708b58c2fd72cb4bfa6 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 13 Dec 2014 18:28:58 +0100 Subject: [PATCH 128/216] ll40ls: fix whitespace --- src/drivers/ll40ls/ll40ls.cpp | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/src/drivers/ll40ls/ll40ls.cpp b/src/drivers/ll40ls/ll40ls.cpp index 6793acd810..a69fc56e9a 100644 --- a/src/drivers/ll40ls/ll40ls.cpp +++ b/src/drivers/ll40ls/ll40ls.cpp @@ -233,11 +233,11 @@ LL40LS::~LL40LS() if (_reports != nullptr) { delete _reports; } - + if (_class_instance != -1) { unregister_class_devname(RANGE_FINDER_DEVICE_PATH, _class_instance); } - + // free perf counters perf_free(_sample_perf); perf_free(_comms_errors); @@ -263,7 +263,7 @@ LL40LS::init() _class_instance = register_class_devname(RANGE_FINDER_DEVICE_PATH); - if (_class_instance == CLASS_DEVICE_PRIMARY) { + if (_class_instance == CLASS_DEVICE_PRIMARY) { /* get a publish handle on the range finder topic */ struct range_finder_report rf_report; measure(); @@ -314,9 +314,9 @@ LL40LS::probe() goto ok; } - debug("WHO_AM_I byte mismatch 0x%02x should be 0x%02x val=0x%02x\n", - (unsigned)who_am_i, - LL40LS_WHO_AM_I_REG_VAL, + debug("WHO_AM_I byte mismatch 0x%02x should be 0x%02x val=0x%02x\n", + (unsigned)who_am_i, + LL40LS_WHO_AM_I_REG_VAL, (unsigned)val); } @@ -704,7 +704,7 @@ LL40LS::print_info() perf_print_counter(_buffer_overflows); printf("poll interval: %u ticks\n", _measure_ticks); _reports->print_info("report queue"); - printf("distance: %ucm (0x%04x)\n", + printf("distance: %ucm (0x%04x)\n", (unsigned)_last_distance, (unsigned)_last_distance); } @@ -969,8 +969,8 @@ ll40ls_main(int argc, char *argv[]) } } - const char *verb = argv[optind]; - + const char *verb = argv[optind]; + /* * Start/load the driver. */ From 4a70da5c2c27f6d41704e99e95697d325f0296a4 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 13 Dec 2014 19:01:00 +0100 Subject: [PATCH 129/216] ll40ls: max distance according to datasheet Datasheet: http://pulsedlight3d.com/pl3d/wp-content/uploads/2014/11/LIDAR-Lite.pdf --- src/drivers/ll40ls/ll40ls.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/ll40ls/ll40ls.cpp b/src/drivers/ll40ls/ll40ls.cpp index a69fc56e9a..ba80f3d954 100644 --- a/src/drivers/ll40ls/ll40ls.cpp +++ b/src/drivers/ll40ls/ll40ls.cpp @@ -89,7 +89,7 @@ /* Device limits */ #define LL40LS_MIN_DISTANCE (0.00f) -#define LL40LS_MAX_DISTANCE (14.00f) +#define LL40LS_MAX_DISTANCE (60.00f) #define LL40LS_CONVERSION_INTERVAL 100000 /* 100ms */ From 29eab8ebd44f96d222c3e8ea6952f8584c17eb63 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 14 Dec 2014 11:28:25 +0100 Subject: [PATCH 130/216] change [init] to [i] This change was introduced in pull #1461. This fixes some missed occurrences. --- ROMFS/px4fmu_common/init.d/rc.io | 2 +- ROMFS/px4fmu_common/init.d/rc.sensors | 2 +- ROMFS/px4fmu_common/init.d/rc.uavcan | 4 ++-- ROMFS/px4fmu_test/init.d/rc.standalone | 4 ++-- ROMFS/px4fmu_test/init.d/rcS | 10 +++++----- 5 files changed, 11 insertions(+), 11 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.io b/ROMFS/px4fmu_common/init.d/rc.io index e23aebd87f..e957626ce0 100644 --- a/ROMFS/px4fmu_common/init.d/rc.io +++ b/ROMFS/px4fmu_common/init.d/rc.io @@ -16,5 +16,5 @@ then set PX4IO_LIMIT 200 fi -echo "[init] Set PX4IO update rate limit: $PX4IO_LIMIT Hz" +echo "[i] Set PX4IO update rate limit: $PX4IO_LIMIT Hz" px4io limit $PX4IO_LIMIT diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index fbac50cf71..461cc856b5 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -56,7 +56,7 @@ fi if meas_airspeed start then - echo "[init] Using MEAS airspeed sensor" + echo "[i] Using MEAS airspeed sensor" else if ets_airspeed start then diff --git a/ROMFS/px4fmu_common/init.d/rc.uavcan b/ROMFS/px4fmu_common/init.d/rc.uavcan index 55a3726096..08ba86d789 100644 --- a/ROMFS/px4fmu_common/init.d/rc.uavcan +++ b/ROMFS/px4fmu_common/init.d/rc.uavcan @@ -10,9 +10,9 @@ then # First sensor publisher to initialize takes lowest instance ID # This delay ensures that UAVCAN-interfaced sensors would be allocated on lowest instance IDs sleep 1 - echo "[init] UAVCAN started" + echo "[i] UAVCAN started" else - echo "[init] ERROR: Could not start UAVCAN" + echo "[i] ERROR: Could not start UAVCAN" tone_alarm $TUNE_ERR fi fi diff --git a/ROMFS/px4fmu_test/init.d/rc.standalone b/ROMFS/px4fmu_test/init.d/rc.standalone index 67e95215b9..5c7470d12d 100644 --- a/ROMFS/px4fmu_test/init.d/rc.standalone +++ b/ROMFS/px4fmu_test/init.d/rc.standalone @@ -3,11 +3,11 @@ # Flight startup script for PX4FMU standalone configuration. # -echo "[init] doing standalone PX4FMU startup..." +echo "[i] doing standalone PX4FMU startup..." # # Start the ORB # uorb start -echo "[init] startup done" +echo "[i] startup done" diff --git a/ROMFS/px4fmu_test/init.d/rcS b/ROMFS/px4fmu_test/init.d/rcS index bc248ac04c..ef032de5cb 100644 --- a/ROMFS/px4fmu_test/init.d/rcS +++ b/ROMFS/px4fmu_test/init.d/rcS @@ -6,7 +6,7 @@ uorb start if sercon then - echo "[init] USB interface connected" + echo "[i] USB interface connected" # Try to get an USB console nshterm /dev/ttyACM0 & @@ -15,14 +15,14 @@ fi # # Try to mount the microSD card. # -echo "[init] looking for microSD..." +echo "[i] looking for microSD..." if mount -t vfat /dev/mmcsd0 /fs/microsd then - echo "[init] card mounted at /fs/microsd" + echo "[i] card mounted at /fs/microsd" # Start playing the startup tune tone_alarm start else - echo "[init] no microSD card found" + echo "[i] no microSD card found" # Play SOS tone_alarm error fi @@ -104,4 +104,4 @@ then else echo echo "Some Unit Tests FAILED:${unit_test_failure_list}" -fi \ No newline at end of file +fi From 642b2088c3175cca24134233c181ad5331222f07 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 14 Dec 2014 11:37:12 +0100 Subject: [PATCH 131/216] autostart ll40ls --- ROMFS/px4fmu_common/init.d/rc.sensors | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index 461cc856b5..e6fc535a6b 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -71,6 +71,10 @@ if px4flow start then fi +if ll40ls start +then +fi + # # Start sensors -> preflight_check # From 682f30afe60baf2b0d8e30c9bf9806f9f601444e Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 13 Dec 2014 18:25:29 +0100 Subject: [PATCH 132/216] mb12xx: write min and max to report --- src/drivers/mb12xx/mb12xx.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/drivers/mb12xx/mb12xx.cpp b/src/drivers/mb12xx/mb12xx.cpp index beb6c8e35e..9cf5686583 100644 --- a/src/drivers/mb12xx/mb12xx.cpp +++ b/src/drivers/mb12xx/mb12xx.cpp @@ -520,6 +520,8 @@ MB12XX::collect() report.timestamp = hrt_absolute_time(); report.error_count = perf_event_count(_comms_errors); report.distance = si_units; + report.minimum_distance = get_minimum_distance(); + report.maximum_distance = get_maximum_distance(); report.valid = si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0; /* publish it, if we are the primary */ From 8ebe463f16326c345f5aeb36738c91b5cf33f1b3 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 13 Dec 2014 18:26:07 +0100 Subject: [PATCH 133/216] sf0x: fix whitespace --- src/drivers/sf0x/sf0x.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp index fdd524189c..45375493ee 100644 --- a/src/drivers/sf0x/sf0x.cpp +++ b/src/drivers/sf0x/sf0x.cpp @@ -545,7 +545,7 @@ SF0X::collect() float si_units; bool valid = false; - + for (int i = 0; i < ret; i++) { if (OK == sf0x_parser(readbuf[i], _linebuf, &_linebuf_index, &_parse_state, &si_units)) { valid = true; From 8fd9f98904c977b1bc41a98b07acb123c976cbac Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 13 Dec 2014 18:26:59 +0100 Subject: [PATCH 134/216] sf0x: write min and max to report --- src/drivers/sf0x/sf0x.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp index 45375493ee..4301750f03 100644 --- a/src/drivers/sf0x/sf0x.cpp +++ b/src/drivers/sf0x/sf0x.cpp @@ -564,6 +564,8 @@ SF0X::collect() report.timestamp = hrt_absolute_time(); report.error_count = perf_event_count(_comms_errors); report.distance = si_units; + report.minimum_distance = get_minimum_distance(); + report.maximum_distance = get_maximum_distance(); report.valid = valid && (si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0); /* publish it */ From 834ff859307b178faf25faecef1b2d29b7e17d6f Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 13 Dec 2014 18:27:38 +0100 Subject: [PATCH 135/216] trone: fix whitespace --- src/drivers/trone/trone.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/drivers/trone/trone.cpp b/src/drivers/trone/trone.cpp index 2f2f692a10..dff86cf716 100644 --- a/src/drivers/trone/trone.cpp +++ b/src/drivers/trone/trone.cpp @@ -256,11 +256,11 @@ TRONE::~TRONE() if (_reports != nullptr) { delete _reports; } - + if (_class_instance != -1) { unregister_class_devname(RANGE_FINDER_DEVICE_PATH, _class_instance); } - + // free perf counters perf_free(_sample_perf); perf_free(_comms_errors); @@ -286,7 +286,7 @@ TRONE::init() _class_instance = register_class_devname(RANGE_FINDER_DEVICE_PATH); - if (_class_instance == CLASS_DEVICE_PRIMARY) { + if (_class_instance == CLASS_DEVICE_PRIMARY) { /* get a publish handle on the range finder topic */ struct range_finder_report rf_report; measure(); @@ -559,7 +559,7 @@ TRONE::collect() report.error_count = perf_event_count(_comms_errors); report.distance = si_units; report.valid = crc8(val, 2) == val[2] && si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0; - + /* publish it, if we are the primary */ if (_range_finder_topic >= 0) { From cc2dae23f9a7c1bfe1a8f35cbbecff4f7466952a Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 13 Dec 2014 18:28:18 +0100 Subject: [PATCH 136/216] trone: write min and max to report --- src/drivers/trone/trone.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/drivers/trone/trone.cpp b/src/drivers/trone/trone.cpp index dff86cf716..83b5c987e8 100644 --- a/src/drivers/trone/trone.cpp +++ b/src/drivers/trone/trone.cpp @@ -558,6 +558,8 @@ TRONE::collect() report.timestamp = hrt_absolute_time(); report.error_count = perf_event_count(_comms_errors); report.distance = si_units; + report.minimum_distance = get_minimum_distance(); + report.maximum_distance = get_maximum_distance(); report.valid = crc8(val, 2) == val[2] && si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0; From 0575f67300e54ebed2dd9d16013efb37cecefb44 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 13 Dec 2014 18:28:58 +0100 Subject: [PATCH 137/216] ll40ls: fix whitespace --- src/drivers/ll40ls/ll40ls.cpp | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/src/drivers/ll40ls/ll40ls.cpp b/src/drivers/ll40ls/ll40ls.cpp index 6793acd810..a69fc56e9a 100644 --- a/src/drivers/ll40ls/ll40ls.cpp +++ b/src/drivers/ll40ls/ll40ls.cpp @@ -233,11 +233,11 @@ LL40LS::~LL40LS() if (_reports != nullptr) { delete _reports; } - + if (_class_instance != -1) { unregister_class_devname(RANGE_FINDER_DEVICE_PATH, _class_instance); } - + // free perf counters perf_free(_sample_perf); perf_free(_comms_errors); @@ -263,7 +263,7 @@ LL40LS::init() _class_instance = register_class_devname(RANGE_FINDER_DEVICE_PATH); - if (_class_instance == CLASS_DEVICE_PRIMARY) { + if (_class_instance == CLASS_DEVICE_PRIMARY) { /* get a publish handle on the range finder topic */ struct range_finder_report rf_report; measure(); @@ -314,9 +314,9 @@ LL40LS::probe() goto ok; } - debug("WHO_AM_I byte mismatch 0x%02x should be 0x%02x val=0x%02x\n", - (unsigned)who_am_i, - LL40LS_WHO_AM_I_REG_VAL, + debug("WHO_AM_I byte mismatch 0x%02x should be 0x%02x val=0x%02x\n", + (unsigned)who_am_i, + LL40LS_WHO_AM_I_REG_VAL, (unsigned)val); } @@ -704,7 +704,7 @@ LL40LS::print_info() perf_print_counter(_buffer_overflows); printf("poll interval: %u ticks\n", _measure_ticks); _reports->print_info("report queue"); - printf("distance: %ucm (0x%04x)\n", + printf("distance: %ucm (0x%04x)\n", (unsigned)_last_distance, (unsigned)_last_distance); } @@ -969,8 +969,8 @@ ll40ls_main(int argc, char *argv[]) } } - const char *verb = argv[optind]; - + const char *verb = argv[optind]; + /* * Start/load the driver. */ From b6b80dcac19c80e535349fe762b566f6e8a5ed4d Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 13 Dec 2014 18:29:24 +0100 Subject: [PATCH 138/216] ll40ls: write min and max to report --- src/drivers/ll40ls/ll40ls.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/drivers/ll40ls/ll40ls.cpp b/src/drivers/ll40ls/ll40ls.cpp index a69fc56e9a..8f85c729a8 100644 --- a/src/drivers/ll40ls/ll40ls.cpp +++ b/src/drivers/ll40ls/ll40ls.cpp @@ -581,6 +581,8 @@ LL40LS::collect() report.timestamp = hrt_absolute_time(); report.error_count = perf_event_count(_comms_errors); report.distance = si_units; + report.minimum_distance = get_minimum_distance(); + report.maximum_distance = get_maximum_distance(); if (si_units > get_minimum_distance() && si_units < get_maximum_distance()) { report.valid = 1; } From aa40c69853be0dc7e79bc3084472b77f9667c1f1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 15 Dec 2014 15:34:19 +0100 Subject: [PATCH 139/216] Ensure no SD alarm always plays. Fixes #1500 --- ROMFS/px4fmu_common/init.d/rcS | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 94c462439a..b027107c86 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -28,9 +28,6 @@ then tone_alarm start else set LOG_FILE /dev/null - echo "[i] No microSD card found" - # Play SOS - tone_alarm error fi # @@ -585,6 +582,14 @@ then fi unset EXIT_ON_END + # Run no SD alarm last + if [ $LOG_FILE == /dev/null ] + then + echo "[i] No microSD card found" + # Play SOS + tone_alarm error + fi + # End of autostart fi From 51a7fbeee091c20eb52bafade3e2041e26a785cc Mon Sep 17 00:00:00 2001 From: tumbili Date: Mon, 15 Dec 2014 22:12:11 +0100 Subject: [PATCH 140/216] added sanity checkto prevent false low airspeed readings during transition --- src/modules/vtol_att_control/vtol_att_control_main.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index 126ec5832e..e3a2dd72e4 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -557,6 +557,11 @@ VtolAttitudeControl::scale_mc_output() { airspeed = math::max(0.5f, _airspeed.true_airspeed_m_s); } + // Sanity check if airspeed is consistent with throttle + if(_manual_control_sp.z >= 0.35f && airspeed < _params.mc_airspeed_trim) { // XXX magic number, should be hover throttle param + airspeed = _params.mc_airspeed_trim; + } + /* * For scaling our actuators using anything less than the min (close to stall) * speed doesn't make any sense - its the strongest reasonable deflection we From 89e2e08de0e9f76114b095782167722597f298fa Mon Sep 17 00:00:00 2001 From: tumbili Date: Mon, 15 Dec 2014 22:47:10 +0100 Subject: [PATCH 141/216] removed white space noise --- ROMFS/px4fmu_common/init.d/rc.autostart | 2 +- ROMFS/px4fmu_common/init.d/rc.vtol_defaults | 2 +- ROMFS/px4fmu_common/init.d/rcS | 4 ++-- src/modules/systemlib/mixer/mixer.h | 2 +- src/modules/vtol_att_control/vtol_att_control_main.cpp | 4 ++-- 5 files changed, 7 insertions(+), 7 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart b/ROMFS/px4fmu_common/init.d/rc.autostart index 19fce6ea56..8388da1dd7 100644 --- a/ROMFS/px4fmu_common/init.d/rc.autostart +++ b/ROMFS/px4fmu_common/init.d/rc.autostart @@ -246,5 +246,5 @@ fi # if param compare SYS_AUTOSTART 13001 then - sh /etc/init.d/13001_caipirinha_vtol + sh /etc/init.d/13001_caipirinha_vtol fi diff --git a/ROMFS/px4fmu_common/init.d/rc.vtol_defaults b/ROMFS/px4fmu_common/init.d/rc.vtol_defaults index 376a2e2579..10ee5db9b4 100644 --- a/ROMFS/px4fmu_common/init.d/rc.vtol_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.vtol_defaults @@ -20,7 +20,7 @@ then param set MC_YAWRATE_I 0.0 param set MC_YAWRATE_D 0.0 param set MC_YAW_FF 0.3 - + # # Default parameters for FW # diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index f0fe6d25bf..5748fe2b52 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -550,7 +550,7 @@ then sh /etc/init.d/rc.mc_apps fi fi - + # # VTOL setup # @@ -589,7 +589,7 @@ then sh /etc/init.d/rc.vtol_apps fi fi - + # # Start the navigator # diff --git a/src/modules/systemlib/mixer/mixer.h b/src/modules/systemlib/mixer/mixer.h index f14bdb9fdf..1fe4380ade 100644 --- a/src/modules/systemlib/mixer/mixer.h +++ b/src/modules/systemlib/mixer/mixer.h @@ -466,7 +466,7 @@ public: OCTA_X, OCTA_PLUS, OCTA_COX, - TWIN_ENGINE, /**< VTOL: one engine on each wing */ + TWIN_ENGINE, /**< VTOL: one engine on each wing */ MAX_GEOMETRY }; diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index e3a2dd72e4..24800cce89 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -436,7 +436,7 @@ void VtolAttitudeControl::fill_mc_att_control_output() { _actuators_out_0.control[0] = _actuators_mc_in.control[0]; _actuators_out_0.control[1] = _actuators_mc_in.control[1]; - _actuators_out_0.control[2] = _actuators_mc_in.control[2]; + _actuators_out_0.control[2] = _actuators_mc_in.control[2]; _actuators_out_0.control[3] = _actuators_mc_in.control[3]; //set neutral position for elevons _actuators_out_1.control[0] = _actuators_mc_in.control[2]; //roll elevon @@ -457,7 +457,7 @@ void VtolAttitudeControl::fill_fw_att_control_output() _actuators_out_1.control[0] = -_actuators_fw_in.control[0]; // roll elevon _actuators_out_1.control[1] = _actuators_fw_in.control[1]; // pitch elevon // unused now but still logged - _actuators_out_1.control[2] = _actuators_fw_in.control[2]; // yaw + _actuators_out_1.control[2] = _actuators_fw_in.control[2]; // yaw _actuators_out_1.control[3] = _actuators_fw_in.control[3]; // throttle } From a65151d1c05dfae8ea2b7361340a04f3bb02d1d2 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Wed, 17 Dec 2014 14:50:11 +0100 Subject: [PATCH 142/216] Airframe config and autostart for the TBS Discovery Long Range --- .../px4fmu_common/init.d/10018_tbs_endurance | 31 +++++++++++++++++++ ROMFS/px4fmu_common/init.d/rc.autostart | 5 +++ 2 files changed, 36 insertions(+) create mode 100644 ROMFS/px4fmu_common/init.d/10018_tbs_endurance diff --git a/ROMFS/px4fmu_common/init.d/10018_tbs_endurance b/ROMFS/px4fmu_common/init.d/10018_tbs_endurance new file mode 100644 index 0000000000..6619d14a76 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/10018_tbs_endurance @@ -0,0 +1,31 @@ +#!nsh +# +# Team Blacksheep Discovery Long Range Quadcopter +# +# Setup: 15 x 6.5" Props, 6S 4000mAh TBS LiPo, TBS 30A ESCs, TBS 400kV Motors +# +# Simon Wilks +# + +sh /etc/init.d/rc.mc_defaults + +if [ $DO_AUTOCONFIG == yes ] +then + param set MC_ROLL_P 7.0 + param set MC_ROLLRATE_P 0.07 + param set MC_ROLLRATE_I 0.02 + param set MC_ROLLRATE_D 0.003 + param set MC_PITCH_P 7.0 + param set MC_PITCHRATE_P 0.1 + param set MC_PITCHRATE_I 0.05 + param set MC_PITCHRATE_D 0.003 + param set MC_YAW_P 2.8 + param set MC_YAWRATE_P 0.4 + param set MC_YAWRATE_I 0.1 + param set MC_YAWRATE_D 0.0 +fi + +set MIXER FMU_quad_w + +set PWM_OUTPUTS 1234 +set PWM_MIN 1100 diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart b/ROMFS/px4fmu_common/init.d/rc.autostart index 8388da1dd7..20f2be0d9e 100644 --- a/ROMFS/px4fmu_common/init.d/rc.autostart +++ b/ROMFS/px4fmu_common/init.d/rc.autostart @@ -221,6 +221,11 @@ then sh /etc/init.d/10017_steadidrone_qu4d fi +if param compare SYS_AUTOSTART 10018 18 +then + sh /etc/init.d/10018_tbs_endurance +fi + # # Hexa Coaxial # From d39e29e6ae1f1c785d4554f70c09e42ba9791de6 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Wed, 17 Dec 2014 16:07:57 +0100 Subject: [PATCH 143/216] Up the min PWM slightly. --- ROMFS/px4fmu_common/init.d/10018_tbs_endurance | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/10018_tbs_endurance b/ROMFS/px4fmu_common/init.d/10018_tbs_endurance index 6619d14a76..2d5e272bdd 100644 --- a/ROMFS/px4fmu_common/init.d/10018_tbs_endurance +++ b/ROMFS/px4fmu_common/init.d/10018_tbs_endurance @@ -28,4 +28,4 @@ fi set MIXER FMU_quad_w set PWM_OUTPUTS 1234 -set PWM_MIN 1100 +set PWM_MIN 1200 From 262b9fc7545805c7b93a15cbb80a2f67db5ecdf0 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 17 Dec 2014 21:13:13 +0100 Subject: [PATCH 144/216] fw pos ctl: make loop performance counter more meaningful --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index e7c95cc864..948b29bcbe 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -1399,8 +1399,6 @@ FixedwingPositionControl::task_main() continue; } - perf_begin(_loop_perf); - /* check vehicle control mode for changes to publication state */ vehicle_control_mode_poll(); @@ -1419,6 +1417,7 @@ FixedwingPositionControl::task_main() /* only run controller if position changed */ if (fds[1].revents & POLLIN) { + perf_begin(_loop_perf); /* XXX Hack to get mavlink output going */ if (_mavlink_fd < 0) { @@ -1473,10 +1472,9 @@ FixedwingPositionControl::task_main() } } - + perf_end(_loop_perf); } - perf_end(_loop_perf); } _task_running = false; From 8fdd694f12cccfe90ed1dbec9bc7892269b3fefa Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 17 Dec 2014 21:13:50 +0100 Subject: [PATCH 145/216] fw pos ctl: compile with -Os --- src/modules/fw_pos_control_l1/module.mk | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/fw_pos_control_l1/module.mk b/src/modules/fw_pos_control_l1/module.mk index 15b575b50f..440bab2c5a 100644 --- a/src/modules/fw_pos_control_l1/module.mk +++ b/src/modules/fw_pos_control_l1/module.mk @@ -45,3 +45,5 @@ SRCS = fw_pos_control_l1_main.cpp \ mtecs/mTecs_params.c MODULE_STACKSIZE = 1200 + +MAXOPTIMIZATION = -Os From 0e79cbf234ba633669bbc1e939b1722f5396dc11 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Thu, 18 Dec 2014 10:43:32 +0100 Subject: [PATCH 146/216] fixed startup for VTOL -> some variable names were changed lately --- ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol | 2 +- ROMFS/px4fmu_common/init.d/rc.vtol_defaults | 2 +- ROMFS/px4fmu_common/init.d/rcS | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol b/ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol index bff971c0ff..8f7a28b973 100644 --- a/ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol +++ b/ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol @@ -9,7 +9,7 @@ sh /etc/init.d/rc.vtol_defaults set MIXER FMU_caipirinha_vtol -set PWM_OUTPUTS 12 +set PWM_OUT 12 set PWM_MAX 2000 set PWM_RATE 400 param set VTOL_MOT_COUNT 2 diff --git a/ROMFS/px4fmu_common/init.d/rc.vtol_defaults b/ROMFS/px4fmu_common/init.d/rc.vtol_defaults index 10ee5db9b4..6dc5a65db7 100644 --- a/ROMFS/px4fmu_common/init.d/rc.vtol_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.vtol_defaults @@ -2,7 +2,7 @@ set VEHICLE_TYPE vtol -if [ $DO_AUTOCONFIG == yes ] +if [ $AUTOCNF == yes ] then # #Default controller parameters for MC diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 5748fe2b52..14fc8d2b47 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -584,7 +584,7 @@ then sh /etc/init.d/rc.interface # Start standard vtol apps - if [ $LOAD_DEFAULT_APPS == yes ] + if [ $LOAD_DAPPS == yes ] then sh /etc/init.d/rc.vtol_apps fi From 669e1f96d8fed848d2fbe71d3a10adb749a5fcc4 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Thu, 18 Dec 2014 10:46:36 +0100 Subject: [PATCH 147/216] fixed parameter issue for VTOL so that parameter wiki tool can document them --- .../init.d/13001_caipirinha_vtol | 2 +- .../vtol_att_control_main.cpp | 2 +- .../vtol_att_control_params.c | 93 +++++++++++++++++-- 3 files changed, 86 insertions(+), 11 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol b/ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol index 8f7a28b973..fb31142fa0 100644 --- a/ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol +++ b/ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol @@ -13,4 +13,4 @@ set PWM_OUT 12 set PWM_MAX 2000 set PWM_RATE 400 param set VTOL_MOT_COUNT 2 -param set IDLE_PWM_MC 1080 +param set VTOL_IDLE_PWM_MC 1080 diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index 24800cce89..de678a5b83 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -238,7 +238,7 @@ VtolAttitudeControl::VtolAttitudeControl() : _params.idle_pwm_mc = PWM_LOWEST_MIN; _params.vtol_motor_count = 0; - _params_handles.idle_pwm_mc = param_find("IDLE_PWM_MC"); + _params_handles.idle_pwm_mc = param_find("VTOL_IDLE_PWM_MC"); _params_handles.vtol_motor_count = param_find("VTOL_MOT_COUNT"); _params_handles.mc_airspeed_min = param_find("VTOL_MC_AIRSPEED_MIN"); _params_handles.mc_airspeed_max = param_find("VTOL_MC_AIRSPEED_MAX"); diff --git a/src/modules/vtol_att_control/vtol_att_control_params.c b/src/modules/vtol_att_control/vtol_att_control_params.c index 44157acba6..6a234a3bad 100644 --- a/src/modules/vtol_att_control/vtol_att_control_params.c +++ b/src/modules/vtol_att_control/vtol_att_control_params.c @@ -1,13 +1,88 @@ +/**************************************************************************** + * + * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file vtol_att_control_params.c + * Parameters for vtol attitude controller. + * + * @author Roman Bapst + */ + #include -// number of engines +/** + * VTOL number of engines + * + * @min 1.0 + * @group VTOL Attitude Control + */ PARAM_DEFINE_INT32(VTOL_MOT_COUNT,0); -// idle pwm in multicopter mode -PARAM_DEFINE_INT32(IDLE_PWM_MC,900); -// min airspeed in multicopter mode -PARAM_DEFINE_FLOAT(VTOL_MC_AIRSPEED_MIN,2); -// max airspeed in multicopter mode -PARAM_DEFINE_FLOAT(VTOL_MC_AIRSPEED_MAX,30); -// trim airspeed in multicopter mode -PARAM_DEFINE_FLOAT(VTOL_MC_AIRSPEED_TRIM,10); + +/** + * Idle speed of VTOL when in multicopter mode + * + * @min 900 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_INT32(VTOL_IDLE_PWM_MC,900); + +/** + * Minimum airspeed in multicopter mode + * + * This is the minimum speed of the air flowing over the control surfaces. + * + * @min 0.0 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VTOL_MC_AIRSPEED_MIN,2.0f); + +/** + * Maximum airspeed in multicopter mode + * + * This is the maximum speed of the air flowing over the control surfaces. + * + * @min 0.0 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VTOL_MC_AIRSPEED_MAX,30.0f); + +/** + * Trim airspeed when in multicopter mode + * + * This is the airflow over the control surfaces for which no airspeed scaling is applied in multicopter mode. + * + * @min 0.0 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VTOL_MC_AIRSPEED_TRIM,10.0f); From f3b5b67eec258476480a225b6b835e46335003b4 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Thu, 18 Dec 2014 12:43:23 +0100 Subject: [PATCH 148/216] shortened parameter names to max 16 bytes --- ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol | 4 ++-- src/modules/vtol_att_control/vtol_att_control_main.cpp | 10 +++++----- src/modules/vtol_att_control/vtol_att_control_params.c | 10 +++++----- 3 files changed, 12 insertions(+), 12 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol b/ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol index fb31142fa0..7e9a6d3dc8 100644 --- a/ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol +++ b/ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol @@ -12,5 +12,5 @@ set MIXER FMU_caipirinha_vtol set PWM_OUT 12 set PWM_MAX 2000 set PWM_RATE 400 -param set VTOL_MOT_COUNT 2 -param set VTOL_IDLE_PWM_MC 1080 +param set VT_MOT_COUNT 2 +param set VT_IDLE_PWM_MC 1080 diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index de678a5b83..958907d1ba 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -238,11 +238,11 @@ VtolAttitudeControl::VtolAttitudeControl() : _params.idle_pwm_mc = PWM_LOWEST_MIN; _params.vtol_motor_count = 0; - _params_handles.idle_pwm_mc = param_find("VTOL_IDLE_PWM_MC"); - _params_handles.vtol_motor_count = param_find("VTOL_MOT_COUNT"); - _params_handles.mc_airspeed_min = param_find("VTOL_MC_AIRSPEED_MIN"); - _params_handles.mc_airspeed_max = param_find("VTOL_MC_AIRSPEED_MAX"); - _params_handles.mc_airspeed_trim = param_find("VTOL_MC_AIRSPEED_TRIM"); + _params_handles.idle_pwm_mc = param_find("VT_IDLE_PWM_MC"); + _params_handles.vtol_motor_count = param_find("VT_MOT_COUNT"); + _params_handles.mc_airspeed_min = param_find("VT_MC_ARSPD_MIN"); + _params_handles.mc_airspeed_max = param_find("VT_MC_ARSPD_MAX"); + _params_handles.mc_airspeed_trim = param_find("VT_MC_ARSPD_TRIM"); /* fetch initial parameter values */ parameters_update(); diff --git a/src/modules/vtol_att_control/vtol_att_control_params.c b/src/modules/vtol_att_control/vtol_att_control_params.c index 6a234a3bad..e21bccb0c7 100644 --- a/src/modules/vtol_att_control/vtol_att_control_params.c +++ b/src/modules/vtol_att_control/vtol_att_control_params.c @@ -46,7 +46,7 @@ * @min 1.0 * @group VTOL Attitude Control */ -PARAM_DEFINE_INT32(VTOL_MOT_COUNT,0); +PARAM_DEFINE_INT32(VT_MOT_COUNT,0); /** * Idle speed of VTOL when in multicopter mode @@ -54,7 +54,7 @@ PARAM_DEFINE_INT32(VTOL_MOT_COUNT,0); * @min 900 * @group VTOL Attitude Control */ -PARAM_DEFINE_INT32(VTOL_IDLE_PWM_MC,900); +PARAM_DEFINE_INT32(VT_IDLE_PWM_MC,900); /** * Minimum airspeed in multicopter mode @@ -64,7 +64,7 @@ PARAM_DEFINE_INT32(VTOL_IDLE_PWM_MC,900); * @min 0.0 * @group VTOL Attitude Control */ -PARAM_DEFINE_FLOAT(VTOL_MC_AIRSPEED_MIN,2.0f); +PARAM_DEFINE_FLOAT(VT_MC_ARSPD_MIN,2.0f); /** * Maximum airspeed in multicopter mode @@ -74,7 +74,7 @@ PARAM_DEFINE_FLOAT(VTOL_MC_AIRSPEED_MIN,2.0f); * @min 0.0 * @group VTOL Attitude Control */ -PARAM_DEFINE_FLOAT(VTOL_MC_AIRSPEED_MAX,30.0f); +PARAM_DEFINE_FLOAT(VT_MC_ARSPD_MAX,30.0f); /** * Trim airspeed when in multicopter mode @@ -84,5 +84,5 @@ PARAM_DEFINE_FLOAT(VTOL_MC_AIRSPEED_MAX,30.0f); * @min 0.0 * @group VTOL Attitude Control */ -PARAM_DEFINE_FLOAT(VTOL_MC_AIRSPEED_TRIM,10.0f); +PARAM_DEFINE_FLOAT(VT_MC_ARSPD_TRIM,10.0f); From ecde80ad3596dd4ceae6be399467510e4a05e3d7 Mon Sep 17 00:00:00 2001 From: The Gitter Badger Date: Thu, 18 Dec 2014 18:13:22 +0000 Subject: [PATCH 149/216] Added Gitter badge --- README.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/README.md b/README.md index 84fb02e3b9..532e9aab25 100644 --- a/README.md +++ b/README.md @@ -1,5 +1,7 @@ ## PX4 Aerial Middleware and Flight Control Stack ## +[![Gitter](https://badges.gitter.im/Join%20Chat.svg)](https://gitter.im/PX4/Firmware?utm_source=badge&utm_medium=badge&utm_campaign=pr-badge&utm_content=badge) + * Official Website: http://px4.io * License: BSD 3-clause (see LICENSE.md) * Supported airframes: From 97fdc69473179d9bd4f92f7fdef768d830fabe3d Mon Sep 17 00:00:00 2001 From: James Goppert Date: Thu, 18 Dec 2014 15:18:33 -0500 Subject: [PATCH 150/216] Added aerocore upload target. --- makefiles/gumstix-aerocore.cfg | 10 ++++++++++ makefiles/upload.mk | 5 +++++ 2 files changed, 15 insertions(+) create mode 100644 makefiles/gumstix-aerocore.cfg diff --git a/makefiles/gumstix-aerocore.cfg b/makefiles/gumstix-aerocore.cfg new file mode 100644 index 0000000000..ba217c0431 --- /dev/null +++ b/makefiles/gumstix-aerocore.cfg @@ -0,0 +1,10 @@ +# JTAG for the STM32F4x chip used on the Gumstix AeroCore is available on +# the first interface of a Quad FTDI chip. nTRST is bit 4. +interface ftdi +ftdi_vid_pid 0x0403 0x6011 + +ftdi_layout_init 0x0000 0x001b +ftdi_layout_signal nTRST -data 0x0010 + +source [find target/stm32f4x.cfg] +reset_config trst_only diff --git a/makefiles/upload.mk b/makefiles/upload.mk index bc26d743d0..29b415688e 100644 --- a/makefiles/upload.mk +++ b/makefiles/upload.mk @@ -30,6 +30,11 @@ upload-serial-px4fmu-v1: $(BUNDLE) $(UPLOADER) upload-serial-px4fmu-v2: $(BUNDLE) $(UPLOADER) $(Q) $(PYTHON) -u $(UPLOADER) --port $(SERIAL_PORTS) $(BUNDLE) +upload-serial-aerocore: + openocd -f $(PX4_BASE)/makefiles/gumstix-aerocore.cfg -c 'init; reset halt; flash write_image erase $(PX4_BASE)/../Bootloader/px4aerocore_bl.bin 0x08000000; flash write_image erase $(PX4_BASE)/Build/aerocore_default.build/firmware.bin 0x08004000; reset run; exit' + + + # # JTAG firmware uploading with OpenOCD # From 2a09473e5fc73bf2f59f0d27b0b5c38b2c91812f Mon Sep 17 00:00:00 2001 From: Ban Siesta Date: Fri, 19 Dec 2014 22:26:31 +0000 Subject: [PATCH 151/216] topics: move geofence status to its own topic --- src/modules/commander/commander.cpp | 51 +++++++++++------- src/modules/navigator/navigator.h | 11 +++- src/modules/navigator/navigator_main.cpp | 24 +++++++-- src/modules/uORB/objects_common.cpp | 5 +- src/modules/uORB/topics/geofence_result.h | 65 +++++++++++++++++++++++ src/modules/uORB/topics/mission_result.h | 11 ++-- 6 files changed, 137 insertions(+), 30 deletions(-) create mode 100644 src/modules/uORB/topics/geofence_result.h diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 744323c011..9262f9e818 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -81,6 +81,7 @@ #include #include #include +#include #include #include @@ -926,6 +927,11 @@ int commander_thread_main(int argc, char *argv[]) struct mission_result_s mission_result; memset(&mission_result, 0, sizeof(mission_result)); + /* Subscribe to geofence result topic */ + int geofence_result_sub = orb_subscribe(ORB_ID(geofence_result)); + struct geofence_result_s geofence_result; + memset(&geofence_result, 0, sizeof(geofence_result)); + /* Subscribe to manual control data */ int sp_man_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); struct manual_control_setpoint_s sp_man; @@ -1545,27 +1551,34 @@ int commander_thread_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(mission_result), mission_result_sub, &mission_result); - - /* Check for geofence violation */ - if (armed.armed && (mission_result.geofence_violated || mission_result.flight_termination)) { - //XXX: make this configurable to select different actions (e.g. navigation modes) - /* this will only trigger if geofence is activated via param and a geofence file is present, also there is a circuit breaker to disable the actual flight termination in the px4io driver */ - armed.force_failsafe = true; - status_changed = true; - static bool flight_termination_printed = false; - - if (!flight_termination_printed) { - warnx("Flight termination because of navigator request or geofence"); - mavlink_log_critical(mavlink_fd, "GF violation: flight termination"); - flight_termination_printed = true; - } - - if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { - mavlink_log_critical(mavlink_fd, "GF violation: flight termination"); - } - } // no reset is done here on purpose, on geofence violation we want to stay in flighttermination } + /* start geofence result check */ + orb_check(geofence_result_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(geofence_result), geofence_result_sub, &geofence_result); + } + + /* Check for geofence violation */ + if (armed.armed && (geofence_result.geofence_violated || mission_result.flight_termination)) { + //XXX: make this configurable to select different actions (e.g. navigation modes) + /* this will only trigger if geofence is activated via param and a geofence file is present, also there is a circuit breaker to disable the actual flight termination in the px4io driver */ + armed.force_failsafe = true; + status_changed = true; + static bool flight_termination_printed = false; + + if (!flight_termination_printed) { + warnx("Flight termination because of navigator request or geofence"); + mavlink_log_critical(mavlink_fd, "GF violation: flight termination"); + flight_termination_printed = true; + } + + if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { + mavlink_log_critical(mavlink_fd, "GF violation: flight termination"); + } + } // no reset is done here on purpose, on geofence violation we want to stay in flighttermination + /* RC input check */ if (!status.rc_input_blocked && sp_man.timestamp != 0 && hrt_absolute_time() < sp_man.timestamp + (uint64_t)(rc_loss_timeout * 1e6f)) { diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 9cd609955b..a701e3f44b 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -54,6 +54,7 @@ #include #include #include +#include #include #include "navigator_mode.h" @@ -111,6 +112,11 @@ public: */ void publish_mission_result(); + /** + * Publish the geofence result + */ + void publish_geofence_result(); + /** * Publish the attitude sp, only to be used in very special modes when position control is deactivated * Example: mode that is triggered on gps failure @@ -134,6 +140,7 @@ public: struct home_position_s* get_home_position() { return &_home_pos; } struct position_setpoint_triplet_s* get_position_setpoint_triplet() { return &_pos_sp_triplet; } struct mission_result_s* get_mission_result() { return &_mission_result; } + struct geofence_result_s* get_geofence_result() { return &_geofence_result; } struct vehicle_attitude_setpoint_s* get_att_sp() { return &_att_sp; } int get_onboard_mission_sub() { return _onboard_mission_sub; } @@ -164,6 +171,7 @@ private: orb_advert_t _pos_sp_triplet_pub; /**< publish position setpoint triplet */ orb_advert_t _mission_result_pub; + orb_advert_t _geofence_result_pub; orb_advert_t _att_sp_pub; /**< publish att sp used only in very special failsafe modes when pos control is deactivated */ @@ -179,7 +187,8 @@ private: position_setpoint_triplet_s _pos_sp_triplet; /**< triplet of position setpoints */ mission_result_s _mission_result; - vehicle_attitude_setpoint_s _att_sp; + geofence_result_s _geofence_result; + vehicle_attitude_setpoint_s _att_sp; bool _mission_item_valid; /**< flags if the current mission item is valid */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index df620e5e7f..0ab85d56eb 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -110,6 +110,7 @@ Navigator::Navigator() : _param_update_sub(-1), _pos_sp_triplet_pub(-1), _mission_result_pub(-1), + _geofence_result_pub(-1), _att_sp_pub(-1), _vstatus{}, _control_mode{}, @@ -398,8 +399,8 @@ Navigator::task_main() have_geofence_position_data = false; if (!inside) { /* inform other apps via the mission result */ - _mission_result.geofence_violated = true; - publish_mission_result(); + _geofence_result.geofence_violated = true; + publish_geofence_result(); /* Issue a warning about the geofence violation once */ if (!_geofence_violation_warning_sent) { @@ -408,8 +409,8 @@ Navigator::task_main() } } else { /* inform other apps via the mission result */ - _mission_result.geofence_violated = false; - publish_mission_result(); + _geofence_result.geofence_violated = false; + publish_geofence_result(); /* Reset the _geofence_violation_warning_sent field */ _geofence_violation_warning_sent = false; } @@ -641,6 +642,21 @@ Navigator::publish_mission_result() } } +void +Navigator::publish_geofence_result() +{ + + /* lazily publish the geofence result only once available */ + if (_geofence_result_pub > 0) { + /* publish mission result */ + orb_publish(ORB_ID(geofence_result), _geofence_result_pub, &_geofence_result); + + } else { + /* advertise and publish */ + _geofence_result_pub = orb_advertise(ORB_ID(geofence_result), &_geofence_result); + } +} + void Navigator::publish_att_sp() { diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 1141431cc7..3d5755a465 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012, 2013 PX4 Development Team. All rights reserved. + * Copyright (C) 2012-2014 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -148,6 +148,9 @@ ORB_DEFINE(onboard_mission, struct mission_s); #include "topics/mission_result.h" ORB_DEFINE(mission_result, struct mission_result_s); +#include "topics/geofence_result.h" +ORB_DEFINE(geofence_result, struct geofence_result_s); + #include "topics/fence.h" ORB_DEFINE(fence, unsigned); diff --git a/src/modules/uORB/topics/geofence_result.h b/src/modules/uORB/topics/geofence_result.h new file mode 100644 index 0000000000..b07e044993 --- /dev/null +++ b/src/modules/uORB/topics/geofence_result.h @@ -0,0 +1,65 @@ +/**************************************************************************** + * + * Copyright (C) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file geofence_result.h + * Status of the plance concerning the geofence + * + * @author Ban Siesta + */ + +#ifndef TOPIC_GEOFENCE_RESULT_H +#define TOPIC_GEOFENCE_RESULT_H + +#include +#include +#include "../uORB.h" + +/** + * @addtogroup topics + * @{ + */ + +struct geofence_result_s +{ + bool geofence_violated; /**< true if the geofence is violated */ +}; + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(geofence_result); + +#endif diff --git a/src/modules/uORB/topics/mission_result.h b/src/modules/uORB/topics/mission_result.h index c7d25d1f08..17ddaa01d3 100644 --- a/src/modules/uORB/topics/mission_result.h +++ b/src/modules/uORB/topics/mission_result.h @@ -1,9 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Thomas Gubler - * @author Julian Oes - * @author Lorenz Meier + * Copyright (C) 2012-2014 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -37,6 +34,11 @@ /** * @file mission_result.h * Mission results that navigator needs to pass on to commander and mavlink. + * + * @author Thomas Gubler + * @author Julian Oes + * @author Lorenz Meier + * @author Ban Siesta */ #ifndef TOPIC_MISSION_RESULT_H @@ -58,7 +60,6 @@ struct mission_result_s bool reached; /**< true if mission has been reached */ bool finished; /**< true if mission has been completed */ bool stay_in_failsafe; /**< true if the commander should not switch out of the failsafe mode*/ - bool geofence_violated; /**< true if the geofence is violated */ bool flight_termination; /**< true if the navigator demands a flight termination from the commander app */ }; From 5bbd0743e082f247e3ff04e78e0e11b0583b9ff1 Mon Sep 17 00:00:00 2001 From: Ban Siesta Date: Fri, 19 Dec 2014 22:27:25 +0000 Subject: [PATCH 152/216] navigator: deleted some verbose output --- src/modules/navigator/mission.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 0765e9b7c0..3714806036 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -718,7 +718,6 @@ Mission::set_mission_item_reached() void Mission::set_current_offboard_mission_item() { - warnx("current offboard mission index: %d", _current_offboard_mission_index); _navigator->get_mission_result()->reached = false; _navigator->get_mission_result()->finished = false; _navigator->get_mission_result()->seq_current = _current_offboard_mission_index; From c9ca61ef5b23a370fcaf3e2a0546ab5452b65733 Mon Sep 17 00:00:00 2001 From: Ban Siesta Date: Fri, 19 Dec 2014 23:34:18 +0000 Subject: [PATCH 153/216] mavlink: don't slow mission updates down like this, otherwise we might miss mission results --- src/modules/mavlink/mavlink_mission.cpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index a3c127cdc5..34c0887780 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -292,9 +292,6 @@ MavlinkMissionManager::send_mission_item_reached(uint16_t seq) void MavlinkMissionManager::send(const hrt_abstime now) { - /* update interval for slow rate limiter */ - _slow_rate_limiter.set_interval(_interval * 10 / _mavlink->get_rate_mult()); - bool updated = false; orb_check(_mission_result_sub, &updated); From f0ff914b626fbfb497143f80b19376efb524b9e1 Mon Sep 17 00:00:00 2001 From: Ban Siesta Date: Fri, 19 Dec 2014 23:36:32 +0000 Subject: [PATCH 154/216] navigator: don't publish mission result immediately but only after every iteration of the navigator --- src/modules/navigator/datalinkloss.cpp | 8 ++++---- src/modules/navigator/gpsfailure.cpp | 2 +- src/modules/navigator/mission.cpp | 6 +++--- src/modules/navigator/navigator.h | 13 ++++++++----- src/modules/navigator/navigator_main.cpp | 6 ++++++ src/modules/navigator/navigator_mode.cpp | 2 +- src/modules/navigator/rcloss.cpp | 6 +++--- 7 files changed, 26 insertions(+), 17 deletions(-) diff --git a/src/modules/navigator/datalinkloss.cpp b/src/modules/navigator/datalinkloss.cpp index e789fd10de..87a6e023a5 100644 --- a/src/modules/navigator/datalinkloss.cpp +++ b/src/modules/navigator/datalinkloss.cpp @@ -155,7 +155,7 @@ DataLinkLoss::set_dll_item() case DLL_STATE_TERMINATE: { /* Request flight termination from the commander */ _navigator->get_mission_result()->flight_termination = true; - _navigator->publish_mission_result(); + _navigator->set_mission_result_updated(); reset_mission_item_reached(); warnx("not switched to manual: request flight termination"); pos_sp_triplet->previous.valid = false; @@ -188,7 +188,7 @@ DataLinkLoss::advance_dll() _navigator->get_vstatus()->data_link_lost_counter, _param_numberdatalinklosses.get()); mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: too many DL losses, fly to airfield home"); _navigator->get_mission_result()->stay_in_failsafe = true; - _navigator->publish_mission_result(); + _navigator->set_mission_result_updated(); reset_mission_item_reached(); _dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP; } else { @@ -209,7 +209,7 @@ DataLinkLoss::advance_dll() mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: fly to airfield home"); _dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP; _navigator->get_mission_result()->stay_in_failsafe = true; - _navigator->publish_mission_result(); + _navigator->set_mission_result_updated(); reset_mission_item_reached(); break; case DLL_STATE_FLYTOAIRFIELDHOMEWP: @@ -217,7 +217,7 @@ DataLinkLoss::advance_dll() warnx("time is up, state should have been changed manually by now"); mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no manual control, terminating"); _navigator->get_mission_result()->stay_in_failsafe = true; - _navigator->publish_mission_result(); + _navigator->set_mission_result_updated(); reset_mission_item_reached(); break; case DLL_STATE_TERMINATE: diff --git a/src/modules/navigator/gpsfailure.cpp b/src/modules/navigator/gpsfailure.cpp index cd55f60b06..e370796c07 100644 --- a/src/modules/navigator/gpsfailure.cpp +++ b/src/modules/navigator/gpsfailure.cpp @@ -141,7 +141,7 @@ GpsFailure::set_gpsf_item() case GPSF_STATE_TERMINATE: { /* Request flight termination from the commander */ _navigator->get_mission_result()->flight_termination = true; - _navigator->publish_mission_result(); + _navigator->set_mission_result_updated(); warnx("gps fail: request flight termination"); } default: diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 3714806036..8de8faf9d3 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -711,7 +711,7 @@ Mission::set_mission_item_reached() { _navigator->get_mission_result()->reached = true; _navigator->get_mission_result()->seq_reached = _current_offboard_mission_index; - _navigator->publish_mission_result(); + _navigator->set_mission_result_updated(); reset_mission_item_reached(); } @@ -721,7 +721,7 @@ Mission::set_current_offboard_mission_item() _navigator->get_mission_result()->reached = false; _navigator->get_mission_result()->finished = false; _navigator->get_mission_result()->seq_current = _current_offboard_mission_index; - _navigator->publish_mission_result(); + _navigator->set_mission_result_updated(); save_offboard_mission_state(); } @@ -730,5 +730,5 @@ void Mission::set_mission_finished() { _navigator->get_mission_result()->finished = true; - _navigator->publish_mission_result(); + _navigator->set_mission_result_updated(); } diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index a701e3f44b..d9d911d9c7 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -107,11 +107,6 @@ public: */ void load_fence_from_file(const char *filename); - /** - * Publish the mission result so commander and mavlink know what is going on - */ - void publish_mission_result(); - /** * Publish the geofence result */ @@ -128,6 +123,7 @@ public: */ void set_can_loiter_at_sp(bool can_loiter) { _can_loiter_at_sp = can_loiter; } void set_position_setpoint_triplet_updated() { _pos_sp_triplet_updated = true; } + void set_mission_result_updated() { _mission_result_updated = true; } /** * Getters @@ -215,6 +211,7 @@ private: bool _can_loiter_at_sp; /**< flags if current position SP can be used to loiter */ bool _pos_sp_triplet_updated; /**< flags if position SP triplet needs to be published */ bool _pos_sp_triplet_published_invalid_once; /**< flags if position SP triplet has been published once to UORB */ + bool _mission_result_updated; /**< flags if mission result has seen an update */ control::BlockParamFloat _param_loiter_radius; /**< loiter radius for fixedwing */ control::BlockParamFloat _param_acceptance_radius; /**< acceptance for takeoff */ @@ -280,6 +277,12 @@ private: */ void publish_position_setpoint_triplet(); + + /** + * Publish the mission result so commander and mavlink know what is going on + */ + void publish_mission_result(); + /* this class has ptr data members, so it should not be copied, * consequently the copy constructors are private. */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 0ab85d56eb..6a255878b5 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -139,6 +139,7 @@ Navigator::Navigator() : _can_loiter_at_sp(false), _pos_sp_triplet_updated(false), _pos_sp_triplet_published_invalid_once(false), + _mission_result_updated(false), _param_loiter_radius(this, "LOITER_RAD"), _param_acceptance_radius(this, "ACC_RAD"), _param_datalinkloss_obc(this, "DLL_OBC"), @@ -491,6 +492,11 @@ Navigator::task_main() _pos_sp_triplet_updated = false; } + if (_mission_result_updated) { + publish_mission_result(); + _mission_result_updated = false; + } + perf_end(_loop_perf); } warnx("exiting."); diff --git a/src/modules/navigator/navigator_mode.cpp b/src/modules/navigator/navigator_mode.cpp index 3807c5ea82..2f322031c3 100644 --- a/src/modules/navigator/navigator_mode.cpp +++ b/src/modules/navigator/navigator_mode.cpp @@ -65,7 +65,7 @@ NavigatorMode::run(bool active) { _first_run = false; /* Reset stay in failsafe flag */ _navigator->get_mission_result()->stay_in_failsafe = false; - _navigator->publish_mission_result(); + _navigator->set_mission_result_updated(); on_activation(); } else { diff --git a/src/modules/navigator/rcloss.cpp b/src/modules/navigator/rcloss.cpp index 42392e7399..a7cde6325d 100644 --- a/src/modules/navigator/rcloss.cpp +++ b/src/modules/navigator/rcloss.cpp @@ -128,7 +128,7 @@ RCLoss::set_rcl_item() case RCL_STATE_TERMINATE: { /* Request flight termination from the commander */ _navigator->get_mission_result()->flight_termination = true; - _navigator->publish_mission_result(); + _navigator->set_mission_result_updated(); warnx("rc not recovered: request flight termination"); pos_sp_triplet->previous.valid = false; pos_sp_triplet->current.valid = false; @@ -162,7 +162,7 @@ RCLoss::advance_rcl() mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: rc loss, terminating"); _rcl_state = RCL_STATE_TERMINATE; _navigator->get_mission_result()->stay_in_failsafe = true; - _navigator->publish_mission_result(); + _navigator->set_mission_result_updated(); reset_mission_item_reached(); } break; @@ -171,7 +171,7 @@ RCLoss::advance_rcl() warnx("time is up, no RC regain, terminating"); mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RC not regained, terminating"); _navigator->get_mission_result()->stay_in_failsafe = true; - _navigator->publish_mission_result(); + _navigator->set_mission_result_updated(); reset_mission_item_reached(); break; case RCL_STATE_TERMINATE: From 7cb4786e7934af50b5ceb370c593e27111d9dd17 Mon Sep 17 00:00:00 2001 From: Ban Siesta Date: Fri, 19 Dec 2014 23:38:10 +0000 Subject: [PATCH 155/216] navigator: report when a waypoint has been reached not when the whole mission is finished --- src/modules/navigator/mission.cpp | 10 ++-------- 1 file changed, 2 insertions(+), 8 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 8de8faf9d3..3f0a6bb51c 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -38,6 +38,7 @@ * @author Julian Oes * @author Thomas Gubler * @author Anton Babushkin + * @author Ban Siesta */ #include @@ -149,18 +150,12 @@ Mission::on_active() /* lets check if we reached the current mission item */ if (_mission_type != MISSION_TYPE_NONE && is_mission_item_reached()) { + set_mission_item_reached(); if (_mission_item.autocontinue) { /* switch to next waypoint if 'autocontinue' flag set */ advance_mission(); set_mission_items(); - } else { - /* else just report that item reached */ - if (_mission_type == MISSION_TYPE_OFFBOARD) { - if (!(_navigator->get_mission_result()->seq_reached == _current_offboard_mission_index && _navigator->get_mission_result()->reached)) { - set_mission_item_reached(); - } - } } } else if (_mission_type != MISSION_TYPE_NONE &&_param_altmode.get() == MISSION_ALTMODE_FOH) { @@ -395,7 +390,6 @@ Mission::set_mission_items() /* reuse setpoint for LOITER only if it's not IDLE */ _navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == SETPOINT_TYPE_LOITER); - reset_mission_item_reached(); set_mission_finished(); _navigator->set_position_setpoint_triplet_updated(); From 180e17de335f337b17c0c411d0eb430cec760619 Mon Sep 17 00:00:00 2001 From: Ban Siesta Date: Fri, 19 Dec 2014 23:38:55 +0000 Subject: [PATCH 156/216] navigator: report using mission result if a DO_JUMP waypoint has been changed --- src/modules/mavlink/mavlink_mission.cpp | 8 +++++++- src/modules/navigator/mission.cpp | 12 ++++++++++++ src/modules/navigator/mission.h | 6 ++++++ src/modules/uORB/topics/mission_result.h | 3 +++ 4 files changed, 28 insertions(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index 34c0887780..859d380fe5 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -309,6 +309,12 @@ MavlinkMissionManager::send(const hrt_abstime now) send_mission_current(_current_seq); + if (mission_result.item_do_jump_changed) { + /* send a mission item again if the remaining DO_JUMPs has changed */ + send_mission_item(_transfer_partner_sysid, _transfer_partner_compid, + (uint16_t)mission_result.item_changed_index); + } + } else { if (_slow_rate_limiter.check(now)) { send_mission_current(_current_seq); @@ -808,7 +814,7 @@ MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s * case NAV_CMD_DO_JUMP: mavlink_mission_item->param1 = mission_item->do_jump_mission_index; - mavlink_mission_item->param2 = mission_item->do_jump_repeat_count; + mavlink_mission_item->param2 = mission_item->do_jump_repeat_count - mission_item->do_jump_current_count; break; default: diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 3f0a6bb51c..9b0a092dad 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -630,6 +630,8 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s "ERROR DO JUMP waypoint could not be written"); return false; } + report_do_jump_mission_changed(*mission_index_ptr, + mission_item_tmp.do_jump_repeat_count); } /* set new mission item index and repeat * we don't have to validate here, if it's invalid, we should realize this later .*/ @@ -700,6 +702,16 @@ Mission::save_offboard_mission_state() dm_unlock(DM_KEY_MISSION_STATE); } +void +Mission::report_do_jump_mission_changed(int index, int do_jumps_remaining) +{ + /* inform about the change */ + _navigator->get_mission_result()->item_do_jump_changed = true; + _navigator->get_mission_result()->item_changed_index = index; + _navigator->get_mission_result()->item_do_jump_remaining = do_jumps_remaining; + _navigator->set_mission_result_updated(); +} + void Mission::set_mission_item_reached() { diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index ea7cc0927c..a8a644b0f5 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -38,6 +38,7 @@ * @author Julian Oes * @author Thomas Gubler * @author Anton Babushkin + * @author Ban Siesta */ #ifndef NAVIGATOR_MISSION_H @@ -130,6 +131,11 @@ private: */ void save_offboard_mission_state(); + /** + * Inform about a changed mission item after a DO_JUMP + */ + void report_do_jump_mission_changed(int index, int do_jumps_remaining); + /** * Set a mission item as reached */ diff --git a/src/modules/uORB/topics/mission_result.h b/src/modules/uORB/topics/mission_result.h index 17ddaa01d3..2ddc529a3f 100644 --- a/src/modules/uORB/topics/mission_result.h +++ b/src/modules/uORB/topics/mission_result.h @@ -61,6 +61,9 @@ struct mission_result_s bool finished; /**< true if mission has been completed */ bool stay_in_failsafe; /**< true if the commander should not switch out of the failsafe mode*/ bool flight_termination; /**< true if the navigator demands a flight termination from the commander app */ + bool item_do_jump_changed; /**< true if the number of do jumps remaining has changed */ + unsigned item_changed_index; /**< indicate which item has changed */ + unsigned item_do_jump_remaining;/**< set to the number of do jumps remaining for that item */ }; /** From a4e27fd849610fe2dcd9426d0d5cb508e9563e97 Mon Sep 17 00:00:00 2001 From: Ban Siesta Date: Fri, 19 Dec 2014 23:39:25 +0000 Subject: [PATCH 157/216] navigator: reset some flags after the mission result has been published --- src/modules/navigator/navigator_main.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 6a255878b5..3f7670ec4a 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -646,6 +646,13 @@ Navigator::publish_mission_result() /* advertise and publish */ _mission_result_pub = orb_advertise(ORB_ID(mission_result), &_mission_result); } + + /* reset some of the flags */ + _mission_result.seq_reached = false; + _mission_result.seq_current = 0; + _mission_result.item_do_jump_changed = false; + _mission_result.item_changed_index = 0; + _mission_result.item_do_jump_remaining = 0; } void From 6e0cf5002914e9045082bdfe1d3acc484a37f7fb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 20 Dec 2014 13:54:58 +0100 Subject: [PATCH 158/216] Move unittests into a more perceivable directory --- {Tools/tests-host => unittests}/.gitignore | 0 {Tools/tests-host => unittests}/Makefile | 0 {Tools/tests-host => unittests}/arch/board/board.h | 0 {Tools/tests-host => unittests}/autodeclination_test.cpp | 0 {Tools/tests-host => unittests}/board_config.h | 0 {Tools/tests-host => unittests}/data/fit_linear_voltage.m | 0 {Tools/tests-host => unittests}/data/px4io_v1.3.csv | 0 {Tools/tests-host => unittests}/debug.h | 0 {Tools/tests-host => unittests}/hrt.cpp | 0 {Tools/tests-host => unittests}/mixer_test.cpp | 0 {Tools/tests-host => unittests}/nuttx/config.h | 0 {Tools/tests-host => unittests}/queue.h | 0 {Tools/tests-host => unittests}/run_tests.sh | 0 {Tools/tests-host => unittests}/sbus2_test.cpp | 0 {Tools/tests-host => unittests}/sf0x_test.cpp | 0 {Tools/tests-host => unittests}/st24_test.cpp | 0 16 files changed, 0 insertions(+), 0 deletions(-) rename {Tools/tests-host => unittests}/.gitignore (100%) rename {Tools/tests-host => unittests}/Makefile (100%) rename {Tools/tests-host => unittests}/arch/board/board.h (100%) rename {Tools/tests-host => unittests}/autodeclination_test.cpp (100%) rename {Tools/tests-host => unittests}/board_config.h (100%) rename {Tools/tests-host => unittests}/data/fit_linear_voltage.m (100%) rename {Tools/tests-host => unittests}/data/px4io_v1.3.csv (100%) rename {Tools/tests-host => unittests}/debug.h (100%) rename {Tools/tests-host => unittests}/hrt.cpp (100%) rename {Tools/tests-host => unittests}/mixer_test.cpp (100%) rename {Tools/tests-host => unittests}/nuttx/config.h (100%) rename {Tools/tests-host => unittests}/queue.h (100%) rename {Tools/tests-host => unittests}/run_tests.sh (100%) rename {Tools/tests-host => unittests}/sbus2_test.cpp (100%) rename {Tools/tests-host => unittests}/sf0x_test.cpp (100%) rename {Tools/tests-host => unittests}/st24_test.cpp (100%) diff --git a/Tools/tests-host/.gitignore b/unittests/.gitignore similarity index 100% rename from Tools/tests-host/.gitignore rename to unittests/.gitignore diff --git a/Tools/tests-host/Makefile b/unittests/Makefile similarity index 100% rename from Tools/tests-host/Makefile rename to unittests/Makefile diff --git a/Tools/tests-host/arch/board/board.h b/unittests/arch/board/board.h similarity index 100% rename from Tools/tests-host/arch/board/board.h rename to unittests/arch/board/board.h diff --git a/Tools/tests-host/autodeclination_test.cpp b/unittests/autodeclination_test.cpp similarity index 100% rename from Tools/tests-host/autodeclination_test.cpp rename to unittests/autodeclination_test.cpp diff --git a/Tools/tests-host/board_config.h b/unittests/board_config.h similarity index 100% rename from Tools/tests-host/board_config.h rename to unittests/board_config.h diff --git a/Tools/tests-host/data/fit_linear_voltage.m b/unittests/data/fit_linear_voltage.m similarity index 100% rename from Tools/tests-host/data/fit_linear_voltage.m rename to unittests/data/fit_linear_voltage.m diff --git a/Tools/tests-host/data/px4io_v1.3.csv b/unittests/data/px4io_v1.3.csv similarity index 100% rename from Tools/tests-host/data/px4io_v1.3.csv rename to unittests/data/px4io_v1.3.csv diff --git a/Tools/tests-host/debug.h b/unittests/debug.h similarity index 100% rename from Tools/tests-host/debug.h rename to unittests/debug.h diff --git a/Tools/tests-host/hrt.cpp b/unittests/hrt.cpp similarity index 100% rename from Tools/tests-host/hrt.cpp rename to unittests/hrt.cpp diff --git a/Tools/tests-host/mixer_test.cpp b/unittests/mixer_test.cpp similarity index 100% rename from Tools/tests-host/mixer_test.cpp rename to unittests/mixer_test.cpp diff --git a/Tools/tests-host/nuttx/config.h b/unittests/nuttx/config.h similarity index 100% rename from Tools/tests-host/nuttx/config.h rename to unittests/nuttx/config.h diff --git a/Tools/tests-host/queue.h b/unittests/queue.h similarity index 100% rename from Tools/tests-host/queue.h rename to unittests/queue.h diff --git a/Tools/tests-host/run_tests.sh b/unittests/run_tests.sh similarity index 100% rename from Tools/tests-host/run_tests.sh rename to unittests/run_tests.sh diff --git a/Tools/tests-host/sbus2_test.cpp b/unittests/sbus2_test.cpp similarity index 100% rename from Tools/tests-host/sbus2_test.cpp rename to unittests/sbus2_test.cpp diff --git a/Tools/tests-host/sf0x_test.cpp b/unittests/sf0x_test.cpp similarity index 100% rename from Tools/tests-host/sf0x_test.cpp rename to unittests/sf0x_test.cpp diff --git a/Tools/tests-host/st24_test.cpp b/unittests/st24_test.cpp similarity index 100% rename from Tools/tests-host/st24_test.cpp rename to unittests/st24_test.cpp From 02e9a76cd71f4d808701202251edaa7fd3276f3d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 20 Dec 2014 18:50:16 +0100 Subject: [PATCH 159/216] Add unit test data --- unittests/testdata/sbus2_r7008SB.txt | 2192 +++ unittests/testdata/st24_data.txt | 18019 +++++++++++++++++++++++++ 2 files changed, 20211 insertions(+) create mode 100644 unittests/testdata/sbus2_r7008SB.txt create mode 100644 unittests/testdata/st24_data.txt diff --git a/unittests/testdata/sbus2_r7008SB.txt b/unittests/testdata/sbus2_r7008SB.txt new file mode 100644 index 0000000000..d504ba7056 --- /dev/null +++ b/unittests/testdata/sbus2_r7008SB.txt @@ -0,0 +1,2192 @@ +Time [s],Value,Parity Error,Framing Error +0,0x0F,, +0.00012,0x11,, +0.00024,0x04,, +0.00036,0x20,, +0.000480125,0xA8,, +0.000600125,0x01,, +0.000720125,0x08,, +0.000840125,0x16,, +0.000960125,0x50,, +0.001080125,0x03,, +0.001200125,0x10,, +0.001320125,0x80,, +0.001440125,0x00,, +0.001560125,0x04,, +0.001680125,0x20,, +0.001800125,0x00,, +0.001920125,0x01,, +0.002040125,0x08,, +0.002160125,0x40,, +0.002280125,0x00,, +0.002400125,0x02,, +0.002520125,0x10,, +0.002640125,0x80,, +0.002760125,0x00,, +0.002880125,0x04,, +0.00504025,0x03,, +0.00516025,0xC4,, +0.00528025,0x00,, +0.015000625,0x0F,, +0.015120625,0x11,, +0.015240625,0x04,, +0.015360625,0x20,, +0.015480625,0xA8,, +0.01560075,0x01,, +0.01572075,0x08,, +0.01584075,0x16,, +0.01596075,0x50,, +0.01608075,0x03,, +0.01620075,0x10,, +0.01632075,0x80,, +0.01644075,0x00,, +0.01656075,0x04,, +0.01668075,0x20,, +0.01680075,0x00,, +0.01692075,0x01,, +0.01704075,0x08,, +0.01716075,0x40,, +0.01728075,0x00,, +0.01740075,0x02,, +0.01752075,0x10,, +0.01764075,0x80,, +0.01776075,0x00,, +0.01788075,0x14,, +0.03000125,0x0F,, +0.03012125,0x11,, +0.03024125,0x04,, +0.03036125,0x20,, +0.03048125,0xA8,, +0.03060125,0x01,, +0.030721375,0x08,, +0.030841375,0x16,, +0.030961375,0x50,, +0.031081375,0x03,, +0.031201375,0x10,, +0.031321375,0x80,, +0.031441375,0x00,, +0.031561375,0x04,, +0.031681375,0x20,, +0.031801375,0x00,, +0.031921375,0x01,, +0.032041375,0x08,, +0.032161375,0x40,, +0.032281375,0x00,, +0.032401375,0x02,, +0.032521375,0x10,, +0.032641375,0x80,, +0.032761375,0x00,, +0.032881375,0x24,, +0.045001875,0x0F,, +0.045121875,0x11,, +0.045241875,0x04,, +0.045361875,0x20,, +0.045481875,0xA8,, +0.045601875,0x01,, +0.045721875,0x08,, +0.045842,0x16,, +0.045962,0x50,, +0.046082,0x03,, +0.046202,0x10,, +0.046322,0x80,, +0.046442,0x00,, +0.046562,0x04,, +0.046682,0x20,, +0.046802,0x00,, +0.046922,0x01,, +0.047042,0x08,, +0.047162,0x40,, +0.047282,0x00,, +0.047402,0x02,, +0.047522,0x10,, +0.047642,0x80,, +0.047762,0x00,, +0.047882,0x34,, +0.0600025,0x0F,, +0.0601225,0x11,, +0.0602425,0x04,, +0.0603625,0x20,, +0.0604825,0xA8,, +0.0606025,0x01,, +0.0607225,0x08,, +0.0608425,0x16,, +0.060962625,0x50,, +0.061082625,0x03,, +0.061202625,0x10,, +0.061322625,0x80,, +0.061442625,0x00,, +0.061562625,0x04,, +0.061682625,0x20,, +0.061802625,0x00,, +0.061922625,0x01,, +0.062042625,0x08,, +0.062162625,0x40,, +0.062282625,0x00,, +0.062402625,0x02,, +0.062522625,0x10,, +0.062642625,0x80,, +0.062762625,0x00,, +0.062882625,0x04,, +0.06503275,0x03,, +0.06515275,0xC0,, +0.06527275,0x2E,, +0.075003125,0x0F,, +0.075123125,0x11,, +0.075243125,0x04,, +0.075363125,0x20,, +0.075483125,0xA8,, +0.075603125,0x01,, +0.075723125,0x08,, +0.075843125,0x16,, +0.075963125,0x50,, +0.07608325,0x03,, +0.07620325,0x10,, +0.07632325,0x80,, +0.07644325,0x00,, +0.07656325,0x04,, +0.07668325,0x20,, +0.07680325,0x00,, +0.07692325,0x01,, +0.07704325,0x08,, +0.07716325,0x40,, +0.07728325,0x00,, +0.07740325,0x02,, +0.07752325,0x10,, +0.07764325,0x80,, +0.07776325,0x00,, +0.07788325,0x14,, +0.09000375,0x0F,, +0.09012375,0x11,, +0.09024375,0x04,, +0.09036375,0x20,, +0.09048375,0xA8,, +0.09060375,0x01,, +0.09072375,0x08,, +0.09084375,0x16,, +0.09096375,0x50,, +0.09108375,0x03,, +0.09120375,0x10,, +0.091323875,0x80,, +0.091443875,0x00,, +0.091563875,0x04,, +0.091683875,0x20,, +0.091803875,0x00,, +0.091923875,0x01,, +0.092043875,0x08,, +0.092163875,0x40,, +0.092283875,0x00,, +0.092403875,0x02,, +0.092523875,0x10,, +0.092643875,0x80,, +0.092763875,0x00,, +0.092883875,0x24,, +0.105014375,0x0F,, +0.105134375,0x11,, +0.105254375,0x04,, +0.105374375,0x20,, +0.105494375,0xA8,, +0.105614375,0x01,, +0.105734375,0x08,, +0.105854375,0x16,, +0.105974375,0x50,, +0.106094375,0x03,, +0.106214375,0x10,, +0.106334375,0x80,, +0.1064545,0x00,, +0.1065745,0x04,, +0.1066945,0x20,, +0.1068145,0x00,, +0.1069345,0x01,, +0.1070545,0x08,, +0.1071745,0x40,, +0.1072945,0x00,, +0.1074145,0x02,, +0.1075345,0x10,, +0.1076545,0x80,, +0.1077745,0x00,, +0.1078945,0x34,, +0.120005,0x0F,, +0.120125,0x11,, +0.120245,0x04,, +0.120365,0x20,, +0.120485,0xA8,, +0.120605,0x01,, +0.120725,0x08,, +0.120845,0x16,, +0.120965,0x50,, +0.121085,0x03,, +0.121205,0x10,, +0.121325,0x80,, +0.121445,0x00,, +0.121565125,0x04,, +0.121685125,0x20,, +0.121805125,0x00,, +0.121925125,0x01,, +0.122045125,0x08,, +0.122165125,0x40,, +0.122285125,0x00,, +0.122405125,0x02,, +0.122525125,0x10,, +0.122645125,0x80,, +0.122765125,0x00,, +0.122885125,0x04,, +0.12503525,0x03,, +0.12515525,0xC4,, +0.12527525,0x00,, +0.135005625,0x0F,, +0.135125625,0x11,, +0.135245625,0x04,, +0.135365625,0x20,, +0.135485625,0xA8,, +0.135605625,0x01,, +0.135725625,0x08,, +0.135845625,0x16,, +0.135965625,0x50,, +0.136085625,0x03,, +0.136205625,0x10,, +0.136325625,0x80,, +0.136445625,0x00,, +0.136565625,0x04,, +0.13668575,0x20,, +0.13680575,0x00,, +0.13692575,0x01,, +0.13704575,0x08,, +0.13716575,0x40,, +0.13728575,0x00,, +0.13740575,0x02,, +0.13752575,0x10,, +0.13764575,0x80,, +0.13776575,0x00,, +0.13788575,0x14,, +0.15000625,0x0F,, +0.15012625,0x11,, +0.15024625,0x04,, +0.15036625,0x20,, +0.15048625,0xA8,, +0.15060625,0x01,, +0.15072625,0x08,, +0.15084625,0x16,, +0.15096625,0x50,, +0.15108625,0x03,, +0.15120625,0x10,, +0.15132625,0x80,, +0.15144625,0x00,, +0.15156625,0x04,, +0.15168625,0x20,, +0.151806375,0x00,, +0.151926375,0x01,, +0.152046375,0x08,, +0.152166375,0x40,, +0.152286375,0x00,, +0.152406375,0x02,, +0.152526375,0x10,, +0.152646375,0x80,, +0.152766375,0x00,, +0.152886375,0x24,, +0.165016875,0x0F,, +0.165136875,0x11,, +0.165256875,0x04,, +0.165376875,0x20,, +0.165496875,0xA8,, +0.165616875,0x01,, +0.165736875,0x08,, +0.165856875,0x16,, +0.165976875,0x50,, +0.166096875,0x03,, +0.166216875,0x10,, +0.166336875,0x80,, +0.166456875,0x00,, +0.166576875,0x04,, +0.166696875,0x20,, +0.166816875,0x00,, +0.166937,0x01,, +0.167057,0x08,, +0.167177,0x40,, +0.167297,0x00,, +0.167417,0x02,, +0.167537,0x10,, +0.167657,0x80,, +0.167777,0x00,, +0.167897,0x34,, +0.1800075,0x0F,, +0.1801275,0x11,, +0.1802475,0x04,, +0.1803675,0x20,, +0.1804875,0xA8,, +0.1806075,0x01,, +0.1807275,0x08,, +0.1808475,0x16,, +0.1809675,0x50,, +0.1810875,0x03,, +0.1812075,0x10,, +0.1813275,0x80,, +0.1814475,0x00,, +0.1815675,0x04,, +0.1816875,0x20,, +0.1818075,0x00,, +0.1819275,0x01,, +0.1820475,0x08,, +0.182167625,0x40,, +0.182287625,0x00,, +0.182407625,0x02,, +0.182527625,0x10,, +0.182647625,0x80,, +0.182767625,0x00,, +0.182887625,0x04,, +0.185037625,0x03,, +0.18515775,0xC0,, +0.18527775,0x2E,, +0.195018125,0x0F,, +0.195138125,0x11,, +0.195258125,0x04,, +0.195378125,0x20,, +0.195498125,0xA8,, +0.195618125,0x01,, +0.195738125,0x08,, +0.195858125,0x16,, +0.195978125,0x50,, +0.196098125,0x03,, +0.196218125,0x10,, +0.196338125,0x80,, +0.196458125,0x00,, +0.196578125,0x04,, +0.196698125,0x20,, +0.196818125,0x00,, +0.196938125,0x01,, +0.197058125,0x08,, +0.197178125,0x40,, +0.19729825,0x00,, +0.19741825,0x02,, +0.19753825,0x10,, +0.19765825,0x80,, +0.19777825,0x00,, +0.19789825,0x14,, +0.21001875,0x0F,, +0.21013875,0x11,, +0.21025875,0x04,, +0.21037875,0x20,, +0.21049875,0xA8,, +0.21061875,0x01,, +0.21073875,0x08,, +0.21085875,0x16,, +0.21097875,0x50,, +0.21109875,0x03,, +0.21121875,0x10,, +0.21133875,0x80,, +0.21145875,0x00,, +0.21157875,0x04,, +0.21169875,0x20,, +0.21181875,0x00,, +0.21193875,0x01,, +0.21205875,0x08,, +0.21217875,0x40,, +0.21229875,0x00,, +0.212418875,0x02,, +0.212538875,0x10,, +0.212658875,0x80,, +0.212778875,0x00,, +0.212898875,0x24,, +0.225019375,0x0F,, +0.225139375,0x11,, +0.225259375,0x04,, +0.225379375,0x20,, +0.225499375,0xA8,, +0.225619375,0x01,, +0.225739375,0x08,, +0.225859375,0x16,, +0.225979375,0x50,, +0.226099375,0x03,, +0.226219375,0x10,, +0.226339375,0x80,, +0.226459375,0x00,, +0.226579375,0x04,, +0.226699375,0x20,, +0.226819375,0x00,, +0.226939375,0x01,, +0.227059375,0x08,, +0.227179375,0x40,, +0.227299375,0x00,, +0.227419375,0x02,, +0.2275395,0x10,, +0.2276595,0x80,, +0.2277795,0x00,, +0.2278995,0x34,, +0.24001,0x0F,, +0.24013,0x11,, +0.24025,0x04,, +0.24037,0x20,, +0.24049,0xA8,, +0.24061,0x01,, +0.24073,0x08,, +0.24085,0x16,, +0.24097,0x50,, +0.24109,0x03,, +0.24121,0x10,, +0.24133,0x80,, +0.24145,0x00,, +0.24157,0x04,, +0.24169,0x20,, +0.24181,0x00,, +0.24193,0x01,, +0.24205,0x08,, +0.24217,0x40,, +0.24229,0x00,, +0.24241,0x02,, +0.24253,0x10,, +0.242650125,0x80,, +0.242770125,0x00,, +0.242890125,0x04,, +0.245040125,0x03,, +0.245160125,0xC4,, +0.245280125,0x00,, +0.255010625,0x0F,, +0.255130625,0x11,, +0.255250625,0x04,, +0.255370625,0x20,, +0.255490625,0xA8,, +0.255610625,0x01,, +0.255730625,0x08,, +0.255850625,0x16,, +0.255970625,0x50,, +0.256090625,0x03,, +0.256210625,0x10,, +0.256330625,0x80,, +0.256450625,0x00,, +0.256570625,0x04,, +0.256690625,0x20,, +0.256810625,0x00,, +0.256930625,0x01,, +0.257050625,0x08,, +0.257170625,0x40,, +0.257290625,0x00,, +0.257410625,0x02,, +0.257530625,0x10,, +0.257650625,0x80,, +0.25777075,0x00,, +0.25789075,0x14,, +0.27002125,0x0F,, +0.27014125,0x11,, +0.27026125,0x04,, +0.27038125,0x20,, +0.27050125,0xA8,, +0.27062125,0x01,, +0.27074125,0x08,, +0.27086125,0x16,, +0.27098125,0x50,, +0.27110125,0x03,, +0.27122125,0x10,, +0.27134125,0x80,, +0.27146125,0x00,, +0.27158125,0x04,, +0.27170125,0x20,, +0.27182125,0x00,, +0.27194125,0x01,, +0.27206125,0x08,, +0.27218125,0x40,, +0.27230125,0x00,, +0.27242125,0x02,, +0.27254125,0x10,, +0.27266125,0x80,, +0.27278125,0x00,, +0.272901375,0x24,, +0.285021875,0x0F,, +0.285141875,0x11,, +0.285261875,0x04,, +0.285381875,0x20,, +0.285501875,0xA8,, +0.285621875,0x01,, +0.285741875,0x08,, +0.285861875,0x16,, +0.285981875,0x50,, +0.286101875,0x03,, +0.286221875,0x10,, +0.286341875,0x80,, +0.286461875,0x00,, +0.286581875,0x04,, +0.286701875,0x20,, +0.286821875,0x00,, +0.286941875,0x01,, +0.287061875,0x08,, +0.287181875,0x40,, +0.287301875,0x00,, +0.287421875,0x02,, +0.287541875,0x10,, +0.287661875,0x80,, +0.287781875,0x00,, +0.287901875,0x34,, +0.300022375,0x0F,, +0.3001425,0x11,, +0.3002625,0x04,, +0.3003825,0x20,, +0.3005025,0xA8,, +0.3006225,0x01,, +0.3007425,0x08,, +0.3008625,0x16,, +0.3009825,0x50,, +0.3011025,0x03,, +0.3012225,0x10,, +0.3013425,0x80,, +0.3014625,0x00,, +0.3015825,0x04,, +0.3017025,0x20,, +0.3018225,0x00,, +0.3019425,0x01,, +0.3020625,0x08,, +0.3021825,0x40,, +0.3023025,0x00,, +0.3024225,0x02,, +0.3025425,0x10,, +0.3026625,0x80,, +0.3027825,0x00,, +0.3029025,0x04,, +0.305052625,0x03,, +0.305172625,0xC0,, +0.305292625,0x2E,, +0.315023,0x0F,, +0.315143,0x11,, +0.315263,0x04,, +0.315383125,0x20,, +0.315503125,0xA8,, +0.315623125,0x01,, +0.315743125,0x08,, +0.315863125,0x16,, +0.315983125,0x50,, +0.316103125,0x03,, +0.316223125,0x10,, +0.316343125,0x80,, +0.316463125,0x00,, +0.316583125,0x04,, +0.316703125,0x20,, +0.316823125,0x00,, +0.316943125,0x01,, +0.317063125,0x08,, +0.317183125,0x40,, +0.317303125,0x00,, +0.317423125,0x02,, +0.317543125,0x10,, +0.317663125,0x80,, +0.317783125,0x00,, +0.317903125,0x14,, +0.330013625,0x0F,, +0.330133625,0x11,, +0.330253625,0x04,, +0.330373625,0x20,, +0.33049375,0xA8,, +0.33061375,0x01,, +0.33073375,0x08,, +0.33085375,0x16,, +0.33097375,0x50,, +0.33109375,0x03,, +0.33121375,0x10,, +0.33133375,0x80,, +0.33145375,0x00,, +0.33157375,0x04,, +0.33169375,0x20,, +0.33181375,0x00,, +0.33193375,0x01,, +0.33205375,0x08,, +0.33217375,0x40,, +0.33229375,0x00,, +0.33241375,0x02,, +0.33253375,0x10,, +0.33265375,0x80,, +0.33277375,0x00,, +0.33289375,0x24,, +0.34502425,0x0F,, +0.34514425,0x11,, +0.34526425,0x04,, +0.34538425,0x20,, +0.34550425,0xA8,, +0.345624375,0x01,, +0.345744375,0x08,, +0.345864375,0x16,, +0.345984375,0x50,, +0.346104375,0x03,, +0.346224375,0x10,, +0.346344375,0x80,, +0.346464375,0x00,, +0.346584375,0x04,, +0.346704375,0x20,, +0.346824375,0x00,, +0.346944375,0x01,, +0.347064375,0x08,, +0.347184375,0x40,, +0.347304375,0x00,, +0.347424375,0x02,, +0.347544375,0x10,, +0.347664375,0x80,, +0.347784375,0x00,, +0.347904375,0x34,, +0.360024875,0x0F,, +0.360144875,0x11,, +0.360264875,0x04,, +0.360384875,0x20,, +0.360504875,0xA8,, +0.360624875,0x01,, +0.360745,0x08,, +0.360865,0x16,, +0.360985,0x50,, +0.361105,0x03,, +0.361225,0x10,, +0.361345,0x80,, +0.361465,0x00,, +0.361585,0x04,, +0.361705,0x20,, +0.361825,0x00,, +0.361945,0x01,, +0.362065,0x08,, +0.362185,0x40,, +0.362305,0x00,, +0.362425,0x02,, +0.362545,0x10,, +0.362665,0x80,, +0.362785,0x00,, +0.362905,0x04,, +0.365055125,0x03,, +0.365175125,0xC4,, +0.365295125,0x00,, +0.3750255,0x0F,, +0.3751455,0x11,, +0.3752655,0x04,, +0.3753855,0x20,, +0.3755055,0xA8,, +0.3756255,0x01,, +0.3757455,0x08,, +0.375865625,0x16,, +0.375985625,0x50,, +0.376105625,0x03,, +0.376225625,0x10,, +0.376345625,0x80,, +0.376465625,0x00,, +0.376585625,0x04,, +0.376705625,0x20,, +0.376825625,0x00,, +0.376945625,0x01,, +0.377065625,0x08,, +0.377185625,0x40,, +0.377305625,0x00,, +0.377425625,0x02,, +0.377545625,0x10,, +0.377665625,0x80,, +0.377785625,0x00,, +0.377905625,0x14,, +0.390026125,0x0F,, +0.390146125,0x11,, +0.390266125,0x04,, +0.390386125,0x20,, +0.390506125,0xA8,, +0.390626125,0x01,, +0.390746125,0x08,, +0.390866125,0x16,, +0.39098625,0x50,, +0.39110625,0x03,, +0.39122625,0x10,, +0.39134625,0x80,, +0.39146625,0x00,, +0.39158625,0x04,, +0.39170625,0x20,, +0.39182625,0x00,, +0.39194625,0x01,, +0.39206625,0x08,, +0.39218625,0x40,, +0.39230625,0x00,, +0.39242625,0x02,, +0.39254625,0x10,, +0.39266625,0x80,, +0.39278625,0x00,, +0.39290625,0x24,, +0.40502675,0x0F,, +0.40514675,0x11,, +0.40526675,0x04,, +0.40538675,0x20,, +0.40550675,0xA8,, +0.40562675,0x01,, +0.40574675,0x08,, +0.40586675,0x16,, +0.40598675,0x50,, +0.40610675,0x03,, +0.406226875,0x10,, +0.406346875,0x80,, +0.406466875,0x00,, +0.406586875,0x04,, +0.406706875,0x20,, +0.406826875,0x00,, +0.406946875,0x01,, +0.407066875,0x08,, +0.407186875,0x40,, +0.407306875,0x00,, +0.407426875,0x02,, +0.407546875,0x10,, +0.407666875,0x80,, +0.407786875,0x00,, +0.407906875,0x34,, +0.420027375,0x0F,, +0.420147375,0x11,, +0.420267375,0x04,, +0.420387375,0x20,, +0.420507375,0xA8,, +0.420627375,0x01,, +0.420747375,0x08,, +0.420867375,0x16,, +0.420987375,0x50,, +0.421107375,0x03,, +0.421227375,0x10,, +0.4213475,0x80,, +0.4214675,0x00,, +0.4215875,0x04,, +0.4217075,0x20,, +0.4218275,0x00,, +0.4219475,0x01,, +0.4220675,0x08,, +0.4221875,0x40,, +0.4223075,0x00,, +0.4224275,0x02,, +0.4225475,0x10,, +0.4226675,0x80,, +0.4227875,0x00,, +0.4229075,0x04,, +0.425057625,0x03,, +0.425177625,0xC0,, +0.425297625,0x2E,, +0.435028,0x0F,, +0.435148,0x11,, +0.435268,0x04,, +0.435388,0x20,, +0.435508,0xA8,, +0.435628,0x01,, +0.435748,0x08,, +0.435868,0x16,, +0.435988,0x50,, +0.436108,0x03,, +0.436228,0x10,, +0.436348,0x80,, +0.436468125,0x00,, +0.436588125,0x04,, +0.436708125,0x20,, +0.436828125,0x00,, +0.436948125,0x01,, +0.437068125,0x08,, +0.437188125,0x40,, +0.437308125,0x00,, +0.437428125,0x02,, +0.437548125,0x10,, +0.437668125,0x80,, +0.437788125,0x00,, +0.437908125,0x14,, +0.450028625,0x0F,, +0.450148625,0x11,, +0.450268625,0x04,, +0.450388625,0x20,, +0.450508625,0xA8,, +0.450628625,0x01,, +0.450748625,0x08,, +0.450868625,0x16,, +0.450988625,0x50,, +0.451108625,0x03,, +0.451228625,0x10,, +0.451348625,0x80,, +0.451468625,0x00,, +0.45158875,0x04,, +0.45170875,0x20,, +0.45182875,0x00,, +0.45194875,0x01,, +0.45206875,0x08,, +0.45218875,0x40,, +0.45230875,0x00,, +0.45242875,0x02,, +0.45254875,0x10,, +0.45266875,0x80,, +0.45278875,0x00,, +0.45290875,0x24,, +0.46502925,0x0F,, +0.46514925,0x11,, +0.46526925,0x04,, +0.46538925,0x20,, +0.46550925,0xA8,, +0.46562925,0x01,, +0.46574925,0x08,, +0.46586925,0x16,, +0.46598925,0x50,, +0.46610925,0x03,, +0.46622925,0x10,, +0.46634925,0x80,, +0.46646925,0x00,, +0.46658925,0x04,, +0.466709375,0x20,, +0.466829375,0x00,, +0.466949375,0x01,, +0.467069375,0x08,, +0.467189375,0x40,, +0.467309375,0x00,, +0.467429375,0x02,, +0.467549375,0x10,, +0.467669375,0x80,, +0.467789375,0x00,, +0.467909375,0x34,, +0.480039875,0x0F,, +0.480159875,0x11,, +0.480279875,0x04,, +0.480399875,0x20,, +0.480519875,0xA8,, +0.480639875,0x01,, +0.480759875,0x08,, +0.480879875,0x16,, +0.480999875,0x50,, +0.481119875,0x03,, +0.481239875,0x10,, +0.481359875,0x80,, +0.481479875,0x00,, +0.481599875,0x04,, +0.481719875,0x20,, +0.48184,0x00,, +0.48196,0x01,, +0.48208,0x08,, +0.4822,0x40,, +0.48232,0x00,, +0.48244,0x02,, +0.48256,0x10,, +0.48268,0x80,, +0.4828,0x00,, +0.48292,0x04,, +0.485070125,0x03,, +0.485190125,0xC4,, +0.485310125,0x00,, +0.4950405,0x0F,, +0.4951605,0x11,, +0.4952805,0x04,, +0.4954005,0x20,, +0.4955205,0xA8,, +0.4956405,0x01,, +0.4957605,0x08,, +0.4958805,0x16,, +0.4960005,0x50,, +0.4961205,0x03,, +0.4962405,0x10,, +0.4963605,0x80,, +0.4964805,0x00,, +0.4966005,0x04,, +0.4967205,0x20,, +0.4968405,0x00,, +0.496960625,0x01,, +0.497080625,0x08,, +0.497200625,0x40,, +0.497320625,0x00,, +0.497440625,0x02,, +0.497560625,0x10,, +0.497680625,0x80,, +0.497800625,0x00,, +0.497920625,0x14,, +0.510031125,0x0F,, +0.510151125,0x11,, +0.510271125,0x04,, +0.510391125,0x20,, +0.510511125,0xA8,, +0.510631125,0x01,, +0.510751125,0x08,, +0.510871125,0x16,, +0.510991125,0x50,, +0.511111125,0x03,, +0.511231125,0x10,, +0.511351125,0x80,, +0.511471125,0x00,, +0.511591125,0x04,, +0.511711125,0x20,, +0.511831125,0x00,, +0.511951125,0x01,, +0.512071125,0x08,, +0.51219125,0x40,, +0.51231125,0x00,, +0.51243125,0x02,, +0.51255125,0x10,, +0.51267125,0x80,, +0.51279125,0x00,, +0.51291125,0x24,, +0.52504175,0x0F,, +0.52516175,0x11,, +0.52528175,0x04,, +0.52540175,0x20,, +0.52552175,0xA8,, +0.52564175,0x01,, +0.52576175,0x08,, +0.52588175,0x16,, +0.52600175,0x50,, +0.52612175,0x03,, +0.52624175,0x10,, +0.52636175,0x80,, +0.52648175,0x00,, +0.52660175,0x04,, +0.52672175,0x20,, +0.52684175,0x00,, +0.52696175,0x01,, +0.52708175,0x08,, +0.52720175,0x40,, +0.527321875,0x00,, +0.527441875,0x02,, +0.527561875,0x10,, +0.527681875,0x80,, +0.527801875,0x00,, +0.527921875,0x34,, +0.540042375,0x0F,, +0.540162375,0x11,, +0.540282375,0x04,, +0.540402375,0x20,, +0.540522375,0xA8,, +0.540642375,0x01,, +0.540762375,0x08,, +0.540882375,0x16,, +0.541002375,0x50,, +0.541122375,0x03,, +0.541242375,0x10,, +0.541362375,0x80,, +0.541482375,0x00,, +0.541602375,0x04,, +0.541722375,0x20,, +0.541842375,0x00,, +0.541962375,0x01,, +0.542082375,0x08,, +0.542202375,0x40,, +0.542322375,0x00,, +0.5424425,0x02,, +0.5425625,0x10,, +0.5426825,0x80,, +0.5428025,0x00,, +0.5429225,0x04,, +0.5450725,0x03,, +0.5451925,0xC0,, +0.5453125,0x2E,, +0.555033,0x0F,, +0.555153,0x11,, +0.555273,0x04,, +0.555393,0x20,, +0.555513,0xA8,, +0.555633,0x01,, +0.555753,0x08,, +0.555873,0x16,, +0.555993,0x50,, +0.556113,0x03,, +0.556233,0x10,, +0.556353,0x80,, +0.556473,0x00,, +0.556593,0x04,, +0.556713,0x20,, +0.556833,0x00,, +0.556953,0x01,, +0.557073,0x08,, +0.557193,0x40,, +0.557313,0x00,, +0.557433,0x02,, +0.557553125,0x10,, +0.557673125,0x80,, +0.557793125,0x00,, +0.557913125,0x14,, +0.570033625,0x0F,, +0.570153625,0x11,, +0.570273625,0x04,, +0.570393625,0x20,, +0.570513625,0xA8,, +0.570633625,0x01,, +0.570753625,0x08,, +0.570873625,0x16,, +0.570993625,0x50,, +0.571113625,0x03,, +0.571233625,0x10,, +0.571353625,0x80,, +0.571473625,0x00,, +0.571593625,0x04,, +0.571713625,0x20,, +0.571833625,0x00,, +0.571953625,0x01,, +0.572073625,0x08,, +0.572193625,0x40,, +0.572313625,0x00,, +0.572433625,0x02,, +0.572553625,0x10,, +0.57267375,0x80,, +0.57279375,0x00,, +0.57291375,0x24,, +0.58504425,0x0F,, +0.58516425,0x11,, +0.58528425,0x04,, +0.58540425,0x20,, +0.58552425,0xA8,, +0.58564425,0x01,, +0.58576425,0x08,, +0.58588425,0x16,, +0.58600425,0x50,, +0.58612425,0x03,, +0.58624425,0x10,, +0.58636425,0x80,, +0.58648425,0x00,, +0.58660425,0x04,, +0.58672425,0x20,, +0.58684425,0x00,, +0.58696425,0x01,, +0.58708425,0x08,, +0.58720425,0x40,, +0.58732425,0x00,, +0.58744425,0x02,, +0.58756425,0x10,, +0.58768425,0x80,, +0.587804375,0x00,, +0.587924375,0x34,, +0.600044875,0x0F,, +0.600164875,0x11,, +0.600284875,0x04,, +0.600404875,0x20,, +0.600524875,0xA8,, +0.600644875,0x01,, +0.600764875,0x08,, +0.600884875,0x16,, +0.601004875,0x50,, +0.601124875,0x03,, +0.601244875,0x10,, +0.601364875,0x80,, +0.601484875,0x00,, +0.601604875,0x04,, +0.601724875,0x20,, +0.601844875,0x00,, +0.601964875,0x01,, +0.602084875,0x08,, +0.602204875,0x40,, +0.602324875,0x00,, +0.602444875,0x02,, +0.602564875,0x10,, +0.602684875,0x80,, +0.602804875,0x00,, +0.602924875,0x04,, +0.605075,0x03,, +0.605195,0xC4,, +0.605315,0x00,, +0.615045375,0x0F,, +0.6151655,0x11,, +0.6152855,0x04,, +0.6154055,0x20,, +0.6155255,0xA8,, +0.6156455,0x01,, +0.6157655,0x08,, +0.6158855,0x16,, +0.6160055,0x50,, +0.6161255,0x03,, +0.6162455,0x10,, +0.6163655,0x80,, +0.6164855,0x00,, +0.6166055,0x04,, +0.6167255,0x20,, +0.6168455,0x00,, +0.6169655,0x01,, +0.6170855,0x08,, +0.6172055,0x40,, +0.6173255,0x00,, +0.6174455,0x02,, +0.6175655,0x10,, +0.6176855,0x80,, +0.6178055,0x00,, +0.6179255,0x14,, +0.630046,0x0F,, +0.630166,0x11,, +0.630286125,0x04,, +0.630406125,0x20,, +0.630526125,0xA8,, +0.630646125,0x01,, +0.630766125,0x08,, +0.630886125,0x16,, +0.631006125,0x50,, +0.631126125,0x03,, +0.631246125,0x10,, +0.631366125,0x80,, +0.631486125,0x00,, +0.631606125,0x04,, +0.631726125,0x20,, +0.631846125,0x00,, +0.631966125,0x01,, +0.632086125,0x08,, +0.632206125,0x40,, +0.632326125,0x00,, +0.632446125,0x02,, +0.632566125,0x10,, +0.632686125,0x80,, +0.632806125,0x00,, +0.632926125,0x24,, +0.645036625,0x0F,, +0.645156625,0x11,, +0.645276625,0x04,, +0.64539675,0x20,, +0.64551675,0xA8,, +0.64563675,0x01,, +0.64575675,0x08,, +0.64587675,0x16,, +0.64599675,0x50,, +0.64611675,0x03,, +0.64623675,0x10,, +0.64635675,0x80,, +0.64647675,0x00,, +0.64659675,0x04,, +0.64671675,0x20,, +0.64683675,0x00,, +0.64695675,0x01,, +0.64707675,0x08,, +0.64719675,0x40,, +0.64731675,0x00,, +0.64743675,0x02,, +0.64755675,0x10,, +0.64767675,0x80,, +0.64779675,0x00,, +0.64791675,0x34,, +0.66004725,0x0F,, +0.66016725,0x11,, +0.66028725,0x04,, +0.66040725,0x20,, +0.660527375,0xA8,, +0.660647375,0x01,, +0.660767375,0x08,, +0.660887375,0x16,, +0.661007375,0x50,, +0.661127375,0x03,, +0.661247375,0x10,, +0.661367375,0x80,, +0.661487375,0x00,, +0.661607375,0x04,, +0.661727375,0x20,, +0.661847375,0x00,, +0.661967375,0x01,, +0.662087375,0x08,, +0.662207375,0x40,, +0.662327375,0x00,, +0.662447375,0x02,, +0.662567375,0x10,, +0.662687375,0x80,, +0.662807375,0x00,, +0.662927375,0x04,, +0.6650775,0x03,, +0.6651975,0xC0,, +0.6653175,0x2E,, +0.675047875,0x0F,, +0.675167875,0x11,, +0.675287875,0x04,, +0.675407875,0x20,, +0.675527875,0xA8,, +0.675648,0x01,, +0.675768,0x08,, +0.675888,0x16,, +0.676008,0x50,, +0.676128,0x03,, +0.676248,0x10,, +0.676368,0x80,, +0.676488,0x00,, +0.676608,0x04,, +0.676728,0x20,, +0.676848,0x00,, +0.676968,0x01,, +0.677088,0x08,, +0.677208,0x40,, +0.677328,0x00,, +0.677448,0x02,, +0.677568,0x10,, +0.677688,0x80,, +0.677808,0x00,, +0.677928,0x14,, +0.6900485,0x0F,, +0.6901685,0x11,, +0.6902885,0x04,, +0.6904085,0x20,, +0.6905285,0xA8,, +0.6906485,0x01,, +0.690768625,0x08,, +0.690888625,0x16,, +0.691008625,0x50,, +0.691128625,0x03,, +0.691248625,0x10,, +0.691368625,0x80,, +0.691488625,0x00,, +0.691608625,0x04,, +0.691728625,0x20,, +0.691848625,0x00,, +0.691968625,0x01,, +0.692088625,0x08,, +0.692208625,0x40,, +0.692328625,0x00,, +0.692448625,0x02,, +0.692568625,0x10,, +0.692688625,0x80,, +0.692808625,0x00,, +0.692928625,0x24,, +0.705049125,0x0F,, +0.705169125,0x11,, +0.705289125,0x04,, +0.705409125,0x20,, +0.705529125,0xA8,, +0.705649125,0x01,, +0.705769125,0x08,, +0.70588925,0x16,, +0.70600925,0x50,, +0.70612925,0x03,, +0.70624925,0x10,, +0.70636925,0x80,, +0.70648925,0x00,, +0.70660925,0x04,, +0.70672925,0x20,, +0.70684925,0x00,, +0.70696925,0x01,, +0.70708925,0x08,, +0.70720925,0x40,, +0.70732925,0x00,, +0.70744925,0x02,, +0.70756925,0x10,, +0.70768925,0x80,, +0.70780925,0x00,, +0.70792925,0x34,, +0.72004975,0x0F,, +0.72016975,0x11,, +0.72028975,0x04,, +0.72040975,0x20,, +0.72052975,0xA8,, +0.72064975,0x01,, +0.72076975,0x08,, +0.72088975,0x16,, +0.72100975,0x50,, +0.721129875,0x03,, +0.721249875,0x10,, +0.721369875,0x80,, +0.721489875,0x00,, +0.721609875,0x04,, +0.721729875,0x20,, +0.721849875,0x00,, +0.721969875,0x01,, +0.722089875,0x08,, +0.722209875,0x40,, +0.722329875,0x00,, +0.722449875,0x02,, +0.722569875,0x10,, +0.722689875,0x80,, +0.722809875,0x00,, +0.722929875,0x04,, +0.72508,0x03,, +0.7252,0xC4,, +0.72532,0x00,, +0.735050375,0x0F,, +0.735170375,0x11,, +0.735290375,0x04,, +0.735410375,0x20,, +0.735530375,0xA8,, +0.735650375,0x01,, +0.735770375,0x08,, +0.735890375,0x16,, +0.736010375,0x50,, +0.736130375,0x03,, +0.7362505,0x10,, +0.7363705,0x80,, +0.7364905,0x00,, +0.7366105,0x04,, +0.7367305,0x20,, +0.7368505,0x00,, +0.7369705,0x01,, +0.7370905,0x08,, +0.7372105,0x40,, +0.7373305,0x00,, +0.7374505,0x02,, +0.7375705,0x10,, +0.7376905,0x80,, +0.7378105,0x00,, +0.7379305,0x14,, +0.750051,0x0F,, +0.750171,0x11,, +0.750291,0x04,, +0.750411,0x20,, +0.750531,0xA8,, +0.750651,0x01,, +0.750771,0x08,, +0.750891,0x16,, +0.751011,0x50,, +0.751131,0x03,, +0.751251,0x10,, +0.751371125,0x80,, +0.751491125,0x00,, +0.751611125,0x04,, +0.751731125,0x20,, +0.751851125,0x00,, +0.751971125,0x01,, +0.752091125,0x08,, +0.752211125,0x40,, +0.752331125,0x00,, +0.752451125,0x02,, +0.752571125,0x10,, +0.752691125,0x80,, +0.752811125,0x00,, +0.752931125,0x24,, +0.765051625,0x0F,, +0.765171625,0x11,, +0.765291625,0x04,, +0.765411625,0x20,, +0.765531625,0xA8,, +0.765651625,0x01,, +0.765771625,0x08,, +0.765891625,0x16,, +0.766011625,0x50,, +0.766131625,0x03,, +0.766251625,0x10,, +0.766371625,0x80,, +0.76649175,0x00,, +0.76661175,0x04,, +0.76673175,0x20,, +0.76685175,0x00,, +0.76697175,0x01,, +0.76709175,0x08,, +0.76721175,0x40,, +0.76733175,0x00,, +0.76745175,0x02,, +0.76757175,0x10,, +0.76769175,0x80,, +0.76781175,0x00,, +0.76793175,0x34,, +0.78005225,0x0F,, +0.78017225,0x11,, +0.78029225,0x04,, +0.78041225,0x20,, +0.78053225,0xA8,, +0.78065225,0x01,, +0.78077225,0x08,, +0.78089225,0x16,, +0.78101225,0x50,, +0.78113225,0x03,, +0.78125225,0x10,, +0.78137225,0x80,, +0.78149225,0x00,, +0.781612375,0x04,, +0.781732375,0x20,, +0.781852375,0x00,, +0.781972375,0x01,, +0.782092375,0x08,, +0.782212375,0x40,, +0.782332375,0x00,, +0.782452375,0x02,, +0.782572375,0x10,, +0.782692375,0x80,, +0.782812375,0x00,, +0.782932375,0x04,, +0.7850825,0x03,, +0.7852025,0xC0,, +0.7853225,0x2E,, +0.795052875,0x0F,, +0.795172875,0x11,, +0.795292875,0x04,, +0.795412875,0x20,, +0.795532875,0xA8,, +0.795652875,0x01,, +0.795772875,0x08,, +0.795892875,0x16,, +0.796012875,0x50,, +0.796132875,0x03,, +0.796252875,0x10,, +0.796372875,0x80,, +0.796492875,0x00,, +0.796612875,0x04,, +0.796733,0x20,, +0.796853,0x00,, +0.796973,0x01,, +0.797093,0x08,, +0.797213,0x40,, +0.797333,0x00,, +0.797453,0x02,, +0.797573,0x10,, +0.797693,0x80,, +0.797813,0x00,, +0.797933,0x14,, +0.8100535,0x0F,, +0.8101735,0x11,, +0.8102935,0x04,, +0.8104135,0x20,, +0.8105335,0xA8,, +0.8106535,0x01,, +0.8107735,0x08,, +0.8108935,0x16,, +0.8110135,0x50,, +0.8111335,0x03,, +0.8112535,0x10,, +0.8113735,0x80,, +0.8114935,0x00,, +0.8116135,0x04,, +0.8117335,0x20,, +0.811853625,0x00,, +0.811973625,0x01,, +0.812093625,0x08,, +0.812213625,0x40,, +0.812333625,0x00,, +0.812453625,0x02,, +0.812573625,0x10,, +0.812693625,0x80,, +0.812813625,0x00,, +0.812933625,0x24,, +0.825064125,0x0F,, +0.825184125,0x11,, +0.825304125,0x04,, +0.825424125,0x20,, +0.825544125,0xA8,, +0.825664125,0x01,, +0.825784125,0x08,, +0.825904125,0x16,, +0.826024125,0x50,, +0.826144125,0x03,, +0.826264125,0x10,, +0.826384125,0x80,, +0.826504125,0x00,, +0.826624125,0x04,, +0.826744125,0x20,, +0.826864125,0x00,, +0.826984125,0x01,, +0.82710425,0x08,, +0.82722425,0x40,, +0.82734425,0x00,, +0.82746425,0x02,, +0.82758425,0x10,, +0.82770425,0x80,, +0.82782425,0x00,, +0.82794425,0x34,, +0.84005475,0x0F,, +0.84017475,0x11,, +0.84029475,0x04,, +0.84041475,0x20,, +0.84053475,0xA8,, +0.84065475,0x01,, +0.84077475,0x08,, +0.84089475,0x16,, +0.84101475,0x50,, +0.84113475,0x03,, +0.84125475,0x10,, +0.84137475,0x80,, +0.84149475,0x00,, +0.84161475,0x04,, +0.84173475,0x20,, +0.84185475,0x00,, +0.84197475,0x01,, +0.84209475,0x08,, +0.842214875,0x40,, +0.842334875,0x00,, +0.842454875,0x02,, +0.842574875,0x10,, +0.842694875,0x80,, +0.842814875,0x00,, +0.842934875,0x04,, +0.845084875,0x03,, +0.845205,0xC4,, +0.845325,0x00,, +0.855055375,0x0F,, +0.855175375,0x11,, +0.855295375,0x04,, +0.855415375,0x20,, +0.855535375,0xA8,, +0.855655375,0x01,, +0.855775375,0x08,, +0.855895375,0x16,, +0.856015375,0x50,, +0.856135375,0x03,, +0.856255375,0x10,, +0.856375375,0x80,, +0.856495375,0x00,, +0.856615375,0x04,, +0.856735375,0x20,, +0.856855375,0x00,, +0.856975375,0x01,, +0.857095375,0x08,, +0.857215375,0x40,, +0.8573355,0x00,, +0.8574555,0x02,, +0.8575755,0x10,, +0.8576955,0x80,, +0.8578155,0x00,, +0.8579355,0x14,, +0.870056,0x0F,, +0.870176,0x11,, +0.870296,0x04,, +0.870416,0x20,, +0.870536,0xA8,, +0.870656,0x01,, +0.870776,0x08,, +0.870896,0x16,, +0.871016,0x50,, +0.871136,0x03,, +0.871256,0x10,, +0.871376,0x80,, +0.871496,0x00,, +0.871616,0x04,, +0.871736,0x20,, +0.871856,0x00,, +0.871976,0x01,, +0.872096,0x08,, +0.872216,0x40,, +0.872336,0x00,, +0.872456125,0x02,, +0.872576125,0x10,, +0.872696125,0x80,, +0.872816125,0x00,, +0.872936125,0x24,, +0.885066625,0x0F,, +0.885186625,0x11,, +0.885306625,0x04,, +0.885426625,0x20,, +0.885546625,0xA8,, +0.885666625,0x01,, +0.885786625,0x08,, +0.885906625,0x16,, +0.886026625,0x50,, +0.886146625,0x03,, +0.886266625,0x10,, +0.886386625,0x80,, +0.886506625,0x00,, +0.886626625,0x04,, +0.886746625,0x20,, +0.886866625,0x00,, +0.886986625,0x01,, +0.887106625,0x08,, +0.887226625,0x40,, +0.887346625,0x00,, +0.887466625,0x02,, +0.88758675,0x10,, +0.88770675,0x80,, +0.88782675,0x00,, +0.88794675,0x34,, +0.90006725,0x0F,, +0.90018725,0x11,, +0.90030725,0x04,, +0.90042725,0x20,, +0.90054725,0xA8,, +0.90066725,0x01,, +0.90078725,0x08,, +0.90090725,0x16,, +0.90102725,0x50,, +0.90114725,0x03,, +0.90126725,0x10,, +0.90138725,0x80,, +0.90150725,0x00,, +0.90162725,0x04,, +0.90174725,0x20,, +0.90186725,0x00,, +0.90198725,0x01,, +0.90210725,0x08,, +0.90222725,0x40,, +0.90234725,0x00,, +0.90246725,0x02,, +0.90258725,0x10,, +0.902707375,0x80,, +0.902827375,0x00,, +0.902947375,0x04,, +0.905097375,0x03,, +0.905217375,0xC0,, +0.905337375,0x2E,, +0.915067875,0x0F,, +0.915187875,0x11,, +0.915307875,0x04,, +0.915427875,0x20,, +0.915547875,0xA8,, +0.915667875,0x01,, +0.915787875,0x08,, +0.915907875,0x16,, +0.916027875,0x50,, +0.916147875,0x03,, +0.916267875,0x10,, +0.916387875,0x80,, +0.916507875,0x00,, +0.916627875,0x04,, +0.916747875,0x20,, +0.916867875,0x00,, +0.916987875,0x01,, +0.917107875,0x08,, +0.917227875,0x40,, +0.917347875,0x00,, +0.917467875,0x02,, +0.917587875,0x10,, +0.917707875,0x80,, +0.917828,0x00,, +0.917948,0x14,, +0.9300685,0x0F,, +0.9301885,0x11,, +0.9303085,0x04,, +0.9304285,0x20,, +0.9305485,0xA8,, +0.9306685,0x01,, +0.9307885,0x08,, +0.9309085,0x16,, +0.9310285,0x50,, +0.9311485,0x03,, +0.9312685,0x10,, +0.9313885,0x80,, +0.9315085,0x00,, +0.9316285,0x04,, +0.9317485,0x20,, +0.9318685,0x00,, +0.9319885,0x01,, +0.9321085,0x08,, +0.9322285,0x40,, +0.9323485,0x00,, +0.9324685,0x02,, +0.9325885,0x10,, +0.9327085,0x80,, +0.9328285,0x00,, +0.9329485,0x24,, +0.945069,0x0F,, +0.945189125,0x11,, +0.945309125,0x04,, +0.945429125,0x20,, +0.945549125,0xA8,, +0.945669125,0x01,, +0.945789125,0x08,, +0.945909125,0x16,, +0.946029125,0x50,, +0.946149125,0x03,, +0.946269125,0x10,, +0.946389125,0x80,, +0.946509125,0x00,, +0.946629125,0x04,, +0.946749125,0x20,, +0.946869125,0x00,, +0.946989125,0x01,, +0.947109125,0x08,, +0.947229125,0x40,, +0.947349125,0x00,, +0.947469125,0x02,, +0.947589125,0x10,, +0.947709125,0x80,, +0.947829125,0x00,, +0.947949125,0x34,, +0.960069625,0x0F,, +0.960189625,0x11,, +0.96030975,0x04,, +0.96042975,0x20,, +0.96054975,0xA8,, +0.96066975,0x01,, +0.96078975,0x08,, +0.96090975,0x16,, +0.96102975,0x50,, +0.96114975,0x03,, +0.96126975,0x10,, +0.96138975,0x80,, +0.96150975,0x00,, +0.96162975,0x04,, +0.96174975,0x20,, +0.96186975,0x00,, +0.96198975,0x01,, +0.96210975,0x08,, +0.96222975,0x40,, +0.96234975,0x00,, +0.96246975,0x02,, +0.96258975,0x10,, +0.96270975,0x80,, +0.96282975,0x00,, +0.96294975,0x04,, +0.965099875,0x03,, +0.965219875,0xC4,, +0.965339875,0x00,, +0.97506025,0x0F,, +0.97518025,0x11,, +0.97530025,0x04,, +0.975420375,0x20,, +0.975540375,0xA8,, +0.975660375,0x01,, +0.975780375,0x08,, +0.975900375,0x16,, +0.976020375,0x50,, +0.976140375,0x03,, +0.976260375,0x10,, +0.976380375,0x80,, +0.976500375,0x00,, +0.976620375,0x04,, +0.976740375,0x20,, +0.976860375,0x00,, +0.976980375,0x01,, +0.977100375,0x08,, +0.977220375,0x40,, +0.977340375,0x00,, +0.977460375,0x02,, +0.977580375,0x10,, +0.977700375,0x80,, +0.977820375,0x00,, +0.977940375,0x14,, +0.990070875,0x0F,, +0.990190875,0x11,, +0.990310875,0x04,, +0.990430875,0x20,, +0.990551,0xA8,, +0.990671,0x01,, +0.990791,0x08,, +0.990911,0x16,, +0.991031,0x50,, +0.991151,0x03,, +0.991271,0x10,, +0.991391,0x80,, +0.991511,0x00,, +0.991631,0x04,, +0.991751,0x20,, +0.991871,0x00,, +0.991991,0x01,, +0.992111,0x08,, +0.992231,0x40,, +0.992351,0x00,, +0.992471,0x02,, +0.992591,0x10,, +0.992711,0x80,, +0.992831,0x00,, +0.992951,0x24,, +1.0050715,0x0F,, +1.0051915,0x11,, +1.0053115,0x04,, +1.0054315,0x20,, +1.0055515,0xA8,, +1.005671625,0x01,, +1.005791625,0x08,, +1.005911625,0x16,, +1.006031625,0x50,, +1.006151625,0x03,, +1.006271625,0x10,, +1.006391625,0x80,, +1.006511625,0x00,, +1.006631625,0x04,, +1.006751625,0x20,, +1.006871625,0x00,, +1.006991625,0x01,, +1.007111625,0x08,, +1.007231625,0x40,, +1.007351625,0x00,, +1.007471625,0x02,, +1.007591625,0x10,, +1.007711625,0x80,, +1.007831625,0x00,, +1.007951625,0x34,, +1.020072125,0x0F,, +1.020192125,0x11,, +1.020312125,0x04,, +1.020432125,0x20,, +1.020552125,0xA8,, +1.020672125,0x01,, +1.02079225,0x08,, +1.02091225,0x16,, +1.02103225,0x50,, +1.02115225,0x03,, +1.02127225,0x10,, +1.02139225,0x80,, +1.02151225,0x00,, +1.02163225,0x04,, +1.02175225,0x20,, +1.02187225,0x00,, +1.02199225,0x01,, +1.02211225,0x08,, +1.02223225,0x40,, +1.02235225,0x00,, +1.02247225,0x02,, +1.02259225,0x10,, +1.02271225,0x80,, +1.02283225,0x00,, +1.02295225,0x04,, +1.025102375,0x03,, +1.025222375,0xC0,, +1.025342375,0x2E,, +1.03507275,0x0F,, +1.03519275,0x11,, +1.03531275,0x04,, +1.03543275,0x20,, +1.03555275,0xA8,, +1.03567275,0x01,, +1.03579275,0x08,, +1.03591275,0x16,, +1.036032875,0x50,, +1.036152875,0x03,, +1.036272875,0x10,, +1.036392875,0x80,, +1.036512875,0x00,, +1.036632875,0x04,, +1.036752875,0x20,, +1.036872875,0x00,, +1.036992875,0x01,, +1.037112875,0x08,, +1.037232875,0x40,, +1.037352875,0x00,, +1.037472875,0x02,, +1.037592875,0x10,, +1.037712875,0x80,, +1.037832875,0x00,, +1.037952875,0x14,, +1.050063375,0x0F,, +1.050183375,0x11,, +1.050303375,0x04,, +1.050423375,0x20,, +1.050543375,0xA8,, +1.050663375,0x01,, +1.050783375,0x08,, +1.050903375,0x16,, +1.051023375,0x50,, +1.0511435,0x03,, +1.0512635,0x10,, +1.0513835,0x80,, +1.0515035,0x00,, +1.0516235,0x04,, +1.0517435,0x20,, +1.0518635,0x00,, +1.0519835,0x01,, +1.0521035,0x08,, +1.0522235,0x40,, +1.0523435,0x00,, +1.0524635,0x02,, +1.0525835,0x10,, +1.0527035,0x80,, +1.0528235,0x00,, +1.0529435,0x24,, +1.065074,0x0F,, +1.065194,0x11,, +1.065314,0x04,, +1.065434,0x20,, +1.065554,0xA8,, +1.065674,0x01,, +1.065794,0x08,, +1.065914,0x16,, +1.066034,0x50,, +1.066154,0x03,, +1.066274125,0x10,, +1.066394125,0x80,, +1.066514125,0x00,, +1.066634125,0x04,, +1.066754125,0x20,, +1.066874125,0x00,, +1.066994125,0x01,, +1.067114125,0x08,, +1.067234125,0x40,, +1.067354125,0x00,, +1.067474125,0x02,, +1.067594125,0x10,, +1.067714125,0x80,, +1.067834125,0x00,, +1.067954125,0x34,, +1.080074625,0x0F,, +1.080194625,0x11,, +1.080314625,0x04,, +1.080434625,0x20,, +1.080554625,0xA8,, +1.080674625,0x01,, +1.080794625,0x08,, +1.080914625,0x16,, +1.081034625,0x50,, +1.081154625,0x03,, +1.081274625,0x10,, +1.08139475,0x80,, +1.08151475,0x00,, +1.08163475,0x04,, +1.08175475,0x20,, +1.08187475,0x00,, +1.08199475,0x01,, +1.08211475,0x08,, +1.08223475,0x40,, +1.08235475,0x00,, +1.08247475,0x02,, +1.08259475,0x10,, +1.08271475,0x80,, +1.08283475,0x00,, +1.08295475,0x04,, +1.085104875,0x03,, +1.085224875,0xC4,, +1.085344875,0x00,, +1.09507525,0x0F,, +1.09519525,0x11,, +1.09531525,0x04,, +1.09543525,0x20,, +1.09555525,0xA8,, +1.09567525,0x01,, +1.09579525,0x08,, +1.09591525,0x16,, +1.09603525,0x50,, +1.09615525,0x03,, +1.09627525,0x10,, +1.09639525,0x80,, +1.096515375,0x00,, +1.096635375,0x04,, +1.096755375,0x20,, +1.096875375,0x00,, +1.096995375,0x01,, +1.097115375,0x08,, +1.097235375,0x40,, +1.097355375,0x00,, +1.097475375,0x02,, +1.097595375,0x10,, +1.097715375,0x80,, +1.097835375,0x00,, +1.097955375,0x14,, +1.110075875,0x0F,, +1.110195875,0x11,, +1.110315875,0x04,, +1.110435875,0x20,, +1.110555875,0xA8,, +1.110675875,0x01,, +1.110795875,0x08,, +1.110915875,0x16,, +1.111035875,0x50,, +1.111155875,0x03,, +1.111275875,0x10,, +1.111395875,0x80,, +1.111515875,0x00,, +1.111636,0x04,, +1.111756,0x20,, +1.111876,0x00,, +1.111996,0x01,, +1.112116,0x08,, +1.112236,0x40,, +1.112356,0x00,, +1.112476,0x02,, +1.112596,0x10,, +1.112716,0x80,, +1.112836,0x00,, +1.112956,0x24,, +1.1250765,0x0F,, +1.1251965,0x11,, +1.1253165,0x04,, +1.1254365,0x20,, +1.1255565,0xA8,, +1.1256765,0x01,, +1.1257965,0x08,, +1.1259165,0x16,, +1.1260365,0x50,, +1.1261565,0x03,, +1.1262765,0x10,, +1.1263965,0x80,, +1.1265165,0x00,, +1.1266365,0x04,, +1.126756625,0x20,, +1.126876625,0x00,, +1.126996625,0x01,, +1.127116625,0x08,, +1.127236625,0x40,, +1.127356625,0x00,, +1.127476625,0x02,, +1.127596625,0x10,, +1.127716625,0x80,, +1.127836625,0x00,, +1.127956625,0x34,, +1.140077125,0x0F,, +1.140197125,0x11,, +1.140317125,0x04,, +1.140437125,0x20,, +1.140557125,0xA8,, +1.140677125,0x01,, +1.140797125,0x08,, +1.140917125,0x16,, +1.141037125,0x50,, +1.141157125,0x03,, +1.141277125,0x10,, +1.141397125,0x80,, +1.141517125,0x00,, +1.141637125,0x04,, +1.141757125,0x20,, +1.141877125,0x00,, +1.14199725,0x01,, +1.14211725,0x08,, +1.14223725,0x40,, +1.14235725,0x00,, +1.14247725,0x02,, +1.14259725,0x10,, +1.14271725,0x80,, +1.14283725,0x00,, +1.14295725,0x04,, +1.145107375,0x03,, +1.145227375,0xC0,, +1.145347375,0x2E,, +1.15507775,0x0F,, +1.15519775,0x11,, +1.15531775,0x04,, +1.15543775,0x20,, +1.15555775,0xA8,, +1.15567775,0x01,, +1.15579775,0x08,, +1.15591775,0x16,, +1.15603775,0x50,, +1.15615775,0x03,, +1.15627775,0x10,, +1.15639775,0x80,, +1.15651775,0x00,, +1.15663775,0x04,, +1.15675775,0x20,, +1.15687775,0x00,, +1.15699775,0x01,, +1.157117875,0x08,, +1.157237875,0x40,, +1.157357875,0x00,, +1.157477875,0x02,, +1.157597875,0x10,, +1.157717875,0x80,, +1.157837875,0x00,, +1.157957875,0x14,, +1.170078375,0x0F,, +1.170198375,0x11,, +1.170318375,0x04,, +1.170438375,0x20,, +1.170558375,0xA8,, +1.170678375,0x01,, +1.170798375,0x08,, +1.170918375,0x16,, +1.171038375,0x50,, +1.171158375,0x03,, +1.171278375,0x10,, +1.171398375,0x80,, +1.171518375,0x00,, +1.171638375,0x04,, +1.171758375,0x20,, +1.171878375,0x00,, +1.171998375,0x01,, +1.172118375,0x08,, +1.1722385,0x40,, +1.1723585,0x00,, +1.1724785,0x02,, +1.1725985,0x10,, +1.1727185,0x80,, +1.1728385,0x00,, +1.1729585,0x24,, +1.185079,0x0F,, +1.185199,0x11,, +1.185319,0x04,, +1.185439,0x20,, +1.185559,0xA8,, +1.185679,0x01,, +1.185799,0x08,, +1.185919,0x16,, +1.186039,0x50,, +1.186159,0x03,, +1.186279,0x10,, +1.186399,0x80,, +1.186519,0x00,, +1.186639,0x04,, +1.186759,0x20,, +1.186879,0x00,, +1.186999,0x01,, +1.187119,0x08,, +1.187239,0x40,, +1.187359125,0x00,, +1.187479125,0x02,, +1.187599125,0x10,, +1.187719125,0x80,, +1.187839125,0x00,, +1.187959125,0x34,, +1.200079625,0x0F,, +1.200199625,0x11,, +1.200319625,0x04,, +1.200439625,0x20,, +1.200559625,0xA8,, +1.200679625,0x01,, +1.200799625,0x08,, +1.200919625,0x16,, +1.201039625,0x50,, +1.201159625,0x03,, +1.201279625,0x10,, +1.201399625,0x80,, +1.201519625,0x00,, +1.201639625,0x04,, +1.201759625,0x20,, +1.201879625,0x00,, +1.201999625,0x01,, +1.202119625,0x08,, +1.202239625,0x40,, +1.202359625,0x00,, +1.20247975,0x02,, +1.20259975,0x10,, +1.20271975,0x80,, +1.20283975,0x00,, +1.20295975,0x04,, +1.20510975,0x03,, +1.20522975,0xC4,, +1.20534975,0x00,, +1.21509025,0x0F,, +1.21521025,0x11,, +1.21533025,0x04,, +1.21545025,0x20,, +1.21557025,0xA8,, +1.21569025,0x01,, +1.21581025,0x08,, +1.21593025,0x16,, +1.21605025,0x50,, +1.21617025,0x03,, +1.21629025,0x10,, +1.21641025,0x80,, +1.21653025,0x00,, +1.21665025,0x04,, +1.21677025,0x20,, +1.21689025,0x00,, +1.21701025,0x01,, +1.21713025,0x08,, +1.21725025,0x40,, +1.21737025,0x00,, +1.21749025,0x02,, +1.217610375,0x10,, +1.217730375,0x80,, +1.217850375,0x00,, +1.217970375,0x14,, +1.230090875,0x0F,, +1.230210875,0x11,, +1.230330875,0x04,, +1.230450875,0x20,, +1.230570875,0xA8,, +1.230690875,0x01,, +1.230810875,0x08,, +1.230930875,0x16,, +1.231050875,0x50,, +1.231170875,0x03,, +1.231290875,0x10,, +1.231410875,0x80,, +1.231530875,0x00,, +1.231650875,0x04,, +1.231770875,0x20,, +1.231890875,0x00,, +1.232010875,0x01,, +1.232130875,0x08,, +1.232250875,0x40,, +1.232370875,0x00,, +1.232490875,0x02,, +1.232610875,0x10,, +1.232730875,0x80,, +1.232851,0x00,, +1.232971,0x24,, +1.2450915,0x0F,, +1.2452115,0x11,, +1.2453315,0x04,, +1.2454515,0x20,, +1.2455715,0xA8,, +1.2456915,0x01,, +1.2458115,0x08,, +1.2459315,0x16,, +1.2460515,0x50,, +1.2461715,0x03,, +1.2462915,0x10,, +1.2464115,0x80,, +1.2465315,0x00,, +1.2466515,0x04,, +1.2467715,0x20,, +1.2468915,0x00,, +1.2470115,0x01,, +1.2471315,0x08,, +1.2472515,0x40,, +1.2473715,0x00,, +1.2474915,0x02,, +1.2476115,0x10,, +1.2477315,0x80,, +1.2478515,0x00,, +1.247971625,0x34,, +1.260082125,0x0F,, +1.260202125,0x11,, +1.260322125,0x04,, +1.260442125,0x20,, +1.260562125,0xA8,, +1.260682125,0x01,, +1.260802125,0x08,, +1.260922125,0x16,, +1.261042125,0x50,, +1.261162125,0x03,, +1.261282125,0x10,, +1.261402125,0x80,, +1.261522125,0x00,, +1.261642125,0x04,, +1.261762125,0x20,, +1.261882125,0x00,, +1.262002125,0x01,, +1.262122125,0x08,, +1.262242125,0x40,, +1.262362125,0x00,, +1.262482125,0x02,, +1.262602125,0x10,, +1.262722125,0x80,, +1.262842125,0x00,, +1.262962125,0x04,, +1.26511225,0x03,, +1.26523225,0xC0,, +1.26535225,0x2E,, diff --git a/unittests/testdata/st24_data.txt b/unittests/testdata/st24_data.txt new file mode 100644 index 0000000000..f6a39ab0b5 --- /dev/null +++ b/unittests/testdata/st24_data.txt @@ -0,0 +1,18019 @@ +Time [s],Value,Parity Error,Framing Error +-8.75e-06,0x55,, +8.675e-05,0x55,, +0.00018225,0x2B,, +0.000277625,0x03,, +0.000373125,0xD8,, +0.000468625,0x76,, +0.000564125,0xDC,, +0.000659625,0x00,, +0.000755,0x2B,, +0.0008505,0xC8,, +0.000946,0x00,, +0.001041375,0x80,, +0.001136875,0x08,, +0.001232375,0x00,, +0.001327875,0xD5,, +0.00142325,0x42,, +0.00151875,0xAB,, +0.00161425,0x80,, +0.00170975,0x0D,, +0.001805125,0x54,, +0.001900625,0x80,, +0.001996125,0x08,, +0.002091625,0x00,, +0.002187,0x2A,, +0.0022825,0x62,, +0.002378,0xA6,, +0.002473375,0x00,, +0.002568875,0x00,, +0.002664375,0x00,, +0.002759875,0x00,, +0.002855375,0x00,, +0.00295075,0x00,, +0.00304625,0x00,, +0.00314175,0x00,, +0.003237125,0x00,, +0.003332625,0x00,, +0.003428125,0x00,, +0.003523625,0x00,, +0.003619,0x00,, +0.0037145,0x00,, +0.00381,0x00,, +0.0039055,0x00,, +0.004000875,0x00,, +0.004096375,0x00,, +0.004191875,0x00,, +0.00428725,0xD7,, +0.019683625,0x55,, +0.019779125,0x55,, +0.0198745,0x18,, +0.01997,0x00,, +0.0200655,0xD9,, +0.020160875,0x3C,, +0.020256375,0xDB,, +0.020351875,0x00,, +0.020447375,0x2B,, +0.02054275,0xC8,, +0.02063825,0x00,, +0.02073375,0x80,, +0.02082925,0x08,, +0.020924625,0x00,, +0.021020125,0xD5,, +0.021115625,0x42,, +0.021211125,0xAB,, +0.0213065,0x80,, +0.021402,0x0D,, +0.0214975,0x54,, +0.021593,0x80,, +0.021688375,0x08,, +0.021783875,0x00,, +0.021879375,0x2A,, +0.021974875,0x62,, +0.02207025,0xA6,, +0.02216575,0x8C,, +0.039783875,0x55,, +0.03987925,0x55,, +0.03997475,0x18,, +0.04007025,0x00,, +0.04016575,0xDA,, +0.040261125,0x02,, +0.040356625,0xDE,, +0.040452125,0x00,, +0.040547625,0x2B,, +0.040643,0xD8,, +0.0407385,0x00,, +0.040834,0x80,, +0.040929375,0x08,, +0.041024875,0x00,, +0.041120375,0xD5,, +0.041215875,0x42,, +0.04131125,0xAB,, +0.04140675,0x80,, +0.04150225,0x0D,, +0.04159775,0x54,, +0.041693125,0x80,, +0.041788625,0x08,, +0.041884125,0x00,, +0.0419795,0x2A,, +0.042075,0x62,, +0.0421705,0xA6,, +0.042266,0x0E,, +0.059884,0x55,, +0.0599795,0x55,, +0.060075,0x18,, +0.0601705,0x00,, +0.060265875,0xDA,, +0.060361375,0xC8,, +0.060456875,0xDC,, +0.06055225,0x00,, +0.06064775,0x2B,, +0.06074325,0xC8,, +0.06083875,0x00,, +0.060934125,0x80,, +0.061029625,0x08,, +0.061125125,0x00,, +0.061220625,0xD5,, +0.061316125,0x42,, +0.0614115,0xAB,, +0.061507,0x80,, +0.0616025,0x0D,, +0.061697875,0x54,, +0.061793375,0x80,, +0.061888875,0x08,, +0.061984375,0x00,, +0.06207975,0x2A,, +0.06217525,0x62,, +0.06227075,0xA6,, +0.06236625,0xB5,, +0.079975625,0x55,, +0.080071,0x55,, +0.0801665,0x18,, +0.080262,0x00,, +0.0803575,0xDB,, +0.080452875,0x8F,, +0.080548375,0xDD,, +0.080643875,0x00,, +0.080739375,0x2B,, +0.08083475,0xC8,, +0.08093025,0x00,, +0.08102575,0x80,, +0.081121125,0x08,, +0.081216625,0x00,, +0.081312125,0xD5,, +0.081407625,0x42,, +0.081503,0xAB,, +0.0815985,0x80,, +0.081694,0x0D,, +0.0817895,0x54,, +0.081885,0x80,, +0.081980375,0x08,, +0.082075875,0x00,, +0.082171375,0x2A,, +0.08226675,0x62,, +0.08236225,0xA6,, +0.08245775,0x0E,, +0.10007575,0x55,, +0.10017125,0x55,, +0.10026675,0x18,, +0.10036225,0x00,, +0.100457625,0xDC,, +0.100553125,0x56,, +0.100648625,0xDD,, +0.100744125,0x00,, +0.1008395,0x2B,, +0.100935,0x88,, +0.1010305,0x00,, +0.101125875,0x80,, +0.101221375,0x08,, +0.101316875,0x00,, +0.101412375,0xD5,, +0.101507875,0x42,, +0.10160325,0xAB,, +0.10169875,0x80,, +0.10179425,0x0D,, +0.101889625,0x54,, +0.101985125,0x80,, +0.102080625,0x08,, +0.102176125,0x00,, +0.1022715,0x2A,, +0.102367,0x62,, +0.1024625,0xA6,, +0.102558,0x12,, +0.120176,0x55,, +0.1202715,0x55,, +0.120367,0x18,, +0.120462375,0x00,, +0.120557875,0xDD,, +0.120653375,0x1C,, +0.120748875,0xDD,, +0.12084425,0x00,, +0.12093975,0x2B,, +0.12103525,0xA8,, +0.12113075,0x00,, +0.121226125,0x80,, +0.121321625,0x08,, +0.121417125,0x00,, +0.121512625,0xD5,, +0.121608,0x42,, +0.1217035,0xAB,, +0.121799,0x80,, +0.121894375,0x0D,, +0.121989875,0x54,, +0.122085375,0x80,, +0.122180875,0x08,, +0.122276375,0x00,, +0.12237175,0x2A,, +0.12246725,0x62,, +0.12256275,0xA6,, +0.122658125,0xAD,, +0.14027625,0x55,, +0.14037175,0x55,, +0.140467125,0x18,, +0.140562625,0x00,, +0.140658125,0xDD,, +0.140753625,0xE2,, +0.140849,0xDA,, +0.1409445,0x00,, +0.14104,0x2B,, +0.1411355,0x98,, +0.141230875,0x00,, +0.141326375,0x80,, +0.141421875,0x08,, +0.14151725,0x00,, +0.14161275,0xD5,, +0.14170825,0x42,, +0.14180375,0xAB,, +0.14189925,0x80,, +0.141994625,0x0D,, +0.142090125,0x54,, +0.142185625,0x80,, +0.142281,0x08,, +0.1423765,0x00,, +0.142472,0x2A,, +0.1425675,0x62,, +0.142662875,0xA6,, +0.142758375,0xF8,, +0.1603765,0x55,, +0.160471875,0x55,, +0.160567375,0x18,, +0.160662875,0x00,, +0.160758375,0xDE,, +0.16085375,0xA8,, +0.16094925,0xDB,, +0.16104475,0x00,, +0.16114025,0x2B,, +0.161235625,0xA8,, +0.161331125,0x00,, +0.161426625,0x80,, +0.161522125,0x08,, +0.1616175,0x00,, +0.161713,0xD5,, +0.1618085,0x42,, +0.161904,0xAB,, +0.161999375,0x80,, +0.162094875,0x0D,, +0.162190375,0x54,, +0.16228575,0x80,, +0.16238125,0x08,, +0.16247675,0x00,, +0.16257225,0x2A,, +0.162667625,0x62,, +0.162763125,0xA6,, +0.162858625,0x2F,, +0.18047675,0x55,, +0.180572125,0x55,, +0.180667625,0x18,, +0.180763125,0x00,, +0.1808585,0xDF,, +0.180954,0x6D,, +0.1810495,0xDB,, +0.181145,0x00,, +0.181240375,0x2B,, +0.181335875,0x98,, +0.181431375,0x00,, +0.181526875,0x80,, +0.18162225,0x08,, +0.18171775,0x00,, +0.18181325,0xD5,, +0.181908625,0x42,, +0.182004125,0xAB,, +0.182099625,0x80,, +0.182195125,0x0D,, +0.1822905,0x54,, +0.182386,0x80,, +0.1824815,0x08,, +0.182577,0x00,, +0.1826725,0x2A,, +0.182767875,0x62,, +0.182863375,0xA6,, +0.182958875,0x39,, +0.200967375,0x55,, +0.201062875,0x55,, +0.201158375,0x2B,, +0.201253875,0x03,, +0.201349375,0xE0,, +0.20144475,0x32,, +0.20154025,0xDE,, +0.20163575,0x00,, +0.20173125,0x2B,, +0.201826625,0x98,, +0.201922125,0x00,, +0.202017625,0x80,, +0.202113,0x08,, +0.2022085,0x00,, +0.202304,0xD5,, +0.2023995,0x42,, +0.202494875,0xAB,, +0.202590375,0x80,, +0.202685875,0x0D,, +0.202781375,0x54,, +0.20287675,0x80,, +0.20297225,0x08,, +0.20306775,0x00,, +0.20316325,0x2A,, +0.203258625,0x62,, +0.203354125,0xA6,, +0.203449625,0x00,, +0.203545125,0x00,, +0.2036405,0x00,, +0.203736,0x00,, +0.2038315,0x00,, +0.203926875,0x00,, +0.204022375,0x00,, +0.204117875,0x00,, +0.204213375,0x00,, +0.20430875,0x00,, +0.20440425,0x00,, +0.20449975,0x00,, +0.20459525,0x00,, +0.20469075,0x00,, +0.204786125,0x00,, +0.204881625,0x00,, +0.204977125,0x00,, +0.2050725,0x00,, +0.205168,0x00,, +0.2052635,0xA9,, +0.2206685,0x55,, +0.220763875,0x55,, +0.220859375,0x18,, +0.220954875,0x00,, +0.221050375,0xE0,, +0.22114575,0xF9,, +0.22124125,0xD9,, +0.22133675,0x00,, +0.221432125,0x2B,, +0.221527625,0xA8,, +0.221623125,0x00,, +0.221718625,0x80,, +0.221814,0x08,, +0.2219095,0x00,, +0.222005,0xD5,, +0.2221005,0x42,, +0.222195875,0xAB,, +0.222291375,0x80,, +0.222386875,0x0D,, +0.222482375,0x54,, +0.22257775,0x80,, +0.22267325,0x08,, +0.22276875,0x00,, +0.22286425,0x2A,, +0.222959625,0x62,, +0.223055125,0xA6,, +0.223150625,0x78,, +0.240768625,0x55,, +0.240864125,0x55,, +0.240959625,0x18,, +0.241055,0x00,, +0.2411505,0xE1,, +0.241246,0xBF,, +0.2413415,0xDB,, +0.241436875,0x00,, +0.241532375,0x2B,, +0.241627875,0x98,, +0.241723375,0x00,, +0.24181875,0x80,, +0.24191425,0x08,, +0.24200975,0x00,, +0.24210525,0xD5,, +0.242200625,0x42,, +0.242296125,0xAB,, +0.242391625,0x80,, +0.242487125,0x0D,, +0.2425825,0x54,, +0.242678,0x80,, +0.2427735,0x08,, +0.242869,0x00,, +0.242964375,0x2A,, +0.243059875,0x62,, +0.243155375,0xA6,, +0.24325075,0x54,, +0.26086025,0x55,, +0.260955625,0x55,, +0.261051125,0x18,, +0.261146625,0x00,, +0.261242125,0xE2,, +0.2613375,0x86,, +0.261433,0xDD,, +0.2615285,0x00,, +0.261623875,0x2B,, +0.261719375,0xB8,, +0.261814875,0x00,, +0.261910375,0x80,, +0.262005875,0x08,, +0.26210125,0x00,, +0.26219675,0xD5,, +0.26229225,0x42,, +0.262387625,0xAB,, +0.262483125,0x80,, +0.262578625,0x0D,, +0.262674125,0x54,, +0.2627695,0x80,, +0.262865,0x08,, +0.2629605,0x00,, +0.263056,0x2A,, +0.263151375,0x62,, +0.263246875,0xA6,, +0.263342375,0xD7,, +0.2809605,0x55,, +0.281055875,0x55,, +0.281151375,0x18,, +0.28124675,0x00,, +0.28134225,0xE3,, +0.28143775,0x4B,, +0.28153325,0xDC,, +0.28162875,0x00,, +0.281724125,0x2B,, +0.281819625,0xB8,, +0.281915125,0x00,, +0.282010625,0x80,, +0.282106,0x08,, +0.2822015,0x00,, +0.282297,0xD5,, +0.282392375,0x42,, +0.282487875,0xAB,, +0.282583375,0x80,, +0.282678875,0x0D,, +0.28277425,0x54,, +0.28286975,0x80,, +0.28296525,0x08,, +0.28306075,0x00,, +0.283156125,0x2A,, +0.283251625,0x62,, +0.283347125,0xA6,, +0.283442625,0xF8,, +0.301060625,0x55,, +0.301156125,0x55,, +0.301251625,0x18,, +0.301347,0x00,, +0.3014425,0xE4,, +0.301538,0x10,, +0.3016335,0xDB,, +0.301728875,0x00,, +0.301824375,0x2B,, +0.301919875,0x88,, +0.30201525,0x00,, +0.30211075,0x80,, +0.30220625,0x08,, +0.30230175,0x00,, +0.302397125,0xD5,, +0.302492625,0x42,, +0.302588125,0xAB,, +0.302683625,0x80,, +0.302779125,0x0D,, +0.3028745,0x54,, +0.30297,0x80,, +0.3030655,0x08,, +0.303160875,0x00,, +0.303256375,0x2A,, +0.303351875,0x62,, +0.303447375,0xA6,, +0.30354275,0x28,, +0.32115225,0x55,, +0.321247625,0x55,, +0.321343125,0x18,, +0.321438625,0x00,, +0.321534,0xE4,, +0.3216295,0xD6,, +0.321725,0xDA,, +0.3218205,0x00,, +0.321915875,0x2B,, +0.322011375,0xB8,, +0.322106875,0x00,, +0.322202375,0x80,, +0.32229775,0x08,, +0.32239325,0x00,, +0.32248875,0xD5,, +0.322584125,0x42,, +0.322679625,0xAB,, +0.322775125,0x80,, +0.322870625,0x0D,, +0.322966125,0x54,, +0.3230615,0x80,, +0.323157,0x08,, +0.3232525,0x00,, +0.323347875,0x2A,, +0.323443375,0x62,, +0.323538875,0xA6,, +0.323634375,0x0B,, +0.341252375,0x55,, +0.341347875,0x55,, +0.341443375,0x18,, +0.34153875,0x00,, +0.34163425,0xE5,, +0.34172975,0x9D,, +0.34182525,0xDD,, +0.341920625,0x00,, +0.342016125,0x2B,, +0.342111625,0xA8,, +0.342207125,0x00,, +0.3423025,0x80,, +0.342398,0x08,, +0.3424935,0x00,, +0.342589,0xD5,, +0.342684375,0x42,, +0.342779875,0xAB,, +0.342875375,0x80,, +0.342970875,0x0D,, +0.34306625,0x54,, +0.34316175,0x80,, +0.34325725,0x08,, +0.343352625,0x00,, +0.343448125,0x2A,, +0.343543625,0x62,, +0.343639125,0xA6,, +0.3437345,0x93,, +0.361352625,0x55,, +0.361448125,0x55,, +0.361543625,0x18,, +0.361639,0x00,, +0.3617345,0xE6,, +0.36183,0x64,, +0.361925375,0xD9,, +0.362020875,0x00,, +0.362116375,0x2B,, +0.362211875,0xC8,, +0.36230725,0x00,, +0.36240275,0x80,, +0.36249825,0x08,, +0.36259375,0x00,, +0.362689125,0xD5,, +0.362784625,0x42,, +0.362880125,0xAB,, +0.3629755,0x80,, +0.363071,0x0D,, +0.3631665,0x54,, +0.363262,0x80,, +0.363357375,0x08,, +0.363452875,0x00,, +0.363548375,0x2A,, +0.363643875,0x62,, +0.363739375,0xA6,, +0.36383475,0x7F,, +0.381444125,0x55,, +0.381539625,0x55,, +0.381635125,0x18,, +0.3817305,0x00,, +0.381826,0xE7,, +0.3819215,0x2A,, +0.382017,0xDC,, +0.3821125,0x00,, +0.382207875,0x2B,, +0.382303375,0x98,, +0.382398875,0x00,, +0.38249425,0x80,, +0.38258975,0x08,, +0.38268525,0x00,, +0.38278075,0xD5,, +0.382876125,0x42,, +0.382971625,0xAB,, +0.383067125,0x80,, +0.383162625,0x0D,, +0.383258,0x54,, +0.3833535,0x80,, +0.383449,0x08,, +0.383544375,0x00,, +0.383639875,0x2A,, +0.383735375,0x62,, +0.383830875,0xA6,, +0.383926375,0xEB,, +0.401943625,0x55,, +0.402039125,0x55,, +0.4021345,0x2B,, +0.40223,0x03,, +0.4023255,0xE7,, +0.402421,0xF1,, +0.402516375,0xDB,, +0.402611875,0x00,, +0.402707375,0x2B,, +0.40280275,0xC8,, +0.40289825,0x00,, +0.40299375,0x80,, +0.40308925,0x08,, +0.403184625,0x00,, +0.403280125,0xD5,, +0.403375625,0x42,, +0.403471125,0xAB,, +0.403566625,0x80,, +0.403662,0x0D,, +0.4037575,0x54,, +0.403853,0x80,, +0.403948375,0x08,, +0.404043875,0x00,, +0.404139375,0x2A,, +0.404234875,0x62,, +0.40433025,0xA6,, +0.40442575,0x00,, +0.40452125,0x00,, +0.40461675,0x00,, +0.404712125,0x00,, +0.404807625,0x00,, +0.404903125,0x00,, +0.4049985,0x00,, +0.405094,0x00,, +0.4051895,0x00,, +0.405285,0x00,, +0.4053805,0x00,, +0.405475875,0x00,, +0.405571375,0x00,, +0.405666875,0x00,, +0.40576225,0x00,, +0.40585775,0x00,, +0.40595325,0x00,, +0.40604875,0x00,, +0.406144125,0x00,, +0.406239625,0x0E,, +0.421644625,0x55,, +0.421740125,0x55,, +0.4218355,0x18,, +0.421931,0x00,, +0.4220265,0xE8,, +0.422121875,0xB8,, +0.422217375,0xDB,, +0.422312875,0x00,, +0.422408375,0x2B,, +0.422503875,0xA8,, +0.42259925,0x00,, +0.42269475,0x80,, +0.42279025,0x08,, +0.422885625,0x00,, +0.422981125,0xD5,, +0.423076625,0x42,, +0.423172125,0xAB,, +0.4232675,0x80,, +0.423363,0x0D,, +0.4234585,0x54,, +0.423554,0x80,, +0.423649375,0x08,, +0.423744875,0x00,, +0.423840375,0x2A,, +0.423935875,0x62,, +0.42403125,0xA6,, +0.42412675,0xED,, +0.441744875,0x55,, +0.44184025,0x55,, +0.44193575,0x18,, +0.44203125,0x00,, +0.44212675,0xE9,, +0.442222125,0x7E,, +0.442317625,0xDB,, +0.442413125,0x00,, +0.442508625,0x2B,, +0.442604,0xC8,, +0.4426995,0x00,, +0.442795,0x80,, +0.442890375,0x08,, +0.442985875,0x00,, +0.443081375,0xD5,, +0.443176875,0x42,, +0.44327225,0xAB,, +0.44336775,0x80,, +0.44346325,0x0D,, +0.44355875,0x54,, +0.443654125,0x80,, +0.443749625,0x08,, +0.443845125,0x00,, +0.443940625,0x2A,, +0.444036,0x62,, +0.4441315,0xA6,, +0.444227,0xD9,, +0.461836375,0x55,, +0.461931875,0x55,, +0.462027375,0x18,, +0.46212275,0x00,, +0.46221825,0xEA,, +0.462313625,0x44,, +0.462409125,0xDB,, +0.462504625,0x00,, +0.462600125,0x2B,, +0.462695625,0xA8,, +0.462791,0x00,, +0.4628865,0x80,, +0.462982,0x08,, +0.4630775,0x00,, +0.463172875,0xD5,, +0.463268375,0x42,, +0.463363875,0xAB,, +0.46345925,0x80,, +0.46355475,0x0D,, +0.46365025,0x54,, +0.46374575,0x80,, +0.463841125,0x08,, +0.463936625,0x00,, +0.464032125,0x2A,, +0.464127625,0x62,, +0.464223,0xA6,, +0.4643185,0x70,, +0.481936625,0x55,, +0.482032,0x55,, +0.4821275,0x18,, +0.482223,0x00,, +0.4823185,0xEB,, +0.482413875,0x0A,, +0.482509375,0xDC,, +0.482604875,0x00,, +0.482700375,0x2B,, +0.48279575,0xA8,, +0.48289125,0x00,, +0.48298675,0x80,, +0.483082125,0x08,, +0.483177625,0x00,, +0.483273125,0xD5,, +0.483368625,0x42,, +0.483464125,0xAB,, +0.4835595,0x80,, +0.483655,0x0D,, +0.4837505,0x54,, +0.483846,0x80,, +0.483941375,0x08,, +0.484036875,0x00,, +0.484132375,0x2A,, +0.48422775,0x62,, +0.48432325,0xA6,, +0.48441875,0xD5,, +0.50203675,0x55,, +0.50213225,0x55,, +0.50222775,0x18,, +0.50232325,0x00,, +0.502418625,0xEB,, +0.502514125,0xCF,, +0.502609625,0xDB,, +0.502705125,0x00,, +0.5028005,0x2B,, +0.502896,0xB8,, +0.5029915,0x00,, +0.503087,0x80,, +0.503182375,0x08,, +0.503277875,0x00,, +0.503373375,0xD5,, +0.503468875,0x42,, +0.50356425,0xAB,, +0.50365975,0x80,, +0.50375525,0x0D,, +0.503850625,0x54,, +0.503946125,0x80,, +0.504041625,0x08,, +0.504137125,0x00,, +0.5042325,0x2A,, +0.504328,0x62,, +0.5044235,0xA6,, +0.504519,0x92,, +0.522137,0x55,, +0.5222325,0x55,, +0.522328,0x18,, +0.522423375,0x00,, +0.522518875,0xEC,, +0.522614375,0x95,, +0.522709875,0xDC,, +0.52280525,0x00,, +0.52290075,0x2B,, +0.52299625,0xB8,, +0.52309175,0x00,, +0.523187125,0x80,, +0.523282625,0x08,, +0.523378125,0x00,, +0.523473625,0xD5,, +0.523569,0x42,, +0.5236645,0xAB,, +0.52376,0x80,, +0.523855375,0x0D,, +0.523950875,0x54,, +0.524046375,0x80,, +0.524141875,0x08,, +0.524237375,0x00,, +0.52433275,0x2A,, +0.52442825,0x62,, +0.52452375,0xA6,, +0.524619125,0xA1,, +0.54223725,0x55,, +0.54233275,0x55,, +0.542428125,0x18,, +0.542523625,0x00,, +0.542619125,0xED,, +0.542714625,0x5C,, +0.54281,0xD9,, +0.5429055,0x00,, +0.543001,0x2B,, +0.5430965,0x98,, +0.543191875,0x00,, +0.543287375,0x80,, +0.543382875,0x08,, +0.54347825,0x00,, +0.54357375,0xD5,, +0.54366925,0x42,, +0.54376475,0xAB,, +0.54386025,0x80,, +0.543955625,0x0D,, +0.544051125,0x54,, +0.544146625,0x80,, +0.544242,0x08,, +0.5443375,0x00,, +0.544433,0x2A,, +0.5445285,0x62,, +0.544623875,0xA6,, +0.544719375,0xE0,, +0.5623375,0x55,, +0.562432875,0x55,, +0.562528375,0x18,, +0.562623875,0x00,, +0.562719375,0xEE,, +0.562814875,0x23,, +0.56291025,0xDC,, +0.56300575,0x00,, +0.56310125,0x2B,, +0.563196625,0xA8,, +0.563292125,0x00,, +0.563387625,0x80,, +0.563483125,0x08,, +0.5635785,0x00,, +0.563674,0xD5,, +0.5637695,0x42,, +0.563865,0xAB,, +0.563960375,0x80,, +0.564055875,0x0D,, +0.564151375,0x54,, +0.56424675,0x80,, +0.56434225,0x08,, +0.56443775,0x00,, +0.56453325,0x2A,, +0.564628625,0x62,, +0.564724125,0xA6,, +0.564819625,0xF6,, +0.582429,0x55,, +0.5825245,0x55,, +0.582619875,0x18,, +0.582715375,0x00,, +0.582810875,0xEE,, +0.582906375,0xEA,, +0.58300175,0xDA,, +0.58309725,0x00,, +0.58319275,0x2B,, +0.58328825,0xB8,, +0.58338375,0x00,, +0.583479125,0x80,, +0.583574625,0x08,, +0.583670125,0x00,, +0.5837655,0xD5,, +0.583861,0x42,, +0.5839565,0xAB,, +0.584052,0x80,, +0.584147375,0x0D,, +0.584242875,0x54,, +0.584338375,0x80,, +0.584433875,0x08,, +0.58452925,0x00,, +0.58462475,0x2A,, +0.58472025,0x62,, +0.584815625,0xA6,, +0.584911125,0xB6,, +0.602928375,0x55,, +0.603023875,0x55,, +0.603119375,0x2B,, +0.603214875,0x03,, +0.603310375,0xEF,, +0.60340575,0xB0,, +0.60350125,0xDB,, +0.60359675,0x00,, +0.60369225,0x2B,, +0.603787625,0xC8,, +0.603883125,0x00,, +0.603978625,0x80,, +0.604074,0x08,, +0.6041695,0x00,, +0.604265,0xD5,, +0.6043605,0x42,, +0.604455875,0xAB,, +0.604551375,0x80,, +0.604646875,0x0D,, +0.604742375,0x54,, +0.60483775,0x80,, +0.60493325,0x08,, +0.60502875,0x00,, +0.60512425,0x2A,, +0.605219625,0x62,, +0.605315125,0xA6,, +0.605410625,0x00,, +0.605506125,0x00,, +0.6056015,0x00,, +0.605697,0x00,, +0.6057925,0x00,, +0.605887875,0x00,, +0.605983375,0x00,, +0.606078875,0x00,, +0.606174375,0x00,, +0.60626975,0x00,, +0.60636525,0x00,, +0.60646075,0x00,, +0.60655625,0x00,, +0.60665175,0x00,, +0.606747125,0x00,, +0.606842625,0x00,, +0.606938125,0x00,, +0.6070335,0x00,, +0.607129,0x00,, +0.6072245,0x36,, +0.62262075,0x55,, +0.62271625,0x55,, +0.62281175,0x18,, +0.622907125,0x00,, +0.623002625,0xF0,, +0.623098125,0x77,, +0.623193625,0xDD,, +0.623289,0x00,, +0.6233845,0x2B,, +0.62348,0xC8,, +0.6235755,0x00,, +0.623670875,0x80,, +0.623766375,0x08,, +0.623861875,0x00,, +0.62395725,0xD5,, +0.62405275,0x42,, +0.62414825,0xAB,, +0.62424375,0x80,, +0.624339125,0x0D,, +0.624434625,0x54,, +0.624530125,0x80,, +0.624625625,0x08,, +0.624721,0x00,, +0.6248165,0x2A,, +0.624912,0x62,, +0.6250075,0xA6,, +0.625102875,0x30,, +0.642721,0x55,, +0.6428165,0x55,, +0.642911875,0x18,, +0.643007375,0x00,, +0.643102875,0xF1,, +0.643198375,0x3D,, +0.64329375,0xDC,, +0.64338925,0x00,, +0.64348475,0x2B,, +0.643580125,0xC8,, +0.643675625,0x00,, +0.643771125,0x80,, +0.643866625,0x08,, +0.643962,0x00,, +0.6440575,0xD5,, +0.644153,0x42,, +0.6442485,0xAB,, +0.644344,0x80,, +0.644439375,0x0D,, +0.644534875,0x54,, +0.644630375,0x80,, +0.64472575,0x08,, +0.64482125,0x00,, +0.64491675,0x2A,, +0.64501225,0x62,, +0.645107625,0xA6,, +0.645203125,0x64,, +0.66282125,0x55,, +0.662916625,0x55,, +0.663012125,0x18,, +0.663107625,0x00,, +0.663203125,0xF2,, +0.6632985,0x03,, +0.663394,0xDD,, +0.6634895,0x00,, +0.663584875,0x2B,, +0.663680375,0xB8,, +0.663775875,0x00,, +0.663871375,0x80,, +0.663966875,0x08,, +0.66406225,0x00,, +0.66415775,0xD5,, +0.66425325,0x42,, +0.664348625,0xAB,, +0.664444125,0x80,, +0.664539625,0x0D,, +0.664635125,0x54,, +0.6647305,0x80,, +0.664826,0x08,, +0.6649215,0x00,, +0.665017,0x2A,, +0.665112375,0x62,, +0.665207875,0xA6,, +0.665303375,0x18,, +0.6829215,0x55,, +0.683016875,0x55,, +0.683112375,0x18,, +0.683207875,0x00,, +0.68330325,0xF2,, +0.68339875,0xC9,, +0.68349425,0xDB,, +0.68358975,0x00,, +0.683685125,0x2B,, +0.683780625,0xC8,, +0.683876125,0x00,, +0.683971625,0x80,, +0.684067,0x08,, +0.6841625,0x00,, +0.684258,0xD5,, +0.684353375,0x42,, +0.684448875,0xAB,, +0.684544375,0x80,, +0.684639875,0x0D,, +0.68473525,0x54,, +0.68483075,0x80,, +0.68492625,0x08,, +0.68502175,0x00,, +0.685117125,0x2A,, +0.685212625,0x62,, +0.685308125,0xA6,, +0.685403625,0x5D,, +0.703013,0x55,, +0.703108375,0x55,, +0.703203875,0x18,, +0.703299375,0x00,, +0.703394875,0xF3,, +0.70349025,0x8F,, +0.70358575,0xDB,, +0.70368125,0x00,, +0.70377675,0x2B,, +0.703872125,0xC8,, +0.703967625,0x00,, +0.704063125,0x80,, +0.704158625,0x08,, +0.704254,0x00,, +0.7043495,0xD5,, +0.704445,0x42,, +0.7045405,0xAB,, +0.704635875,0x80,, +0.704731375,0x0D,, +0.704826875,0x54,, +0.70492225,0x80,, +0.70501775,0x08,, +0.70511325,0x00,, +0.70520875,0x2A,, +0.70530425,0x62,, +0.705399625,0xA6,, +0.705495125,0x0E,, +0.72311325,0x55,, +0.723208625,0x55,, +0.723304125,0x18,, +0.723399625,0x00,, +0.723495,0xF4,, +0.7235905,0x55,, +0.723686,0xDC,, +0.7237815,0x00,, +0.723876875,0x2B,, +0.723972375,0xA8,, +0.724067875,0x00,, +0.724163375,0x80,, +0.72425875,0x08,, +0.72435425,0x00,, +0.72444975,0xD5,, +0.724545125,0x42,, +0.724640625,0xAB,, +0.724736125,0x80,, +0.724831625,0x0D,, +0.724927125,0x54,, +0.7250225,0x80,, +0.725118,0x08,, +0.7252135,0x00,, +0.725308875,0x2A,, +0.725404375,0x62,, +0.725499875,0xA6,, +0.725595375,0x5A,, +0.743213375,0x55,, +0.743308875,0x55,, +0.743404375,0x18,, +0.74349975,0x00,, +0.74359525,0xF5,, +0.74369075,0x1C,, +0.74378625,0xDA,, +0.74388175,0x00,, +0.743977125,0x2B,, +0.744072625,0xA8,, +0.744168125,0x00,, +0.7442635,0x80,, +0.744359,0x08,, +0.7444545,0x00,, +0.74455,0xD5,, +0.744645375,0x42,, +0.744740875,0xAB,, +0.744836375,0x80,, +0.744931875,0x0D,, +0.74502725,0x54,, +0.74512275,0x80,, +0.74521825,0x08,, +0.745313625,0x00,, +0.745409125,0x2A,, +0.745504625,0x62,, +0.745600125,0xA6,, +0.7456955,0x81,, +0.763626125,0x55,, +0.7637215,0x55,, +0.763817,0x18,, +0.7639125,0x00,, +0.764007875,0xF5,, +0.764103375,0xE6,, +0.764198875,0xDB,, +0.764294375,0x00,, +0.76438975,0x2B,, +0.76448525,0xC8,, +0.76458075,0x00,, +0.76467625,0x80,, +0.764771625,0x08,, +0.764867125,0x00,, +0.764962625,0xD5,, +0.765058125,0x42,, +0.7651535,0xAB,, +0.765249,0x80,, +0.7653445,0x0D,, +0.76544,0x54,, +0.765535375,0x80,, +0.765630875,0x08,, +0.765726375,0x00,, +0.76582175,0x2A,, +0.76591725,0x62,, +0.76601275,0xA6,, +0.76610825,0x4C,, +0.783743625,0x55,, +0.783839125,0x55,, +0.783934625,0x18,, +0.78403,0x00,, +0.7841255,0xF6,, +0.784221,0xAD,, +0.7843165,0xDC,, +0.784411875,0x00,, +0.784507375,0x2B,, +0.784602875,0xC8,, +0.784698375,0x00,, +0.78479375,0x80,, +0.78488925,0x08,, +0.78498475,0x00,, +0.785080125,0xD5,, +0.785175625,0x42,, +0.785271125,0xAB,, +0.785366625,0x80,, +0.785462,0x0D,, +0.7855575,0x54,, +0.785653,0x80,, +0.7857485,0x08,, +0.785844,0x00,, +0.785939375,0x2A,, +0.786034875,0x62,, +0.78613025,0xA6,, +0.78622575,0x90,, +0.80407825,0x55,, +0.804173625,0x55,, +0.804269125,0x2B,, +0.804364625,0x03,, +0.80446,0xF7,, +0.8045555,0x70,, +0.804651,0xC9,, +0.8047465,0x00,, +0.804842,0x2B,, +0.804937375,0xC8,, +0.805032875,0x00,, +0.805128375,0x80,, +0.80522375,0x08,, +0.80531925,0x00,, +0.80541475,0xD5,, +0.80551025,0x42,, +0.805605625,0xAB,, +0.805701125,0x80,, +0.805796625,0x0D,, +0.805892125,0x54,, +0.8059875,0x80,, +0.806083,0x08,, +0.8061785,0x00,, +0.806273875,0x2A,, +0.806369375,0x62,, +0.806464875,0xA6,, +0.806560375,0x00,, +0.80665575,0x00,, +0.80675125,0x00,, +0.80684675,0x00,, +0.80694225,0x00,, +0.80703775,0x00,, +0.807133125,0x00,, +0.807228625,0x00,, +0.807324125,0x00,, +0.8074195,0x00,, +0.807515,0x00,, +0.8076105,0x00,, +0.807706,0x00,, +0.807801375,0x00,, +0.807896875,0x00,, +0.807992375,0x00,, +0.808087875,0x00,, +0.80818325,0x00,, +0.80827875,0x00,, +0.80837425,0x05,, +0.82414375,0x55,, +0.824239125,0x55,, +0.824334625,0x18,, +0.824430125,0x00,, +0.824525625,0xF8,, +0.824621,0x3C,, +0.8247165,0xDD,, +0.824812,0x00,, +0.824907375,0x2B,, +0.825002875,0xB8,, +0.825098375,0x00,, +0.825193875,0x80,, +0.82528925,0x08,, +0.82538475,0x00,, +0.82548025,0xD5,, +0.82557575,0x42,, +0.825671125,0xAB,, +0.825766625,0x80,, +0.825862125,0x0D,, +0.8259575,0x54,, +0.826053,0x80,, +0.8261485,0x08,, +0.826244,0x00,, +0.8263395,0x2A,, +0.826434875,0x62,, +0.826530375,0xA6,, +0.826625875,0xEE,, +0.843705875,0x55,, +0.84380125,0x55,, +0.84389675,0x18,, +0.84399225,0x00,, +0.84408775,0xF8,, +0.844183125,0xFC,, +0.844278625,0xDB,, +0.844374125,0x00,, +0.844469625,0x2B,, +0.844565,0xC8,, +0.8446605,0x00,, +0.844756,0x80,, +0.844851375,0x08,, +0.844946875,0x00,, +0.845042375,0xD5,, +0.845137875,0x42,, +0.84523325,0xAB,, +0.84532875,0x80,, +0.84542425,0x0D,, +0.84551975,0x54,, +0.845615125,0x80,, +0.845710625,0x08,, +0.845806125,0x00,, +0.845901625,0x2A,, +0.845997,0x62,, +0.8460925,0xA6,, +0.846188,0x16,, +0.863797375,0x55,, +0.863892875,0x55,, +0.863988375,0x18,, +0.86408375,0x00,, +0.86417925,0xF9,, +0.86427475,0xC2,, +0.864370125,0xDC,, +0.864465625,0x00,, +0.864561125,0x2B,, +0.864656625,0xC8,, +0.864752,0x00,, +0.8648475,0x80,, +0.864943,0x08,, +0.8650385,0x00,, +0.865133875,0xD5,, +0.865229375,0x42,, +0.865324875,0xAB,, +0.86542025,0x80,, +0.86551575,0x0D,, +0.86561125,0x54,, +0.86570675,0x80,, +0.86580225,0x08,, +0.865897625,0x00,, +0.865993125,0x2A,, +0.866088625,0x62,, +0.866184,0xA6,, +0.8662795,0x88,, +0.883897625,0x55,, +0.883993,0x55,, +0.8840885,0x18,, +0.884184,0x00,, +0.8842795,0xFA,, +0.884374875,0x89,, +0.884470375,0xDD,, +0.884565875,0x00,, +0.884661375,0x2B,, +0.88475675,0xC8,, +0.88485225,0x00,, +0.88494775,0x80,, +0.885043125,0x08,, +0.885138625,0x00,, +0.885234125,0xD5,, +0.885329625,0x42,, +0.885425125,0xAB,, +0.8855205,0x80,, +0.885616,0x0D,, +0.8857115,0x54,, +0.885807,0x80,, +0.885902375,0x08,, +0.885997875,0x00,, +0.886093375,0x2A,, +0.88618875,0x62,, +0.88628425,0xA6,, +0.88637975,0xBC,, +0.90399775,0x55,, +0.90409325,0x55,, +0.90418875,0x18,, +0.90428425,0x00,, +0.904379625,0xFB,, +0.904475125,0x50,, +0.904570625,0xDC,, +0.904666125,0x00,, +0.9047615,0x2B,, +0.904857,0xC8,, +0.9049525,0x00,, +0.905048,0x80,, +0.905143375,0x08,, +0.905238875,0x00,, +0.905334375,0xD5,, +0.905429875,0x42,, +0.90552525,0xAB,, +0.90562075,0x80,, +0.90571625,0x0D,, +0.905811625,0x54,, +0.905907125,0x80,, +0.906002625,0x08,, +0.906098125,0x00,, +0.9061935,0x2A,, +0.906289,0x62,, +0.9063845,0xA6,, +0.90648,0xEE,, +0.924749,0x55,, +0.924844375,0x55,, +0.924939875,0x18,, +0.925035375,0x00,, +0.92513075,0xFC,, +0.92522625,0x1D,, +0.92532175,0xDB,, +0.92541725,0x00,, +0.925512625,0x2B,, +0.925608125,0xB8,, +0.925703625,0x00,, +0.925799125,0x80,, +0.9258945,0x08,, +0.92599,0x00,, +0.9260855,0xD5,, +0.926181,0x42,, +0.926276375,0xAB,, +0.926371875,0x80,, +0.926467375,0x0D,, +0.926562875,0x54,, +0.92665825,0x80,, +0.92675375,0x08,, +0.92684925,0x00,, +0.926944625,0x2A,, +0.927040125,0x62,, +0.927135625,0xA6,, +0.927231125,0x45,, +0.9441895,0x55,, +0.944285,0x55,, +0.9443805,0x18,, +0.944476,0x00,, +0.9445715,0xFC,, +0.944666875,0xDB,, +0.944762375,0xDD,, +0.944857875,0x00,, +0.94495325,0x2B,, +0.94504875,0x98,, +0.94514425,0x00,, +0.94523975,0x80,, +0.945335125,0x08,, +0.945430625,0x00,, +0.945526125,0xD5,, +0.945621625,0x42,, +0.945717,0xAB,, +0.9458125,0x80,, +0.945908,0x0D,, +0.946003375,0x54,, +0.946098875,0x80,, +0.946194375,0x08,, +0.946289875,0x00,, +0.946385375,0x2A,, +0.94648075,0x62,, +0.94657625,0xA6,, +0.94667175,0x42,, +0.9642985,0x55,, +0.964393875,0x55,, +0.964489375,0x18,, +0.964584875,0x00,, +0.964680375,0xFD,, +0.964775875,0xA2,, +0.96487125,0xDC,, +0.96496675,0x00,, +0.96506225,0x2B,, +0.965157625,0x98,, +0.965253125,0x00,, +0.965348625,0x80,, +0.965444125,0x08,, +0.9655395,0x00,, +0.965635,0xD5,, +0.9657305,0x42,, +0.965826,0xAB,, +0.965921375,0x80,, +0.966016875,0x0D,, +0.966112375,0x54,, +0.96620775,0x80,, +0.96630325,0x08,, +0.96639875,0x00,, +0.96649425,0x2A,, +0.966589625,0x62,, +0.966685125,0xA6,, +0.966780625,0xF1,, +0.98439,0x55,, +0.9844855,0x55,, +0.984580875,0x18,, +0.984676375,0x00,, +0.984771875,0xFE,, +0.984867375,0x68,, +0.98496275,0xDB,, +0.98505825,0x00,, +0.98515375,0x2B,, +0.98524925,0xA8,, +0.98534475,0x00,, +0.985440125,0x80,, +0.985535625,0x08,, +0.985631125,0x00,, +0.9857265,0xD5,, +0.985822,0x42,, +0.9859175,0xAB,, +0.986013,0x80,, +0.986108375,0x0D,, +0.986203875,0x54,, +0.986299375,0x80,, +0.986394875,0x08,, +0.98649025,0x00,, +0.98658575,0x2A,, +0.98668125,0x62,, +0.986776625,0xA6,, +0.986872125,0xE7,, +1.0048895,0x55,, +1.004984875,0x55,, +1.005080375,0x2B,, +1.005175875,0x03,, +1.005271375,0xFF,, +1.00536675,0x2F,, +1.00546225,0xDC,, +1.00555775,0x00,, +1.00565325,0x2B,, +1.005748625,0xC8,, +1.005844125,0x00,, +1.005939625,0x80,, +1.006035,0x08,, +1.0061305,0x00,, +1.006226,0xD5,, +1.0063215,0x42,, +1.006416875,0xAB,, +1.006512375,0x80,, +1.006607875,0x0D,, +1.006703375,0x54,, +1.00679875,0x80,, +1.00689425,0x08,, +1.00698975,0x00,, +1.00708525,0x2A,, +1.007180625,0x62,, +1.007276125,0xA6,, +1.007371625,0x00,, +1.007467125,0x00,, +1.0075625,0x00,, +1.007658,0x00,, +1.0077535,0x00,, +1.007848875,0x00,, +1.007944375,0x00,, +1.008039875,0x00,, +1.008135375,0x00,, +1.00823075,0x00,, +1.00832625,0x00,, +1.00842175,0x00,, +1.00851725,0x00,, +1.00861275,0x00,, +1.008708125,0x00,, +1.008803625,0x00,, +1.008899125,0x00,, +1.0089945,0x00,, +1.00909,0x00,, +1.0091855,0xC3,, +1.0245905,0x55,, +1.024685875,0x55,, +1.024781375,0x18,, +1.024876875,0x00,, +1.024972375,0xFF,, +1.02506775,0xF6,, +1.02516325,0xDC,, +1.02525875,0x00,, +1.025354125,0x2B,, +1.025449625,0xC8,, +1.025545125,0x00,, +1.025640625,0x80,, +1.025736125,0x08,, +1.0258315,0x00,, +1.025927,0xD5,, +1.0260225,0x42,, +1.026117875,0xAB,, +1.026213375,0x80,, +1.026308875,0x0D,, +1.026404375,0x54,, +1.02649975,0x80,, +1.02659525,0x08,, +1.02669075,0x00,, +1.02678625,0x2A,, +1.026881625,0x62,, +1.026977125,0xA6,, +1.027072625,0xD6,, +1.044690625,0x55,, +1.044786125,0x55,, +1.044881625,0x18,, +1.044977,0x00,, +1.0450725,0x00,, +1.045168,0xBC,, +1.0452635,0xDB,, +1.045359,0x00,, +1.045454375,0x2B,, +1.045549875,0xB8,, +1.045645375,0x00,, +1.04574075,0x80,, +1.04583625,0x08,, +1.04593175,0x00,, +1.04602725,0xD5,, +1.046122625,0x42,, +1.046218125,0xAB,, +1.046313625,0x80,, +1.046409125,0x0D,, +1.0465045,0x54,, +1.0466,0x80,, +1.0466955,0x08,, +1.046791,0x00,, +1.046886375,0x2A,, +1.046981875,0x62,, +1.047077375,0xA6,, +1.047172875,0xCF,, +1.06478225,0x55,, +1.064877625,0x55,, +1.064973125,0x18,, +1.065068625,0x00,, +1.065164125,0x01,, +1.0652595,0x81,, +1.065355,0xDC,, +1.0654505,0x00,, +1.065545875,0x2B,, +1.065641375,0xC8,, +1.065736875,0x00,, +1.065832375,0x80,, +1.065927875,0x08,, +1.06602325,0x00,, +1.06611875,0xD5,, +1.06621425,0x42,, +1.066309625,0xAB,, +1.066405125,0x80,, +1.066500625,0x0D,, +1.066596125,0x54,, +1.0666915,0x80,, +1.066787,0x08,, +1.0668825,0x00,, +1.066978,0x2A,, +1.067073375,0x62,, +1.067168875,0xA6,, +1.067264375,0xB4,, +1.08487375,0x55,, +1.08496925,0x55,, +1.085064625,0x18,, +1.085160125,0x00,, +1.085255625,0x02,, +1.085351125,0x47,, +1.0854465,0xDB,, +1.085542,0x00,, +1.0856375,0x2B,, +1.085733,0x98,, +1.085828375,0x00,, +1.085923875,0x80,, +1.086019375,0x08,, +1.086114875,0x00,, +1.08621025,0xD5,, +1.08630575,0x42,, +1.08640125,0xAB,, +1.08649675,0x80,, +1.086592125,0x0D,, +1.086687625,0x54,, +1.086783125,0x80,, +1.0868785,0x08,, +1.086974,0x00,, +1.0870695,0x2A,, +1.087165,0x62,, +1.087260375,0xA6,, +1.087355875,0xC7,, +1.104974,0x55,, +1.1050695,0x55,, +1.105164875,0x18,, +1.105260375,0x00,, +1.105355875,0x03,, +1.10545125,0x0E,, +1.10554675,0xDC,, +1.10564225,0x00,, +1.10573775,0x2B,, +1.105833125,0xB8,, +1.105928625,0x00,, +1.106024125,0x80,, +1.106119625,0x08,, +1.106215,0x00,, +1.1063105,0xD5,, +1.106406,0x42,, +1.1065015,0xAB,, +1.106596875,0x80,, +1.106692375,0x0D,, +1.106787875,0x54,, +1.10688325,0x80,, +1.10697875,0x08,, +1.10707425,0x00,, +1.10716975,0x2A,, +1.10726525,0x62,, +1.107360625,0xA6,, +1.107456125,0xF7,, +1.125082875,0x55,, +1.125178375,0x55,, +1.12527375,0x18,, +1.12536925,0x00,, +1.12546475,0x03,, +1.12556025,0xD4,, +1.125655625,0xDD,, +1.125751125,0x00,, +1.125846625,0x2B,, +1.125942125,0x98,, +1.1260375,0x00,, +1.126133,0x80,, +1.1262285,0x08,, +1.126324,0x00,, +1.126419375,0xD5,, +1.126514875,0x42,, +1.126610375,0xAB,, +1.12670575,0x80,, +1.12680125,0x0D,, +1.12689675,0x54,, +1.12699225,0x80,, +1.127087625,0x08,, +1.127183125,0x00,, +1.127278625,0x2A,, +1.127374125,0x62,, +1.1274695,0xA6,, +1.127565,0x7B,, +1.145174375,0x55,, +1.145269875,0x55,, +1.145365375,0x18,, +1.14546075,0x00,, +1.14555625,0x04,, +1.14565175,0x9B,, +1.14574725,0xDC,, +1.14584275,0x00,, +1.145938125,0x2B,, +1.146033625,0x98,, +1.146129125,0x00,, +1.1462245,0x80,, +1.14632,0x08,, +1.1464155,0x00,, +1.146511,0xD5,, +1.146606375,0x42,, +1.146701875,0xAB,, +1.146797375,0x80,, +1.146892875,0x0D,, +1.14698825,0x54,, +1.14708375,0x80,, +1.14717925,0x08,, +1.147274625,0x00,, +1.147370125,0x2A,, +1.147465625,0x62,, +1.147561125,0xA6,, +1.147656625,0x19,, +1.165274625,0x55,, +1.165370125,0x55,, +1.165465625,0x18,, +1.165561,0x00,, +1.1656565,0x05,, +1.165752,0x60,, +1.165847375,0xDB,, +1.165942875,0x00,, +1.166038375,0x2B,, +1.166133875,0xB8,, +1.16622925,0x00,, +1.16632475,0x80,, +1.16642025,0x08,, +1.16651575,0xC9,, +1.166611125,0xD5,, +1.166706625,0x42,, +1.166802125,0xAB,, +1.1668975,0x80,, +1.166993,0x0D,, +1.1670885,0x54,, +1.167184,0x80,, +1.1672795,0x08,, +1.167374875,0x00,, +1.167470375,0x2A,, +1.167565875,0x62,, +1.167661375,0xA6,, +1.16775675,0x01,, +1.185366125,0x55,, +1.185461625,0x55,, +1.185557125,0x18,, +1.185652625,0x00,, +1.185748,0x06,, +1.1858435,0x27,, +1.185939,0xDC,, +1.1860345,0x00,, +1.186129875,0x2B,, +1.186225375,0xA8,, +1.186320875,0x00,, +1.18641625,0x80,, +1.18651175,0x08,, +1.18660725,0x5A,, +1.18670275,0xD5,, +1.186798125,0x42,, +1.186893625,0xAB,, +1.186989125,0x80,, +1.187084625,0x0D,, +1.18718,0x54,, +1.1872755,0x80,, +1.187371,0x08,, +1.187466375,0x00,, +1.187561875,0x2A,, +1.187657375,0x62,, +1.187752875,0xA6,, +1.187848375,0x23,, +1.205865625,0x55,, +1.205961125,0x55,, +1.2060565,0x2B,, +1.206152,0x03,, +1.2062475,0x06,, +1.206343,0xEE,, +1.206438375,0xDD,, +1.206533875,0x00,, +1.206629375,0x2B,, +1.20672475,0xA8,, +1.20682025,0x00,, +1.20691575,0x80,, +1.20701125,0x08,, +1.20710675,0x00,, +1.207202125,0xD5,, +1.207297625,0x42,, +1.207393125,0xAB,, +1.207488625,0x80,, +1.207584,0x0D,, +1.2076795,0x54,, +1.207775,0x80,, +1.207870375,0x08,, +1.207965875,0x00,, +1.208061375,0x2A,, +1.208156875,0x62,, +1.20825225,0xA6,, +1.20834775,0x00,, +1.20844325,0x00,, +1.20853875,0x00,, +1.208634125,0x00,, +1.208729625,0x00,, +1.208825125,0x00,, +1.2089205,0x00,, +1.209016,0x00,, +1.2091115,0x00,, +1.209207,0x00,, +1.2093025,0x00,, +1.209397875,0x00,, +1.209493375,0x00,, +1.209588875,0x00,, +1.20968425,0x00,, +1.20977975,0x00,, +1.20987525,0x00,, +1.20997075,0x00,, +1.210066125,0x00,, +1.210161625,0x5A,, +1.225566625,0x55,, +1.225662125,0x55,, +1.2257575,0x18,, +1.225853,0x00,, +1.2259485,0x07,, +1.226043875,0xB5,, +1.226139375,0xDD,, +1.226234875,0x00,, +1.226330375,0x2B,, +1.226425875,0xC8,, +1.22652125,0x00,, +1.22661675,0x80,, +1.22671225,0x08,, +1.226807625,0x00,, +1.226903125,0xD5,, +1.226998625,0x42,, +1.227094125,0xAB,, +1.2271895,0x80,, +1.227285,0x0D,, +1.2273805,0x54,, +1.227476,0x80,, +1.227571375,0x08,, +1.227666875,0x00,, +1.227762375,0x2A,, +1.227857875,0x62,, +1.22795325,0xA6,, +1.22804875,0xC6,, +1.246491375,0x55,, +1.24658675,0x55,, +1.24668225,0x18,, +1.24677775,0x00,, +1.246873125,0x08,, +1.246968625,0x84,, +1.247064125,0xDB,, +1.247159625,0x00,, +1.247255125,0x2B,, +1.2473505,0xB8,, +1.247446,0x00,, +1.2475415,0x80,, +1.247636875,0x08,, +1.247732375,0x00,, +1.247827875,0xD5,, +1.247923375,0x42,, +1.24801875,0xAB,, +1.24811425,0x80,, +1.24820975,0x0D,, +1.24830525,0x54,, +1.248400625,0x80,, +1.248496125,0x08,, +1.248591625,0x00,, +1.248687,0x2A,, +1.2487825,0x62,, +1.248878,0xA6,, +1.2489735,0xCF,, +1.26635725,0x55,, +1.266452625,0x55,, +1.266548125,0x18,, +1.266643625,0x00,, +1.266739125,0x09,, +1.2668345,0x48,, +1.26693,0xDB,, +1.2670255,0x00,, +1.267120875,0x2B,, +1.267216375,0xB8,, +1.267311875,0x00,, +1.267407375,0x80,, +1.267502875,0x08,, +1.26759825,0x00,, +1.26769375,0xD5,, +1.26778925,0x42,, +1.26788475,0xAB,, +1.267980125,0x80,, +1.268075625,0x0D,, +1.268171125,0x54,, +1.2682665,0x80,, +1.268362,0x08,, +1.2684575,0x00,, +1.268553,0x2A,, +1.268648375,0x62,, +1.268743875,0xA6,, +1.268839375,0x08,, +1.2865355,0x55,, +1.286631,0x55,, +1.2867265,0x18,, +1.286822,0x00,, +1.286917375,0x0A,, +1.287012875,0x0F,, +1.287108375,0xDC,, +1.28720375,0x00,, +1.28729925,0x2B,, +1.28739475,0xB8,, +1.28749025,0x00,, +1.287585625,0x80,, +1.287681125,0x08,, +1.287776625,0x00,, +1.287872125,0xD5,, +1.287967625,0x42,, +1.288063,0xAB,, +1.2881585,0x80,, +1.288253875,0x0D,, +1.288349375,0x54,, +1.288444875,0x80,, +1.288540375,0x08,, +1.288635875,0x00,, +1.28873125,0x2A,, +1.28882675,0x62,, +1.28892225,0xA6,, +1.28901775,0xFF,, +1.30595875,0x55,, +1.30605425,0x55,, +1.30614975,0x18,, +1.30624525,0x00,, +1.306340625,0x0A,, +1.306436125,0xCF,, +1.306531625,0xDD,, +1.306627125,0x00,, +1.3067225,0x2C,, +1.306818,0x98,, +1.3069135,0x00,, +1.307009,0x80,, +1.307104375,0x08,, +1.307199875,0x00,, +1.307295375,0xD5,, +1.307390875,0x42,, +1.30748625,0xAB,, +1.30758175,0x80,, +1.30767725,0x0D,, +1.307772625,0x54,, +1.307868125,0x80,, +1.307963625,0x08,, +1.308059125,0x00,, +1.3081545,0x2A,, +1.30825,0x62,, +1.3083455,0xA6,, +1.308441,0x7C,, +1.326692625,0x55,, +1.326788,0x55,, +1.3268835,0x18,, +1.326979,0x00,, +1.3270745,0x0B,, +1.327169875,0x9C,, +1.327265375,0xDD,, +1.327360875,0x00,, +1.32745625,0x38,, +1.32755175,0x58,, +1.32764725,0x00,, +1.32774275,0x80,, +1.32783825,0x07,, +1.327933625,0xDE,, +1.328029125,0xD5,, +1.328124625,0x42,, +1.328220125,0xAB,, +1.3283155,0x80,, +1.328411,0x0D,, +1.3285065,0x54,, +1.328601875,0x80,, +1.328697375,0x08,, +1.328792875,0x00,, +1.328888375,0x2A,, +1.32898375,0x62,, +1.32907925,0xA6,, +1.32917475,0x3C,, +1.34720075,0x55,, +1.347296125,0x55,, +1.347391625,0x18,, +1.347487125,0x00,, +1.347582625,0x0C,, +1.347678,0x67,, +1.3477735,0xDD,, +1.347869,0x00,, +1.347964375,0x4D,, +1.348059875,0xC8,, +1.348155375,0x00,, +1.348250875,0x80,, +1.348346375,0x07,, +1.34844175,0xD8,, +1.34853725,0xD5,, +1.34863275,0x42,, +1.34872825,0xAB,, +1.348823625,0x80,, +1.348919125,0x0D,, +1.349014625,0x54,, +1.34911,0x80,, +1.3492055,0x08,, +1.349301,0x00,, +1.3493965,0x2A,, +1.349491875,0x62,, +1.349587375,0xA6,, +1.349682875,0xAB,, +1.36683225,0x55,, +1.36692775,0x55,, +1.36702325,0x18,, +1.367118625,0x00,, +1.367214125,0x0D,, +1.367309625,0x28,, +1.367405125,0xDB,, +1.3675005,0x00,, +1.367596,0x61,, +1.3676915,0xD8,, +1.367787,0x00,, +1.367882375,0x80,, +1.367977875,0x07,, +1.368073375,0xE4,, +1.36816875,0xD5,, +1.36826425,0x42,, +1.36835975,0xAB,, +1.36845525,0x80,, +1.36855075,0x0D,, +1.368646125,0x54,, +1.368741625,0x80,, +1.368837125,0x08,, +1.3689325,0x00,, +1.369028,0x2A,, +1.3691235,0x62,, +1.369219,0xA6,, +1.369314375,0xC1,, +1.3872015,0x55,, +1.387297,0x55,, +1.3873925,0x18,, +1.387487875,0x00,, +1.387583375,0x0D,, +1.387678875,0xF1,, +1.387774375,0xDC,, +1.387869875,0x00,, +1.38796525,0x79,, +1.38806075,0x88,, +1.38815625,0x00,, +1.388251625,0x80,, +1.388347125,0x08,, +1.388442625,0x00,, +1.388538125,0xD5,, +1.3886335,0x42,, +1.388729,0xAB,, +1.3888245,0x80,, +1.38892,0x0D,, +1.389015375,0x54,, +1.389110875,0x80,, +1.389206375,0x08,, +1.38930175,0x00,, +1.38939725,0x2A,, +1.38949275,0x62,, +1.38958825,0xA6,, +1.38968375,0x9B,, +1.40739725,0x55,, +1.40749275,0x55,, +1.407588125,0x2B,, +1.407683625,0x03,, +1.407779125,0x0E,, +1.4078745,0xB4,, +1.40797,0xDB,, +1.4080655,0x00,, +1.408161,0x8C,, +1.408256375,0x78,, +1.408351875,0x00,, +1.408447375,0x80,, +1.408542875,0x08,, +1.40863825,0x00,, +1.40873375,0xD5,, +1.40882925,0x42,, +1.40892475,0xAB,, +1.409020125,0x80,, +1.409115625,0x0D,, +1.409211125,0x54,, +1.409306625,0x80,, +1.409402,0x08,, +1.4094975,0x00,, +1.409593,0x2A,, +1.4096885,0x62,, +1.409783875,0xA6,, +1.409879375,0x00,, +1.409974875,0x00,, +1.41007025,0x00,, +1.41016575,0x00,, +1.41026125,0x00,, +1.41035675,0x00,, +1.410452125,0x00,, +1.410547625,0x00,, +1.410643125,0x00,, +1.410738625,0x00,, +1.410834,0x00,, +1.4109295,0x00,, +1.411025,0x00,, +1.411120375,0x00,, +1.411215875,0x00,, +1.411311375,0x00,, +1.411406875,0x00,, +1.411502375,0x00,, +1.41159775,0x00,, +1.41169325,0x61,, +1.42758425,0x55,, +1.42767975,0x55,, +1.427775125,0x18,, +1.427870625,0x00,, +1.427966125,0x0F,, +1.428061625,0x7F,, +1.428157,0xDC,, +1.4282525,0x00,, +1.428348,0xA1,, +1.428443375,0xE8,, +1.428538875,0x00,, +1.428634375,0x80,, +1.428729875,0x08,, +1.42882525,0x18,, +1.42892075,0xD5,, +1.42901625,0x42,, +1.42911175,0xAB,, +1.42920725,0x80,, +1.429302625,0x0D,, +1.429398125,0x54,, +1.429493625,0x80,, +1.429589,0x08,, +1.4296845,0x00,, +1.42978,0x2A,, +1.4298755,0x62,, +1.429970875,0xA6,, +1.430066375,0x1B,, +1.446816625,0x55,, +1.446912,0x55,, +1.4470075,0x18,, +1.447103,0x00,, +1.447198375,0x10,, +1.447293875,0x3C,, +1.447389375,0xC4,, +1.447484875,0x00,, +1.44758025,0xA1,, +1.44767575,0xE8,, +1.44777125,0x00,, +1.44786675,0x80,, +1.447962125,0x08,, +1.448057625,0x18,, +1.448153125,0xD5,, +1.448248625,0x42,, +1.448344,0xAB,, +1.4484395,0x80,, +1.448535,0x0D,, +1.4486305,0x54,, +1.448725875,0x80,, +1.448821375,0x08,, +1.448916875,0x00,, +1.44901225,0x2A,, +1.44910775,0x62,, +1.44920325,0xA6,, +1.44929875,0x72,, +1.466882125,0x55,, +1.4669775,0x55,, +1.467073,0x18,, +1.4671685,0x00,, +1.467264,0x11,, +1.467359375,0x01,, +1.467454875,0xCD,, +1.467550375,0x00,, +1.46764575,0xB7,, +1.46774125,0xF8,, +1.46783675,0x00,, +1.46793225,0x80,, +1.468027625,0x08,, +1.468123125,0x31,, +1.468218625,0xD5,, +1.468314125,0x42,, +1.4684095,0xAB,, +1.468505,0x80,, +1.4686005,0x0D,, +1.468695875,0x54,, +1.468791375,0x80,, +1.468886875,0x08,, +1.468982375,0x00,, +1.469077875,0x2A,, +1.46917325,0x62,, +1.46926875,0xA6,, +1.46936425,0x4C,, +1.486973625,0x55,, +1.487069125,0x55,, +1.4871645,0x18,, +1.48726,0x00,, +1.4873555,0x11,, +1.487451,0xC7,, +1.487546375,0xCD,, +1.487641875,0x00,, +1.487737375,0xC6,, +1.487832875,0xB8,, +1.48792825,0x00,, +1.48802375,0x80,, +1.48811925,0x08,, +1.488214625,0x36,, +1.488310125,0xD5,, +1.488405625,0x42,, +1.488501125,0xAB,, +1.4885965,0x80,, +1.488692,0x0D,, +1.4887875,0x54,, +1.488883,0x80,, +1.488978375,0x08,, +1.489073875,0x00,, +1.489169375,0x2A,, +1.489264875,0x62,, +1.48936025,0xA6,, +1.48945575,0x61,, +1.506943625,0x55,, +1.507039125,0x55,, +1.507134625,0x18,, +1.50723,0x00,, +1.5073255,0x12,, +1.507421,0x8D,, +1.5075165,0xCD,, +1.507611875,0x00,, +1.507707375,0xD4,, +1.507802875,0xF8,, +1.50789825,0x00,, +1.50799375,0x80,, +1.50808925,0x08,, +1.50818475,0x35,, +1.50828025,0xD5,, +1.508375625,0x42,, +1.508471125,0xAB,, +1.508566625,0x80,, +1.508662125,0x0D,, +1.5087575,0x54,, +1.508853,0x80,, +1.5089485,0x08,, +1.509043875,0x00,, +1.509139375,0x2A,, +1.509234875,0x62,, +1.509330375,0xA6,, +1.50942575,0x75,, +1.52703525,0x55,, +1.527130625,0x55,, +1.527226125,0x18,, +1.527321625,0x00,, +1.527417,0x13,, +1.5275125,0x54,, +1.527608,0xCD,, +1.5277035,0x00,, +1.527798875,0xD5,, +1.527894375,0x18,, +1.527989875,0x00,, +1.528085375,0x80,, +1.52818075,0x08,, +1.52827625,0x00,, +1.52837175,0xD5,, +1.52846725,0x42,, +1.528562625,0xAB,, +1.528658125,0x80,, +1.528753625,0x0D,, +1.528849125,0x54,, +1.5289445,0x80,, +1.52904,0x08,, +1.5291355,0x00,, +1.529230875,0x2A,, +1.529326375,0x62,, +1.529421875,0xA6,, +1.529517375,0x31,, +1.547135375,0x55,, +1.547230875,0x55,, +1.547326375,0x18,, +1.54742175,0x00,, +1.54751725,0x14,, +1.54761275,0x1B,, +1.54770825,0xC7,, +1.54780375,0x00,, +1.547899125,0xD5,, +1.547994625,0x08,, +1.548090125,0x00,, +1.5481855,0x80,, +1.548281,0x07,, +1.5483765,0xFA,, +1.548472,0xD5,, +1.548567375,0x42,, +1.548662875,0xAB,, +1.548758375,0x80,, +1.548853875,0x0D,, +1.54894925,0x54,, +1.54904475,0x80,, +1.54914025,0x08,, +1.549235625,0x00,, +1.549331125,0x2A,, +1.549426625,0x62,, +1.549522125,0xA6,, +1.549617625,0x30,, +1.56724425,0x55,, +1.56733975,0x55,, +1.56743525,0x18,, +1.56753075,0x00,, +1.567626125,0x14,, +1.567721625,0xE1,, +1.567817125,0xBB,, +1.567912625,0x00,, +1.568008,0xD5,, +1.5681035,0x18,, +1.568199,0x00,, +1.5682945,0x80,, +1.568389875,0x07,, +1.568485375,0xDA,, +1.568580875,0xD5,, +1.568676375,0x42,, +1.56877175,0xAB,, +1.56886725,0x80,, +1.56896275,0x0D,, +1.569058125,0x54,, +1.569153625,0x80,, +1.569249125,0x08,, +1.569344625,0x00,, +1.56944,0x2A,, +1.5695355,0x62,, +1.569631,0xA6,, +1.5697265,0x32,, +1.587327125,0x55,, +1.587422625,0x55,, +1.587518125,0x18,, +1.587613625,0x00,, +1.587709,0x15,, +1.5878045,0xA7,, +1.5879,0xCD,, +1.5879955,0x00,, +1.588090875,0xD5,, +1.588186375,0x18,, +1.588281875,0x00,, +1.58837725,0x80,, +1.58847275,0x07,, +1.58856825,0xDB,, +1.58866375,0xD5,, +1.588759125,0x42,, +1.588854625,0xAB,, +1.588950125,0x80,, +1.589045625,0x0D,, +1.589141,0x54,, +1.5892365,0x80,, +1.589332,0x08,, +1.589427375,0x00,, +1.589522875,0x2A,, +1.589618375,0x62,, +1.589713875,0xA6,, +1.589809375,0x79,, +1.607922125,0x55,, +1.6080175,0x55,, +1.608113,0x2B,, +1.6082085,0x03,, +1.608304,0x16,, +1.608399375,0x6F,, +1.608494875,0xCF,, +1.608590375,0x00,, +1.60868575,0xCB,, +1.60878125,0xE8,, +1.60887675,0x00,, +1.60897225,0x80,, +1.60906775,0x07,, +1.609163125,0x90,, +1.609258625,0xD5,, +1.609354125,0x42,, +1.609449625,0xAB,, +1.609545,0x80,, +1.6096405,0x0D,, +1.609736,0x54,, +1.609831375,0x80,, +1.609926875,0x08,, +1.610022375,0x00,, +1.610117875,0x2A,, +1.61021325,0x62,, +1.61030875,0xA6,, +1.61040425,0x00,, +1.61049975,0x00,, +1.610595125,0x00,, +1.610690625,0x00,, +1.610786125,0x00,, +1.610881625,0x00,, +1.610977,0x00,, +1.6110725,0x00,, +1.611168,0x00,, +1.6112635,0x00,, +1.611358875,0x00,, +1.611454375,0x00,, +1.611549875,0x00,, +1.61164525,0x00,, +1.61174075,0x00,, +1.61183625,0x00,, +1.61193175,0x00,, +1.612027125,0x00,, +1.612122625,0x00,, +1.612218125,0x25,, +1.62753625,0x55,, +1.62763175,0x55,, +1.62772725,0x18,, +1.62782275,0x00,, +1.627918125,0x17,, +1.628013625,0x33,, +1.628109125,0xCF,, +1.6282045,0x00,, +1.6283,0xBC,, +1.6283955,0x08,, +1.628491,0x00,, +1.628586375,0x80,, +1.628681875,0x07,, +1.628777375,0x1E,, +1.628872875,0xD5,, +1.62896825,0x42,, +1.62906375,0xAB,, +1.62915925,0x80,, +1.629254625,0x0D,, +1.629350125,0x54,, +1.629445625,0x80,, +1.629541125,0x08,, +1.629636625,0x00,, +1.629732,0x2A,, +1.6298275,0x62,, +1.629923,0xA6,, +1.6300185,0x61,, +1.647888125,0x55,, +1.647983625,0x55,, +1.648079125,0x18,, +1.648174625,0x00,, +1.648270125,0x17,, +1.6483655,0xFD,, +1.648461,0xD0,, +1.6485565,0x00,, +1.648652,0xAA,, +1.648747375,0x78,, +1.648842875,0x00,, +1.648938375,0x80,, +1.64903375,0x06,, +1.64912925,0xCB,, +1.64922475,0xD5,, +1.64932025,0x42,, +1.649415625,0xAB,, +1.649511125,0x80,, +1.649606625,0x0D,, +1.649702125,0x54,, +1.6497975,0x80,, +1.649893,0x08,, +1.6499885,0x00,, +1.650083875,0x2A,, +1.650179375,0x62,, +1.650274875,0xA6,, +1.650370375,0xB0,, +1.66868275,0x55,, +1.668778125,0x55,, +1.668873625,0x18,, +1.668969125,0x00,, +1.669064625,0x18,, +1.669160125,0xCA,, +1.6692555,0xD1,, +1.669351,0x00,, +1.6694465,0x99,, +1.669541875,0x88,, +1.669637375,0x00,, +1.669732875,0x80,, +1.669828375,0x06,, +1.66992375,0x92,, +1.67001925,0xD5,, +1.67011475,0x42,, +1.67021025,0xAB,, +1.670305625,0x80,, +1.670401125,0x0D,, +1.670496625,0x54,, +1.670592,0x80,, +1.6706875,0x08,, +1.670783,0x00,, +1.6708785,0x2A,, +1.670974,0x62,, +1.671069375,0xA6,, +1.671164875,0xD9,, +1.68782825,0x55,, +1.68792375,0x55,, +1.68801925,0x18,, +1.688114625,0x00,, +1.688210125,0x19,, +1.688305625,0x87,, +1.688401125,0xD0,, +1.6884965,0x00,, +1.688592,0x8C,, +1.6886875,0x08,, +1.688783,0x00,, +1.688878375,0x80,, +1.688973875,0x06,, +1.689069375,0x65,, +1.68916475,0xD5,, +1.68926025,0x42,, +1.68935575,0xAB,, +1.68945125,0x80,, +1.689546625,0x0D,, +1.689642125,0x54,, +1.689737625,0x80,, +1.689833125,0x08,, +1.689928625,0x00,, +1.690024,0x2A,, +1.6901195,0x62,, +1.690214875,0xA6,, +1.690310375,0x88,, +1.7079545,0x55,, +1.70805,0x55,, +1.7081455,0x18,, +1.708240875,0x00,, +1.708336375,0x1A,, +1.708431875,0x4D,, +1.708527375,0xDA,, +1.70862275,0x00,, +1.70871825,0x7C,, +1.70881375,0x98,, +1.708909125,0x00,, +1.709004625,0x80,, +1.709100125,0x06,, +1.709195625,0x5A,, +1.709291125,0xD5,, +1.7093865,0x42,, +1.709482,0xAB,, +1.7095775,0x80,, +1.709672875,0x0D,, +1.709768375,0x54,, +1.709863875,0x80,, +1.709959375,0x08,, +1.71005475,0x00,, +1.71015025,0x2A,, +1.71024575,0x62,, +1.71034125,0xA6,, +1.710436625,0x47,, +1.729044125,0x55,, +1.729139625,0x55,, +1.729235125,0x18,, +1.7293305,0x00,, +1.729426,0x1B,, +1.7295215,0x1E,, +1.729616875,0xBC,, +1.729712375,0x00,, +1.729807875,0x6C,, +1.729903375,0xB8,, +1.72999875,0x00,, +1.73009425,0x80,, +1.73018975,0x06,, +1.73028525,0x78,, +1.73038075,0xD5,, +1.730476125,0x42,, +1.730571625,0xAB,, +1.730667125,0x80,, +1.7307625,0x0D,, +1.730858,0x54,, +1.7309535,0x80,, +1.731049,0x08,, +1.731144375,0x00,, +1.731239875,0x2A,, +1.731335375,0x62,, +1.731430875,0xA6,, +1.73152625,0x57,, +1.748771125,0x55,, +1.748866625,0x55,, +1.748962125,0x18,, +1.749057625,0x00,, +1.749153,0x1B,, +1.7492485,0xE1,, +1.749344,0xC2,, +1.7494395,0x00,, +1.749534875,0x5C,, +1.749630375,0xC8,, +1.749725875,0x00,, +1.74982125,0x80,, +1.74991675,0x06,, +1.75001225,0xA5,, +1.75010775,0xD5,, +1.750203125,0x42,, +1.750298625,0xAB,, +1.750394125,0x80,, +1.750489625,0x0D,, +1.750585,0x54,, +1.7506805,0x80,, +1.750776,0x08,, +1.750871375,0x00,, +1.750966875,0x2A,, +1.751062375,0x62,, +1.751157875,0xA6,, +1.751253375,0x96,, +1.768836625,0x55,, +1.768932125,0x55,, +1.769027625,0x18,, +1.769123125,0x00,, +1.7692185,0x1C,, +1.769314,0xA5,, +1.7694095,0xD0,, +1.769504875,0x00,, +1.769600375,0x4C,, +1.769695875,0xA8,, +1.769791375,0x00,, +1.769886875,0x80,, +1.76998225,0x06,, +1.77007775,0xCA,, +1.77017325,0xD5,, +1.770268625,0x42,, +1.770364125,0xAB,, +1.770459625,0x80,, +1.770555125,0x0D,, +1.7706505,0x54,, +1.770746,0x80,, +1.7708415,0x08,, +1.770937,0x00,, +1.771032375,0x2A,, +1.771127875,0x62,, +1.771223375,0xA6,, +1.77131875,0x8B,, +1.788312,0x55,, +1.7884075,0x55,, +1.788502875,0x18,, +1.788598375,0x00,, +1.788693875,0x1D,, +1.788789375,0x65,, +1.788884875,0xD0,, +1.78898025,0x00,, +1.78907575,0x3A,, +1.78917125,0x38,, +1.78926675,0x00,, +1.789362125,0x80,, +1.789457625,0x06,, +1.789553125,0xFB,, +1.7896485,0xD5,, +1.789744,0x42,, +1.7898395,0xAB,, +1.789935,0x80,, +1.790030375,0x0D,, +1.790125875,0x54,, +1.790221375,0x80,, +1.790316875,0x08,, +1.79041225,0x00,, +1.79050775,0x2A,, +1.79060325,0x62,, +1.790698625,0xA6,, +1.790794125,0x38,, +1.8088115,0x55,, +1.808906875,0x55,, +1.809002375,0x2B,, +1.809097875,0x03,, +1.809193375,0x1E,, +1.80928875,0x2A,, +1.80938425,0xD0,, +1.80947975,0x00,, +1.80957525,0x2B,, +1.809670625,0x48,, +1.809766125,0x00,, +1.809861625,0x80,, +1.809957,0x07,, +1.8100525,0x52,, +1.810148,0xD5,, +1.8102435,0x42,, +1.810339,0xAB,, +1.810434375,0x80,, +1.810529875,0x0D,, +1.810625375,0x54,, +1.81072075,0x80,, +1.81081625,0x08,, +1.81091175,0x00,, +1.81100725,0x2A,, +1.811102625,0x62,, +1.811198125,0xA6,, +1.811293625,0x00,, +1.811389125,0x00,, +1.8114845,0x00,, +1.81158,0x00,, +1.8116755,0x00,, +1.811770875,0x00,, +1.811866375,0x00,, +1.811961875,0x00,, +1.812057375,0x00,, +1.81215275,0x00,, +1.81224825,0x00,, +1.81234375,0x00,, +1.81243925,0x00,, +1.81253475,0x00,, +1.812630125,0x00,, +1.812725625,0x00,, +1.812821125,0x00,, +1.8129165,0x00,, +1.813012,0x00,, +1.8131075,0x9A,, +1.8285125,0x55,, +1.828607875,0x55,, +1.828703375,0x18,, +1.828798875,0x00,, +1.828894375,0x1E,, +1.82898975,0xF1,, +1.82908525,0xD1,, +1.82918075,0x00,, +1.829276125,0x2B,, +1.829371625,0xD8,, +1.829467125,0x00,, +1.829562625,0x80,, +1.829658125,0x07,, +1.8297535,0xA6,, +1.829849,0xD5,, +1.8299445,0x42,, +1.830039875,0xAB,, +1.830135375,0x80,, +1.830230875,0x0D,, +1.830326375,0x54,, +1.83042175,0x80,, +1.83051725,0x08,, +1.83061275,0x00,, +1.83070825,0x2A,, +1.830803625,0x62,, +1.830899125,0xA6,, +1.830994625,0x7E,, +1.848604,0x55,, +1.8486995,0x55,, +1.848794875,0x18,, +1.848890375,0x00,, +1.848985875,0x1F,, +1.849081375,0xB8,, +1.84917675,0xCB,, +1.84927225,0x00,, +1.84936775,0x2B,, +1.849463125,0x48,, +1.849558625,0x00,, +1.849654125,0x80,, +1.849749625,0x08,, +1.849845125,0x00,, +1.8499405,0xD5,, +1.850036,0x42,, +1.8501315,0xAB,, +1.850227,0x80,, +1.850322375,0x0D,, +1.850417875,0x54,, +1.850513375,0x80,, +1.85060875,0x08,, +1.85070425,0x00,, +1.85079975,0x2A,, +1.85089525,0x62,, +1.850990625,0xA6,, +1.851086125,0xBF,, +1.86870425,0x55,, +1.868799625,0x55,, +1.868895125,0x18,, +1.868990625,0x00,, +1.869086125,0x20,, +1.8691815,0x7E,, +1.869277,0xC9,, +1.8693725,0x00,, +1.869468,0x2B,, +1.869563375,0x48,, +1.869658875,0x00,, +1.869754375,0x80,, +1.869849875,0x08,, +1.86994525,0x00,, +1.87004075,0xD5,, +1.87013625,0x42,, +1.870231625,0xAB,, +1.870327125,0x80,, +1.870422625,0x0D,, +1.870518125,0x54,, +1.8706135,0x80,, +1.870709,0x08,, +1.8708045,0x00,, +1.8709,0x2A,, +1.8709955,0x62,, +1.871090875,0xA6,, +1.871186375,0xA5,, +1.8888045,0x55,, +1.888899875,0x55,, +1.888995375,0x18,, +1.889090875,0x00,, +1.88918625,0x21,, +1.88928175,0x43,, +1.88937725,0xC1,, +1.88947275,0x00,, +1.889568125,0x2B,, +1.889663625,0x48,, +1.889759125,0x00,, +1.889854625,0x80,, +1.88995,0x08,, +1.8900455,0x47,, +1.890141,0xD5,, +1.890236375,0x42,, +1.890331875,0xAB,, +1.890427375,0x80,, +1.890522875,0x0D,, +1.890618375,0x54,, +1.89071375,0x80,, +1.89080925,0x08,, +1.89090475,0x00,, +1.891000125,0x2A,, +1.891095625,0x62,, +1.891191125,0xA6,, +1.891286625,0xCD,, +1.908904625,0x55,, +1.909000125,0x55,, +1.909095625,0x18,, +1.909191,0x00,, +1.9092865,0x22,, +1.909382,0x09,, +1.9094775,0xD0,, +1.909572875,0x00,, +1.909668375,0x2B,, +1.909763875,0x68,, +1.90985925,0x00,, +1.90995475,0x80,, +1.91005025,0x08,, +1.91014575,0x5A,, +1.91024125,0xD5,, +1.910336625,0x42,, +1.910432125,0xAB,, +1.910527625,0x80,, +1.910623125,0x0D,, +1.9107185,0x54,, +1.910814,0x80,, +1.9109095,0x08,, +1.911004875,0x00,, +1.911100375,0x2A,, +1.911195875,0x62,, +1.911291375,0xA6,, +1.91138675,0x3D,, +1.92899625,0x55,, +1.929091625,0x55,, +1.929187125,0x18,, +1.929282625,0x00,, +1.929378,0x22,, +1.9294735,0xD0,, +1.929569,0xDD,, +1.9296645,0x00,, +1.929759875,0x2B,, +1.929855375,0x88,, +1.929950875,0x00,, +1.930046375,0x80,, +1.93014175,0x08,, +1.93023725,0x74,, +1.93033275,0xD5,, +1.93042825,0x42,, +1.930523625,0xAB,, +1.930619125,0x80,, +1.930714625,0x0D,, +1.930810125,0x54,, +1.9309055,0x80,, +1.931001,0x08,, +1.9310965,0x00,, +1.931191875,0x2A,, +1.931287375,0x62,, +1.931382875,0xA6,, +1.931478375,0x82,, +1.94954775,0x55,, +1.949643125,0x55,, +1.949738625,0x18,, +1.949834125,0x00,, +1.949929625,0x23,, +1.950025,0x9B,, +1.9501205,0xCF,, +1.950216,0x00,, +1.950311375,0x2B,, +1.950406875,0x88,, +1.950502375,0x00,, +1.950597875,0x80,, +1.950693375,0x08,, +1.95078875,0x79,, +1.95088425,0xD5,, +1.95097975,0x42,, +1.95107525,0xAB,, +1.951170625,0x80,, +1.951266125,0x0D,, +1.951361625,0x54,, +1.951457,0x80,, +1.9515525,0x08,, +1.951648,0x00,, +1.9517435,0x2A,, +1.951838875,0x62,, +1.951934375,0xA6,, +1.952029875,0xDE,, +1.969318125,0x55,, +1.969413625,0x55,, +1.969509125,0x18,, +1.9696045,0x00,, +1.9697,0x24,, +1.9697955,0x5D,, +1.969890875,0xDD,, +1.969986375,0x00,, +1.970081875,0x2B,, +1.970177375,0x78,, +1.970272875,0x00,, +1.97036825,0x80,, +1.97046375,0x08,, +1.97055925,0x79,, +1.970654625,0xD5,, +1.970750125,0x42,, +1.970845625,0xAB,, +1.970941125,0x80,, +1.9710365,0x0D,, +1.971132,0x54,, +1.9712275,0x80,, +1.971323,0x08,, +1.971418375,0x00,, +1.971513875,0x2A,, +1.971609375,0x62,, +1.97170475,0xA6,, +1.97180025,0x62,, +1.989296875,0x55,, +1.989392375,0x55,, +1.98948775,0x18,, +1.98958325,0x00,, +1.98967875,0x25,, +1.989774125,0x23,, +1.989869625,0xDC,, +1.989965125,0x00,, +1.990060625,0x2B,, +1.990156,0x88,, +1.9902515,0x00,, +1.990347,0x80,, +1.9904425,0x08,, +1.990537875,0x6F,, +1.990633375,0xD5,, +1.990728875,0x42,, +1.990824375,0xAB,, +1.99091975,0x80,, +1.99101525,0x0D,, +1.99111075,0x54,, +1.99120625,0x80,, +1.991301625,0x08,, +1.991397125,0x00,, +1.991492625,0x2A,, +1.991588,0x62,, +1.9916835,0xA6,, +1.991779,0x36,, +2.00991775,0x55,, +2.01001325,0x55,, +2.01010875,0x2B,, +2.01020425,0x03,, +2.010299625,0x25,, +2.010395125,0xE9,, +2.010490625,0xDD,, +2.010586125,0x00,, +2.0106815,0x2B,, +2.010777,0xA8,, +2.0108725,0x00,, +2.010967875,0x80,, +2.011063375,0x08,, +2.011158875,0x51,, +2.011254375,0xD5,, +2.011349875,0x42,, +2.01144525,0xAB,, +2.01154075,0x80,, +2.01163625,0x0D,, +2.011731625,0x54,, +2.011827125,0x80,, +2.011922625,0x08,, +2.012018125,0x00,, +2.0121135,0x2A,, +2.012209,0x62,, +2.0123045,0xA6,, +2.0124,0x00,, +2.012495375,0x00,, +2.012590875,0x00,, +2.012686375,0x00,, +2.01278175,0x00,, +2.01287725,0x00,, +2.01297275,0x00,, +2.01306825,0x00,, +2.01316375,0x00,, +2.013259125,0x00,, +2.013354625,0x00,, +2.013450125,0x00,, +2.013545625,0x00,, +2.013641,0x00,, +2.0137365,0x00,, +2.013832,0x00,, +2.013927375,0x00,, +2.014022875,0x00,, +2.014118375,0x00,, +2.014213875,0xEC,, +2.029488625,0x55,, +2.029584125,0x55,, +2.0296795,0x18,, +2.029775,0x00,, +2.0298705,0x26,, +2.029965875,0xB0,, +2.030061375,0xDD,, +2.030156875,0x00,, +2.030252375,0x2B,, +2.030347875,0xC8,, +2.03044325,0x00,, +2.03053875,0x80,, +2.03063425,0x08,, +2.030729625,0x0D,, +2.030825125,0xD5,, +2.030920625,0x42,, +2.031016125,0xAB,, +2.0311115,0x80,, +2.031207,0x0D,, +2.0313025,0x54,, +2.031398,0x80,, +2.031493375,0x08,, +2.031588875,0x00,, +2.031684375,0x2A,, +2.031779875,0x62,, +2.03187525,0xA6,, +2.03197075,0x49,, +2.049588875,0x55,, +2.04968425,0x55,, +2.04977975,0x18,, +2.04987525,0x00,, +2.04997075,0x27,, +2.050066125,0x77,, +2.050161625,0xDA,, +2.050257125,0x00,, +2.050352625,0x2D,, +2.050448,0x78,, +2.0505435,0x00,, +2.050639,0x80,, +2.050734375,0x07,, +2.050829875,0xF2,, +2.050925375,0xD5,, +2.051020875,0x42,, +2.05111625,0xAB,, +2.05121175,0x80,, +2.05130725,0x0D,, +2.05140275,0x54,, +2.051498125,0x80,, +2.051593625,0x08,, +2.051689125,0x00,, +2.051784625,0x2A,, +2.05188,0x62,, +2.0519755,0xA6,, +2.052071,0xA7,, +2.069689,0x55,, +2.0697845,0x55,, +2.06988,0x18,, +2.0699755,0x00,, +2.070070875,0x28,, +2.070166375,0x3E,, +2.070261875,0xDD,, +2.07035725,0x00,, +2.07045275,0x31,, +2.07054825,0xD8,, +2.07064375,0x00,, +2.07073925,0x80,, +2.070834625,0x07,, +2.070930125,0x8D,, +2.071025625,0xD5,, +2.071121125,0x42,, +2.0712165,0xAB,, +2.071312,0x80,, +2.0714075,0x0D,, +2.071502875,0x54,, +2.071598375,0x80,, +2.071693875,0x08,, +2.071789375,0x00,, +2.07188475,0x2A,, +2.07198025,0x62,, +2.07207575,0xA6,, +2.07217125,0x85,, +2.08978925,0x55,, +2.08988475,0x55,, +2.08998025,0x18,, +2.090075625,0x00,, +2.090171125,0x29,, +2.090266625,0x03,, +2.090362125,0xDB,, +2.0904575,0x00,, +2.090553,0x36,, +2.0906485,0x78,, +2.090744,0x00,, +2.090839375,0x80,, +2.090934875,0x06,, +2.091030375,0xDC,, +2.09112575,0xD5,, +2.09122125,0x42,, +2.09131675,0xAB,, +2.09141225,0x80,, +2.091507625,0x0D,, +2.091603125,0x54,, +2.091698625,0x80,, +2.091794125,0x08,, +2.091889625,0x00,, +2.091985,0x2A,, +2.0920805,0x62,, +2.092175875,0xA6,, +2.092271375,0x8D,, +2.10988075,0x55,, +2.10997625,0x55,, +2.11007175,0x18,, +2.11016725,0x00,, +2.11026275,0x29,, +2.110358125,0xC9,, +2.110453625,0xDD,, +2.110549125,0x00,, +2.1106445,0x3A,, +2.11074,0x38,, +2.1108355,0x00,, +2.110931,0x80,, +2.111026375,0x06,, +2.111121875,0x4E,, +2.111217375,0xD5,, +2.111312875,0x42,, +2.11140825,0xAB,, +2.11150375,0x80,, +2.11159925,0x0D,, +2.111694625,0x54,, +2.111790125,0x80,, +2.111885625,0x08,, +2.111981125,0x00,, +2.1120765,0x2A,, +2.112172,0x62,, +2.1122675,0xA6,, +2.112363,0xB1,, +2.129981,0x55,, +2.1300765,0x55,, +2.130172,0x18,, +2.130267375,0x00,, +2.130362875,0x2A,, +2.130458375,0x8F,, +2.130553875,0xDD,, +2.13064925,0x00,, +2.13074475,0x3D,, +2.13084025,0x98,, +2.13093575,0x00,, +2.131031125,0x80,, +2.131126625,0x05,, +2.131222125,0x78,, +2.131317625,0xD5,, +2.131413,0x42,, +2.1315085,0xAB,, +2.131604,0x80,, +2.131699375,0x0D,, +2.131794875,0x54,, +2.131890375,0x80,, +2.131985875,0x08,, +2.132081375,0x00,, +2.13217675,0x2A,, +2.13227225,0x62,, +2.13236775,0xA6,, +2.132463125,0x9A,, +2.15020275,0x55,, +2.15029825,0x55,, +2.150393625,0x18,, +2.150489125,0x00,, +2.150584625,0x2B,, +2.150680125,0x55,, +2.150775625,0xDB,, +2.150871,0x00,, +2.1509665,0x3E,, +2.151062,0x18,, +2.151157375,0x00,, +2.151252875,0x80,, +2.151348375,0x04,, +2.151443875,0xC7,, +2.15153925,0xD5,, +2.15163475,0x42,, +2.15173025,0xAB,, +2.15182575,0x80,, +2.151921125,0x0D,, +2.152016625,0x54,, +2.152112125,0x80,, +2.1522075,0x08,, +2.152303,0x00,, +2.1523985,0x2A,, +2.152494,0x62,, +2.1525895,0xA6,, +2.152684875,0xE0,, +2.170303,0x55,, +2.1703985,0x55,, +2.170493875,0x18,, +2.170589375,0x00,, +2.170684875,0x2C,, +2.17078025,0x1A,, +2.17087575,0xD9,, +2.17097125,0x00,, +2.17106675,0x3D,, +2.171162125,0xE8,, +2.171257625,0x00,, +2.171353125,0x80,, +2.171448625,0x03,, +2.171544,0xEA,, +2.1716395,0xD5,, +2.171735,0x42,, +2.171830375,0xAB,, +2.171925875,0x80,, +2.172021375,0x0D,, +2.172116875,0x54,, +2.172212375,0x80,, +2.17230775,0x08,, +2.17240325,0x00,, +2.17249875,0x2A,, +2.17259425,0x62,, +2.172689625,0xA6,, +2.172785125,0x0E,, +2.190273,0x55,, +2.1903685,0x55,, +2.190463875,0x18,, +2.190559375,0x00,, +2.190654875,0x2C,, +2.190750375,0xE0,, +2.190845875,0xDD,, +2.19094125,0x00,, +2.19103675,0x3B,, +2.19113225,0x48,, +2.19122775,0x00,, +2.191323125,0x80,, +2.191418625,0x03,, +2.191514125,0x04,, +2.1916095,0xD5,, +2.191705,0x42,, +2.1918005,0xAB,, +2.191896,0x80,, +2.191991375,0x0D,, +2.192086875,0x54,, +2.192182375,0x80,, +2.192277875,0x08,, +2.19237325,0x00,, +2.19246875,0x2A,, +2.19256425,0x62,, +2.192659625,0xA6,, +2.192755125,0x06,, +2.210781125,0x55,, +2.210876625,0x55,, +2.210972,0x2B,, +2.2110675,0x03,, +2.211163,0x2D,, +2.2112585,0xA6,, +2.211353875,0xDC,, +2.211449375,0x00,, +2.211544875,0x36,, +2.211640375,0xF8,, +2.211735875,0x00,, +2.21183125,0x80,, +2.21192675,0x02,, +2.21202225,0xB0,, +2.212117625,0xD5,, +2.212213125,0x42,, +2.212308625,0xAB,, +2.212404125,0x80,, +2.2124995,0x0D,, +2.212595,0x54,, +2.2126905,0x80,, +2.212786,0x08,, +2.212881375,0x00,, +2.212976875,0x2A,, +2.213072375,0x62,, +2.21316775,0xA6,, +2.21326325,0x00,, +2.21335875,0x00,, +2.21345425,0x00,, +2.21354975,0x00,, +2.213645125,0x00,, +2.213740625,0x00,, +2.213836125,0x00,, +2.2139315,0x00,, +2.214027,0x00,, +2.2141225,0x00,, +2.214218,0x00,, +2.214313375,0x00,, +2.214408875,0x00,, +2.214504375,0x00,, +2.214599875,0x00,, +2.21469525,0x00,, +2.21479075,0x00,, +2.21488625,0x00,, +2.21498175,0x00,, +2.215077125,0xDD,, +2.230595,0x55,, +2.230690375,0x55,, +2.230785875,0x18,, +2.230881375,0x00,, +2.230976875,0x2E,, +2.23107225,0x6D,, +2.23116775,0xD9,, +2.23126325,0x00,, +2.23135875,0x36,, +2.231454125,0xC8,, +2.231549625,0x00,, +2.231645125,0x80,, +2.2317405,0x02,, +2.231836,0xAB,, +2.2319315,0xD5,, +2.232027,0x42,, +2.232122375,0xAB,, +2.232217875,0x80,, +2.232313375,0x0D,, +2.232408875,0x54,, +2.232504375,0x80,, +2.23259975,0x08,, +2.23269525,0x00,, +2.232790625,0x2A,, +2.232886125,0x62,, +2.232981625,0xA6,, +2.233077125,0xA7,, +2.250695125,0x55,, +2.250790625,0x55,, +2.250886125,0x18,, +2.250981625,0x00,, +2.251077,0x2F,, +2.2511725,0x33,, +2.251268,0xDC,, +2.2513635,0x00,, +2.251458875,0x36,, +2.251554375,0xC8,, +2.251649875,0x00,, +2.25174525,0x80,, +2.25184075,0x02,, +2.25193625,0xAB,, +2.25203175,0xD5,, +2.25212725,0x42,, +2.252222625,0xAB,, +2.252318125,0x80,, +2.252413625,0x0D,, +2.252509,0x54,, +2.2526045,0x80,, +2.2527,0x08,, +2.2527955,0x00,, +2.252890875,0x2A,, +2.252986375,0x62,, +2.253081875,0xA6,, +2.253177375,0x3E,, +2.27117725,0x55,, +2.27127275,0x55,, +2.271368125,0x18,, +2.271463625,0x00,, +2.271559125,0x2F,, +2.271654625,0xFF,, +2.271750125,0xC5,, +2.2718455,0x00,, +2.271941,0x36,, +2.2720365,0xB8,, +2.272132,0x00,, +2.272227375,0x80,, +2.272322875,0x02,, +2.272418375,0xAB,, +2.27251375,0xD5,, +2.27260925,0x42,, +2.27270475,0xAB,, +2.27280025,0x80,, +2.272895625,0x0D,, +2.272991125,0x54,, +2.273086625,0x80,, +2.273182125,0x08,, +2.2732775,0x00,, +2.273373,0x2A,, +2.2734685,0x62,, +2.273564,0xA6,, +2.273659375,0x80,, +2.291399,0x55,, +2.2914945,0x55,, +2.291589875,0x18,, +2.291685375,0x00,, +2.291780875,0x30,, +2.29187625,0xC7,, +2.29197175,0xCA,, +2.29206725,0x00,, +2.29216275,0x35,, +2.29225825,0xE8,, +2.292353625,0x00,, +2.292449125,0x80,, +2.292544625,0x02,, +2.292640125,0xAB,, +2.2927355,0xD5,, +2.292831,0x42,, +2.2929265,0xAB,, +2.293021875,0x80,, +2.293117375,0x0D,, +2.293212875,0x54,, +2.293308375,0x80,, +2.29340375,0x08,, +2.29349925,0x00,, +2.29359475,0x2A,, +2.29369025,0x62,, +2.293785625,0xA6,, +2.293881125,0xB6,, +2.310865625,0x55,, +2.310961125,0x55,, +2.311056625,0x18,, +2.311152,0x00,, +2.3112475,0x31,, +2.311343,0x86,, +2.3114385,0xDD,, +2.311533875,0x00,, +2.311629375,0x32,, +2.311724875,0x28,, +2.31182025,0x00,, +2.31191575,0x80,, +2.31201125,0x02,, +2.31210675,0xAB,, +2.31220225,0xD5,, +2.312297625,0x42,, +2.312393125,0xAB,, +2.312488625,0x80,, +2.312584125,0x0D,, +2.3126795,0x54,, +2.312775,0x80,, +2.3128705,0x08,, +2.312965875,0x00,, +2.313061375,0x2A,, +2.313156875,0x62,, +2.313252375,0xA6,, +2.31334775,0xF7,, +2.331087375,0x55,, +2.331182875,0x55,, +2.33127825,0x18,, +2.33137375,0x00,, +2.33146925,0x32,, +2.33156475,0x4B,, +2.331660125,0xDD,, +2.331755625,0x00,, +2.331851125,0x2F,, +2.331946625,0x78,, +2.332042,0x00,, +2.3321375,0x80,, +2.332233,0x02,, +2.332328375,0xAC,, +2.332423875,0xD5,, +2.332519375,0x42,, +2.332614875,0xAB,, +2.332710375,0x80,, +2.33280575,0x0D,, +2.33290125,0x54,, +2.33299675,0x80,, +2.33309225,0x08,, +2.333187625,0x00,, +2.333283125,0x2A,, +2.333378625,0x62,, +2.333474,0xA6,, +2.3335695,0xA7,, +2.351057375,0x55,, +2.351152875,0x55,, +2.351248375,0x18,, +2.35134375,0x00,, +2.35143925,0x33,, +2.35153475,0x11,, +2.35163025,0xDB,, +2.35172575,0x00,, +2.351821125,0x2F,, +2.351916625,0x78,, +2.352012125,0x00,, +2.3521075,0x80,, +2.352203,0x02,, +2.3522985,0xB0,, +2.352394,0xD5,, +2.352489375,0x42,, +2.352584875,0xAB,, +2.352680375,0x80,, +2.352775875,0x0D,, +2.35287125,0x54,, +2.35296675,0x80,, +2.35306225,0x08,, +2.353157625,0x00,, +2.353253125,0x2A,, +2.353348625,0x62,, +2.353444125,0xA6,, +2.353539625,0xCE,, +2.371157625,0x55,, +2.371253125,0x55,, +2.371348625,0x18,, +2.371444,0x00,, +2.3715395,0x33,, +2.371635,0xD7,, +2.371730375,0xDD,, +2.371825875,0x00,, +2.371921375,0x2F,, +2.372016875,0x78,, +2.37211225,0x00,, +2.37220775,0x80,, +2.37230325,0x02,, +2.37239875,0xD3,, +2.372494125,0xD5,, +2.372589625,0x42,, +2.372685125,0xAB,, +2.3727805,0x80,, +2.372876,0x0D,, +2.3729715,0x54,, +2.373067,0x80,, +2.3731625,0x08,, +2.373257875,0x00,, +2.373353375,0x2A,, +2.373448875,0x62,, +2.373544375,0xA6,, +2.37363975,0xB0,, +2.391249125,0x55,, +2.391344625,0x55,, +2.391440125,0x18,, +2.391535625,0x00,, +2.391631,0x34,, +2.3917265,0x9D,, +2.391822,0xDA,, +2.3919175,0x00,, +2.392012875,0x2F,, +2.392108375,0xA8,, +2.392203875,0x00,, +2.39229925,0x80,, +2.39239475,0x03,, +2.39249025,0x1E,, +2.39258575,0xD5,, +2.392681125,0x42,, +2.392776625,0xAB,, +2.392872125,0x80,, +2.392967625,0x0D,, +2.393063,0x54,, +2.3931585,0x80,, +2.393254,0x08,, +2.3933495,0x00,, +2.393444875,0x2A,, +2.393540375,0x62,, +2.393635875,0xA6,, +2.393731375,0x02,, +2.41175725,0x55,, +2.41185275,0x55,, +2.41194825,0x2B,, +2.41204375,0x03,, +2.412139125,0x35,, +2.412234625,0x63,, +2.412330125,0xDB,, +2.412425625,0x00,, +2.412521,0x2F,, +2.4126165,0x88,, +2.412712,0x00,, +2.412807375,0x80,, +2.412902875,0x03,, +2.412998375,0x9B,, +2.413093875,0xD5,, +2.41318925,0x42,, +2.41328475,0xAB,, +2.41338025,0x80,, +2.41347575,0x0D,, +2.41357125,0x54,, +2.413666625,0x80,, +2.413762125,0x08,, +2.413857625,0x00,, +2.413953,0x2A,, +2.4140485,0x62,, +2.414144,0xA6,, +2.4142395,0x00,, +2.414334875,0x00,, +2.414430375,0x00,, +2.414525875,0x00,, +2.414621375,0x00,, +2.41471675,0x00,, +2.41481225,0x00,, +2.41490775,0x00,, +2.415003125,0x00,, +2.415098625,0x00,, +2.415194125,0x00,, +2.415289625,0x00,, +2.415385,0x00,, +2.4154805,0x00,, +2.415576,0x00,, +2.4156715,0x00,, +2.415766875,0x00,, +2.415862375,0x00,, +2.415957875,0x00,, +2.416053375,0x2D,, +2.431449625,0x55,, +2.431545125,0x55,, +2.4316405,0x18,, +2.431736,0x00,, +2.4318315,0x36,, +2.431926875,0x28,, +2.432022375,0xDD,, +2.432117875,0x00,, +2.432213375,0x2F,, +2.432308875,0xA8,, +2.43240425,0x00,, +2.43249975,0x80,, +2.43259525,0x04,, +2.432690625,0x4C,, +2.432786125,0xD5,, +2.432881625,0x42,, +2.432977125,0xAB,, +2.4330725,0x80,, +2.433168,0x0D,, +2.4332635,0x54,, +2.433359,0x80,, +2.433454375,0x08,, +2.433549875,0x00,, +2.433645375,0x2A,, +2.433740875,0x62,, +2.43383625,0xA6,, +2.43393175,0x24,, +2.45168,0x55,, +2.4517755,0x55,, +2.451871,0x18,, +2.451966375,0x00,, +2.452061875,0x36,, +2.452157375,0xEE,, +2.452252875,0xDB,, +2.45234825,0x00,, +2.45244375,0x2F,, +2.45253925,0x88,, +2.452634625,0x00,, +2.452730125,0x80,, +2.452825625,0x05,, +2.452921125,0x25,, +2.4530165,0xD5,, +2.453112,0x42,, +2.4532075,0xAB,, +2.453303,0x80,, +2.4533985,0x0D,, +2.453493875,0x54,, +2.453589375,0x80,, +2.45368475,0x08,, +2.45378025,0x00,, +2.45387575,0x2A,, +2.45397125,0x62,, +2.45406675,0xA6,, +2.454162125,0xEB,, +2.47165,0x55,, +2.4717455,0x55,, +2.471841,0x18,, +2.4719365,0x00,, +2.472031875,0x37,, +2.472127375,0xB4,, +2.472222875,0xDB,, +2.47231825,0x00,, +2.47241375,0x2F,, +2.47250925,0xB8,, +2.47260475,0x00,, +2.47270025,0x80,, +2.472795625,0x06,, +2.472891125,0x52,, +2.472986625,0xD5,, +2.473082125,0x42,, +2.4731775,0xAB,, +2.473273,0x80,, +2.4733685,0x0D,, +2.473463875,0x54,, +2.473559375,0x80,, +2.473654875,0x08,, +2.473750375,0x00,, +2.47384575,0x2A,, +2.47394125,0x62,, +2.47403675,0xA6,, +2.47413225,0x67,, +2.491741625,0x55,, +2.491837,0x55,, +2.4919325,0x18,, +2.492028,0x00,, +2.4921235,0x38,, +2.492218875,0x7B,, +2.492314375,0xDD,, +2.492409875,0x00,, +2.492505375,0x30,, +2.49260075,0x18,, +2.49269625,0x00,, +2.49279175,0x80,, +2.492887125,0x07,, +2.492982625,0x9A,, +2.493078125,0xD5,, +2.493173625,0x42,, +2.493269125,0xAB,, +2.4933645,0x80,, +2.49346,0x0D,, +2.4935555,0x54,, +2.493651,0x80,, +2.493746375,0x08,, +2.493841875,0x00,, +2.493937375,0x2A,, +2.49403275,0x62,, +2.49412825,0xA6,, +2.49422375,0xD8,, +2.51184175,0x55,, +2.51193725,0x55,, +2.51203275,0x18,, +2.51212825,0x00,, +2.51222375,0x39,, +2.512319125,0x41,, +2.512414625,0xDD,, +2.512510125,0x00,, +2.5126055,0x30,, +2.512701,0x08,, +2.5127965,0x00,, +2.512892,0x80,, +2.512987375,0x09,, +2.513082875,0x17,, +2.513178375,0xD5,, +2.513273875,0x42,, +2.51336925,0xAB,, +2.51346475,0x80,, +2.51356025,0x0D,, +2.513655625,0x54,, +2.513751125,0x80,, +2.513846625,0x08,, +2.513942125,0x00,, +2.5140375,0x2A,, +2.514133,0x62,, +2.5142285,0xA6,, +2.514324,0x91,, +2.531942,0x55,, +2.5320375,0x55,, +2.532133,0x18,, +2.532228375,0x00,, +2.532323875,0x3A,, +2.532419375,0x07,, +2.532514875,0xDE,, +2.53261025,0x00,, +2.53270575,0x2F,, +2.53280125,0xF8,, +2.53289675,0x00,, +2.532992125,0x80,, +2.533087625,0x0A,, +2.533183125,0xA8,, +2.533278625,0xD5,, +2.533374,0x42,, +2.5334695,0xAB,, +2.533565,0x80,, +2.533660375,0x0D,, +2.533755875,0x54,, +2.533851375,0x80,, +2.533946875,0x08,, +2.534042375,0x00,, +2.53413775,0x2A,, +2.53423325,0x62,, +2.53432875,0xA6,, +2.534424125,0x87,, +2.5520335,0x55,, +2.552129,0x55,, +2.5522245,0x18,, +2.55232,0x00,, +2.5524155,0x3A,, +2.552510875,0xCE,, +2.552606375,0xDC,, +2.552701875,0x00,, +2.55279725,0x2F,, +2.55289275,0xE8,, +2.55298825,0x00,, +2.55308375,0x80,, +2.553179125,0x0C,, +2.553274625,0x35,, +2.553370125,0xD5,, +2.553465625,0x42,, +2.553561,0xAB,, +2.5536565,0x80,, +2.553752,0x0D,, +2.553847375,0x54,, +2.553942875,0x80,, +2.554038375,0x08,, +2.554133875,0x00,, +2.554229375,0x2A,, +2.55432475,0x62,, +2.55442025,0xA6,, +2.55451575,0xDF,, +2.57213375,0x55,, +2.57222925,0x55,, +2.57232475,0x18,, +2.57242025,0x00,, +2.572515625,0x3B,, +2.572611125,0x95,, +2.572706625,0xDB,, +2.572802,0x00,, +2.5728975,0x30,, +2.572993,0x08,, +2.5730885,0x00,, +2.573183875,0x80,, +2.573279375,0x0D,, +2.573374875,0x51,, +2.573470375,0xD5,, +2.57356575,0x42,, +2.57366125,0xAB,, +2.57375675,0x80,, +2.57385225,0x0D,, +2.573947625,0x54,, +2.574043125,0x80,, +2.574138625,0x08,, +2.574234125,0x00,, +2.5743295,0x2A,, +2.574425,0x62,, +2.5745205,0xA6,, +2.574615875,0x09,, +2.592234,0x55,, +2.5923295,0x55,, +2.592424875,0x18,, +2.592520375,0x00,, +2.592615875,0x3C,, +2.592711375,0x5C,, +2.592806875,0xDD,, +2.59290225,0x00,, +2.59299775,0x2F,, +2.59309325,0xE8,, +2.59318875,0x00,, +2.593284125,0x80,, +2.593379625,0x0D,, +2.593475125,0x52,, +2.5935705,0xD5,, +2.593666,0x42,, +2.5937615,0xAB,, +2.593857,0x80,, +2.593952375,0x0D,, +2.594047875,0x54,, +2.594143375,0x80,, +2.594238875,0x08,, +2.59433425,0x00,, +2.59442975,0x2A,, +2.59452525,0x62,, +2.594620625,0xA6,, +2.594716125,0xDF,, +2.6127335,0x55,, +2.612828875,0x55,, +2.612924375,0x2B,, +2.613019875,0x03,, +2.613115375,0x3D,, +2.61321075,0x23,, +2.61330625,0xDD,, +2.61340175,0x00,, +2.61349725,0x2F,, +2.613592625,0xE8,, +2.613688125,0x00,, +2.613783625,0x80,, +2.613879,0x0D,, +2.6139745,0x52,, +2.61407,0xD5,, +2.6141655,0x42,, +2.614261,0xAB,, +2.614356375,0x80,, +2.614451875,0x0D,, +2.614547375,0x54,, +2.61464275,0x80,, +2.61473825,0x08,, +2.61483375,0x00,, +2.61492925,0x2A,, +2.615024625,0x62,, +2.615120125,0xA6,, +2.615215625,0x00,, +2.615311125,0x00,, +2.6154065,0x00,, +2.615502,0x00,, +2.6155975,0x00,, +2.615692875,0x00,, +2.615788375,0x00,, +2.615883875,0x00,, +2.615979375,0x00,, +2.61607475,0x00,, +2.61617025,0x00,, +2.61626575,0x00,, +2.61636125,0x00,, +2.61645675,0x00,, +2.616552125,0x00,, +2.616647625,0x00,, +2.616743125,0x00,, +2.6168385,0x00,, +2.616934,0x00,, +2.6170295,0xB6,, +2.6324345,0x55,, +2.632529875,0x55,, +2.632625375,0x18,, +2.632720875,0x00,, +2.632816375,0x3D,, +2.63291175,0xEA,, +2.63300725,0xDE,, +2.63310275,0x00,, +2.633198125,0x2F,, +2.633293625,0xE8,, +2.633389125,0x00,, +2.633484625,0x80,, +2.633580125,0x0D,, +2.6336755,0x51,, +2.633771,0xD5,, +2.6338665,0x42,, +2.633961875,0xAB,, +2.634057375,0x80,, +2.634152875,0x0D,, +2.634248375,0x54,, +2.63434375,0x80,, +2.63443925,0x08,, +2.63453475,0x00,, +2.63463025,0x2A,, +2.634725625,0x62,, +2.634821125,0xA6,, +2.634916625,0x51,, +2.652534625,0x55,, +2.652630125,0x55,, +2.652725625,0x18,, +2.652821,0x00,, +2.6529165,0x3E,, +2.653012,0xB0,, +2.6531075,0xDE,, +2.653203,0x00,, +2.653298375,0x2C,, +2.653393875,0xF8,, +2.653489375,0x00,, +2.65358475,0x80,, +2.65368025,0x0D,, +2.65377575,0x51,, +2.65387125,0xD5,, +2.653966625,0x42,, +2.654062125,0xAB,, +2.654157625,0x80,, +2.654253125,0x0D,, +2.6543485,0x54,, +2.654444,0x80,, +2.6545395,0x08,, +2.654635,0x00,, +2.654730375,0x2A,, +2.654825875,0x62,, +2.654921375,0xA6,, +2.655016875,0x77,, +2.67262625,0x55,, +2.672721625,0x55,, +2.672817125,0x18,, +2.672912625,0x00,, +2.673008125,0x3F,, +2.6731035,0x77,, +2.673199,0xDB,, +2.6732945,0x00,, +2.67339,0x2C,, +2.673485375,0xC8,, +2.673580875,0x00,, +2.673676375,0x80,, +2.673771875,0x0D,, +2.67386725,0x50,, +2.67396275,0xD5,, +2.67405825,0x42,, +2.674153625,0xAB,, +2.674249125,0x80,, +2.674344625,0x0D,, +2.674440125,0x54,, +2.6745355,0x80,, +2.674631,0x08,, +2.6747265,0x00,, +2.674822,0x2A,, +2.6749175,0x62,, +2.675012875,0xA6,, +2.675108375,0xE6,, +2.6927265,0x55,, +2.692821875,0x55,, +2.692917375,0x18,, +2.693012875,0x00,, +2.69310825,0x40,, +2.69320375,0x3E,, +2.69329925,0xDD,, +2.69339475,0x00,, +2.693490125,0x2C,, +2.693585625,0x68,, +2.693681125,0x00,, +2.693776625,0x80,, +2.693872,0x0D,, +2.6939675,0x51,, +2.694063,0xD5,, +2.694158375,0x42,, +2.694253875,0xAB,, +2.694349375,0x80,, +2.694444875,0x0D,, +2.694540375,0x54,, +2.69463575,0x80,, +2.69473125,0x08,, +2.69482675,0x00,, +2.694922125,0x2A,, +2.695017625,0x62,, +2.695113125,0xA6,, +2.695208625,0xAF,, +2.712826625,0x55,, +2.712922125,0x55,, +2.713017625,0x18,, +2.713113,0x00,, +2.7132085,0x41,, +2.713304,0x04,, +2.7133995,0xDC,, +2.713494875,0x00,, +2.713590375,0x2C,, +2.713685875,0x48,, +2.71378125,0x00,, +2.71387675,0x80,, +2.71397225,0x0D,, +2.71406775,0x50,, +2.71416325,0xD5,, +2.714258625,0x42,, +2.714354125,0xAB,, +2.714449625,0x80,, +2.714545125,0x0D,, +2.7146405,0x54,, +2.714736,0x80,, +2.7148315,0x08,, +2.714926875,0x00,, +2.715022375,0x2A,, +2.715117875,0x62,, +2.715213375,0xA6,, +2.71530875,0x93,, +2.732926875,0x55,, +2.733022375,0x55,, +2.733117875,0x18,, +2.73321325,0x00,, +2.73330875,0x41,, +2.73340425,0xCB,, +2.733499625,0xDC,, +2.733595125,0x00,, +2.733690625,0x2C,, +2.733786125,0x58,, +2.7338815,0x00,, +2.733977,0x80,, +2.7340725,0x0D,, +2.734168,0x26,, +2.734263375,0xD5,, +2.734358875,0x42,, +2.734454375,0xAB,, +2.73454975,0x80,, +2.73464525,0x0D,, +2.73474075,0x54,, +2.73483625,0x80,, +2.734931625,0x08,, +2.735027125,0x00,, +2.735122625,0x2A,, +2.735218125,0x62,, +2.7353135,0xA6,, +2.735409,0xA6,, +2.753018375,0x55,, +2.753113875,0x55,, +2.753209375,0x18,, +2.75330475,0x00,, +2.75340025,0x42,, +2.75349575,0x92,, +2.75359125,0xDE,, +2.75368675,0x00,, +2.753782125,0x2C,, +2.753877625,0x58,, +2.753973125,0x00,, +2.7540685,0x80,, +2.754164,0x0C,, +2.7542595,0x54,, +2.754355,0xD5,, +2.754450375,0x42,, +2.754545875,0xAB,, +2.754641375,0x80,, +2.754736875,0x0D,, +2.75483225,0x54,, +2.75492775,0x80,, +2.75502325,0x08,, +2.755118625,0x00,, +2.755214125,0x2A,, +2.755309625,0x62,, +2.755405125,0xA6,, +2.755500625,0xBD,, +2.773118625,0x55,, +2.773214125,0x55,, +2.773309625,0x18,, +2.773405,0x00,, +2.7735005,0x43,, +2.773596,0x58,, +2.773691375,0xDE,, +2.773786875,0x00,, +2.773882375,0x2C,, +2.773977875,0x58,, +2.77407325,0x00,, +2.77416875,0x80,, +2.77426425,0x0A,, +2.77435975,0xE9,, +2.774455125,0xD5,, +2.774550625,0x42,, +2.774646125,0xAB,, +2.7747415,0x80,, +2.774837,0x0D,, +2.7749325,0x54,, +2.775028,0x80,, +2.7751235,0x08,, +2.775218875,0x00,, +2.775314375,0x2A,, +2.775409875,0x62,, +2.775505375,0xA6,, +2.77560075,0xBA,, +2.793218875,0x55,, +2.793314375,0x55,, +2.79340975,0x18,, +2.79350525,0x00,, +2.79360075,0x44,, +2.793696125,0x1F,, +2.793791625,0xDD,, +2.793887125,0x00,, +2.793982625,0x2C,, +2.794078,0x38,, +2.7941735,0x00,, +2.794269,0x80,, +2.7943645,0x09,, +2.794459875,0x13,, +2.794555375,0xD5,, +2.794650875,0x42,, +2.794746375,0xAB,, +2.79484175,0x80,, +2.79493725,0x0D,, +2.79503275,0x54,, +2.79512825,0x80,, +2.795223625,0x08,, +2.795319125,0x00,, +2.795414625,0x2A,, +2.79551,0x62,, +2.7956055,0xA6,, +2.795701,0xCC,, +2.81371825,0x55,, +2.81381375,0x55,, +2.81390925,0x2B,, +2.81400475,0x03,, +2.814100125,0x44,, +2.814195625,0xE5,, +2.814291125,0xDC,, +2.814386625,0x00,, +2.814482,0x2C,, +2.8145775,0x68,, +2.814673,0x00,, +2.814768375,0x80,, +2.814863875,0x08,, +2.814959375,0x00,, +2.815054875,0xD5,, +2.81515025,0x42,, +2.81524575,0xAB,, +2.81534125,0x80,, +2.81543675,0x0D,, +2.815532125,0x54,, +2.815627625,0x80,, +2.815723125,0x08,, +2.815818625,0x00,, +2.815914,0x2A,, +2.8160095,0x62,, +2.816105,0xA6,, +2.8162005,0x00,, +2.816295875,0x00,, +2.816391375,0x00,, +2.816486875,0x00,, +2.816582375,0x00,, +2.81667775,0x00,, +2.81677325,0x00,, +2.81686875,0x00,, +2.816964125,0x00,, +2.817059625,0x00,, +2.817155125,0x00,, +2.817250625,0x00,, +2.817346,0x00,, +2.8174415,0x00,, +2.817537,0x00,, +2.8176325,0x00,, +2.817727875,0x00,, +2.817823375,0x00,, +2.817918875,0x00,, +2.818014375,0x3E,, +2.833410625,0x55,, +2.833506125,0x55,, +2.8336015,0x18,, +2.833697,0x00,, +2.8337925,0x45,, +2.833887875,0xAA,, +2.833983375,0xDD,, +2.834078875,0x00,, +2.834174375,0x2C,, +2.834269875,0x48,, +2.83436525,0x00,, +2.83446075,0x80,, +2.83455625,0x08,, +2.834651625,0x00,, +2.834747125,0xD5,, +2.834842625,0x42,, +2.834938125,0xAB,, +2.8350335,0x80,, +2.835129,0x0D,, +2.8352245,0x54,, +2.83532,0x80,, +2.835415375,0x08,, +2.835510875,0x00,, +2.835606375,0x2A,, +2.835701875,0x62,, +2.83579725,0xA6,, +2.83589275,0x49,, +2.853510875,0x55,, +2.85360625,0x55,, +2.85370175,0x18,, +2.85379725,0x00,, +2.85389275,0x46,, +2.853988125,0x6F,, +2.854083625,0xDB,, +2.854179125,0x00,, +2.854274625,0x2C,, +2.85437,0x48,, +2.8544655,0x00,, +2.854561,0x80,, +2.854656375,0x08,, +2.854751875,0x00,, +2.854847375,0xD5,, +2.854942875,0x42,, +2.85503825,0xAB,, +2.85513375,0x80,, +2.85522925,0x0D,, +2.85532475,0x54,, +2.855420125,0x80,, +2.855515625,0x08,, +2.855611125,0x00,, +2.855706625,0x2A,, +2.855802,0x62,, +2.8558975,0xA6,, +2.855993,0x34,, +2.873611,0x55,, +2.8737065,0x55,, +2.873802,0x18,, +2.8738975,0x00,, +2.873992875,0x47,, +2.874088375,0x35,, +2.874183875,0xDD,, +2.87427925,0x00,, +2.87437475,0x2C,, +2.87447025,0x68,, +2.87456575,0x00,, +2.874661125,0x80,, +2.874756625,0x08,, +2.874852125,0x00,, +2.874947625,0xD5,, +2.875043125,0x42,, +2.8751385,0xAB,, +2.875234,0x80,, +2.8753295,0x0D,, +2.875424875,0x54,, +2.875520375,0x80,, +2.875615875,0x08,, +2.875711375,0x00,, +2.87580675,0x2A,, +2.87590225,0x62,, +2.87599775,0xA6,, +2.87609325,0x07,, +2.893702625,0x55,, +2.893798,0x55,, +2.8938935,0x18,, +2.893989,0x00,, +2.8940845,0x47,, +2.894179875,0xFC,, +2.894275375,0xDB,, +2.894370875,0x00,, +2.894466375,0x2C,, +2.89456175,0x48,, +2.89465725,0x00,, +2.89475275,0x80,, +2.894848125,0x08,, +2.894943625,0x00,, +2.895039125,0xD5,, +2.895134625,0x42,, +2.895230125,0xAB,, +2.8953255,0x80,, +2.895421,0x0D,, +2.8955165,0x54,, +2.895612,0x80,, +2.895707375,0x08,, +2.895802875,0x00,, +2.895898375,0x2A,, +2.89599375,0x62,, +2.89608925,0xA6,, +2.89618475,0x60,, +2.9138115,0x55,, +2.913907,0x55,, +2.914002375,0x18,, +2.914097875,0x00,, +2.914193375,0x48,, +2.914288875,0xC2,, +2.91438425,0xDC,, +2.91447975,0x00,, +2.91457525,0x2C,, +2.91467075,0x68,, +2.914766125,0x00,, +2.914861625,0x80,, +2.914957125,0x08,, +2.9150525,0x00,, +2.915148,0xD5,, +2.9152435,0x42,, +2.915339,0xAB,, +2.9154345,0x80,, +2.915529875,0x0D,, +2.915625375,0x54,, +2.915720875,0x80,, +2.91581625,0x08,, +2.91591175,0x00,, +2.91600725,0x2A,, +2.91610275,0x62,, +2.916198125,0xA6,, +2.916293625,0x4C,, +2.933903,0x55,, +2.9339985,0x55,, +2.934094,0x18,, +2.934189375,0x00,, +2.934284875,0x49,, +2.934380375,0x88,, +2.934475875,0xDC,, +2.93457125,0x00,, +2.93466675,0x2C,, +2.93476225,0x58,, +2.93485775,0x00,, +2.934953125,0x80,, +2.935048625,0x08,, +2.935144125,0x00,, +2.935239625,0xD5,, +2.935335,0x42,, +2.9354305,0xAB,, +2.935526,0x80,, +2.935621375,0x0D,, +2.935716875,0x54,, +2.935812375,0x80,, +2.935907875,0x08,, +2.936003375,0x00,, +2.93609875,0x2A,, +2.93619425,0x62,, +2.93628975,0xA6,, +2.936385125,0x13,, +2.9539945,0x55,, +2.95409,0x55,, +2.9541855,0x18,, +2.954281,0x00,, +2.9543765,0x4A,, +2.954471875,0x4F,, +2.954567375,0xDB,, +2.954662875,0x00,, +2.95475825,0x2C,, +2.95485375,0x68,, +2.95494925,0x00,, +2.95504475,0x80,, +2.955140125,0x08,, +2.955235625,0x00,, +2.955331125,0xD5,, +2.955426625,0x42,, +2.955522,0xAB,, +2.9556175,0x80,, +2.955713,0x0D,, +2.955808375,0x54,, +2.955903875,0x80,, +2.955999375,0x08,, +2.956094875,0x00,, +2.956190375,0x2A,, +2.95628575,0x62,, +2.95638125,0xA6,, +2.95647675,0xEA,, +2.97409475,0x55,, +2.97419025,0x55,, +2.97428575,0x18,, +2.97438125,0x00,, +2.974476625,0x4B,, +2.974572125,0x16,, +2.974667625,0xDC,, +2.974763,0x00,, +2.9748585,0x2C,, +2.974954,0x68,, +2.9750495,0x00,, +2.975144875,0x80,, +2.975240375,0x08,, +2.975335875,0x00,, +2.975431375,0xD5,, +2.97552675,0x42,, +2.97562225,0xAB,, +2.97571775,0x80,, +2.97581325,0x0D,, +2.975908625,0x54,, +2.976004125,0x80,, +2.976099625,0x08,, +2.976195125,0x00,, +2.9762905,0x2A,, +2.976386,0x62,, +2.9764815,0xA6,, +2.976576875,0x79,, +2.994195,0x55,, +2.9942905,0x55,, +2.994385875,0x18,, +2.994481375,0x00,, +2.994576875,0x4B,, +2.994672375,0xDD,, +2.994767875,0xDB,, +2.99486325,0x00,, +2.99495875,0x2C,, +2.99505425,0x48,, +2.99514975,0x00,, +2.995245125,0x80,, +2.995340625,0x08,, +2.995436125,0x00,, +2.9955315,0xD5,, +2.995627,0x42,, +2.9957225,0xAB,, +2.995818,0x80,, +2.995913375,0x0D,, +2.996008875,0x54,, +2.996104375,0x80,, +2.996199875,0x08,, +2.99629525,0x00,, +2.99639075,0x2A,, +2.99648625,0x62,, +2.996581625,0xA6,, +2.996677125,0xBD,, +3.0146945,0x55,, +3.014789875,0x55,, +3.014885375,0x2B,, +3.014980875,0x03,, +3.015076375,0x4C,, +3.01517175,0xA3,, +3.01526725,0xDC,, +3.01536275,0x00,, +3.01545825,0x2C,, +3.015553625,0x68,, +3.015649125,0x00,, +3.015744625,0x80,, +3.01584,0x08,, +3.0159355,0x00,, +3.016031,0xD5,, +3.0161265,0x42,, +3.016222,0xAB,, +3.016317375,0x80,, +3.016412875,0x0D,, +3.016508375,0x54,, +3.01660375,0x80,, +3.01669925,0x08,, +3.01679475,0x00,, +3.01689025,0x2A,, +3.016985625,0x62,, +3.017081125,0xA6,, +3.017176625,0x00,, +3.017272125,0x00,, +3.0173675,0x00,, +3.017463,0x00,, +3.0175585,0x00,, +3.017653875,0x00,, +3.017749375,0x00,, +3.017844875,0x00,, +3.017940375,0x00,, +3.01803575,0x00,, +3.01813125,0x00,, +3.01822675,0x00,, +3.01832225,0x00,, +3.01841775,0x00,, +3.018513125,0x00,, +3.018608625,0x00,, +3.018704125,0x00,, +3.0187995,0x00,, +3.018895,0x00,, +3.0189905,0xE5,, +3.0343955,0x55,, +3.034490875,0x55,, +3.034586375,0x18,, +3.034681875,0x00,, +3.034777375,0x4D,, +3.03487275,0x68,, +3.03496825,0xDC,, +3.03506375,0x00,, +3.035159125,0x2C,, +3.035254625,0x58,, +3.035350125,0x00,, +3.035445625,0x80,, +3.035541125,0x08,, +3.0356365,0x00,, +3.035732,0xD5,, +3.0358275,0x42,, +3.035922875,0xAB,, +3.036018375,0x80,, +3.036113875,0x0D,, +3.036209375,0x54,, +3.03630475,0x80,, +3.03640025,0x08,, +3.03649575,0x00,, +3.03659125,0x2A,, +3.036686625,0x62,, +3.036782125,0xA6,, +3.036877625,0x2A,, +3.054487,0x55,, +3.0545825,0x55,, +3.054677875,0x18,, +3.054773375,0x00,, +3.054868875,0x4E,, +3.054964375,0x2E,, +3.05505975,0xDC,, +3.05515525,0x00,, +3.05525075,0x2C,, +3.055346125,0x48,, +3.055441625,0x00,, +3.055537125,0x80,, +3.055632625,0x08,, +3.055728125,0x00,, +3.0558235,0xD5,, +3.055919,0x42,, +3.0560145,0xAB,, +3.05611,0x80,, +3.056205375,0x0D,, +3.056300875,0x54,, +3.056396375,0x80,, +3.05649175,0x08,, +3.05658725,0x00,, +3.05668275,0x2A,, +3.05677825,0x62,, +3.056873625,0xA6,, +3.056969125,0x3D,, +3.074595875,0x55,, +3.074691375,0x55,, +3.074786875,0x18,, +3.07488225,0x00,, +3.07497775,0x4E,, +3.07507325,0xF5,, +3.07516875,0xDC,, +3.075264125,0x00,, +3.075359625,0x2C,, +3.075455125,0x68,, +3.0755505,0x00,, +3.075646,0x80,, +3.0757415,0x08,, +3.075837,0x00,, +3.075932375,0xD5,, +3.076027875,0x42,, +3.076123375,0xAB,, +3.076218875,0x80,, +3.07631425,0x0D,, +3.07640975,0x54,, +3.07650525,0x80,, +3.07660075,0x08,, +3.076696125,0x00,, +3.076791625,0x2A,, +3.076887125,0x62,, +3.076982625,0xA6,, +3.077078,0x59,, +3.0946875,0x55,, +3.094782875,0x55,, +3.094878375,0x18,, +3.094973875,0x00,, +3.09506925,0x4F,, +3.09516475,0xBC,, +3.09526025,0xDC,, +3.09535575,0x00,, +3.095451125,0x2C,, +3.095546625,0x68,, +3.095642125,0x00,, +3.095737625,0x80,, +3.095833,0x08,, +3.0959285,0x00,, +3.096024,0xD5,, +3.096119375,0x42,, +3.096214875,0xAB,, +3.096310375,0x80,, +3.096405875,0x0D,, +3.096501375,0x54,, +3.09659675,0x80,, +3.09669225,0x08,, +3.09678775,0x00,, +3.096883125,0x2A,, +3.096978625,0x62,, +3.097074125,0xA6,, +3.097169625,0x6A,, +3.114779,0x55,, +3.1148745,0x55,, +3.114969875,0x18,, +3.115065375,0x00,, +3.115160875,0x50,, +3.11525625,0x82,, +3.11535175,0xDB,, +3.11544725,0x00,, +3.11554275,0x2C,, +3.115638125,0x68,, +3.115733625,0x00,, +3.115829125,0x80,, +3.115924625,0x08,, +3.11602,0x00,, +3.1161155,0xD5,, +3.116211,0x42,, +3.1163065,0xAB,, +3.116401875,0x80,, +3.116497375,0x0D,, +3.116592875,0x54,, +3.11668825,0x80,, +3.11678375,0x08,, +3.11687925,0x00,, +3.11697475,0x2A,, +3.11707025,0x62,, +3.117165625,0xA6,, +3.117261125,0xBA,, +3.13487925,0x55,, +3.134974625,0x55,, +3.135070125,0x18,, +3.135165625,0x00,, +3.135261,0x51,, +3.1353565,0x48,, +3.135452,0xDC,, +3.1355475,0x00,, +3.135642875,0x2C,, +3.135738375,0x68,, +3.135833875,0x00,, +3.135929375,0x80,, +3.13602475,0x08,, +3.13612025,0x00,, +3.13621575,0xD5,, +3.136311125,0x42,, +3.136406625,0xAB,, +3.136502125,0x80,, +3.136597625,0x0D,, +3.136693125,0x54,, +3.1367885,0x80,, +3.136884,0x08,, +3.1369795,0x00,, +3.137074875,0x2A,, +3.137170375,0x62,, +3.137265875,0xA6,, +3.137361375,0x2F,, +3.154979375,0x55,, +3.155074875,0x55,, +3.155170375,0x18,, +3.15526575,0x00,, +3.15536125,0x52,, +3.15545675,0x0E,, +3.15555225,0xDD,, +3.15564775,0x00,, +3.155743125,0x2C,, +3.155838625,0x68,, +3.155934125,0x00,, +3.1560295,0x6E,, +3.156125,0x98,, +3.1562205,0x00,, +3.156316,0xD5,, +3.156411375,0x42,, +3.156506875,0xAB,, +3.156602375,0x80,, +3.156697875,0x0D,, +3.15679325,0x54,, +3.15688875,0x80,, +3.15698425,0x08,, +3.157079625,0x00,, +3.157175125,0x2A,, +3.157270625,0x62,, +3.157366125,0xA6,, +3.157461625,0x13,, +3.175071,0x55,, +3.175166375,0x55,, +3.175261875,0x18,, +3.175357375,0x00,, +3.17545275,0x52,, +3.17554825,0xD4,, +3.17564375,0xDC,, +3.17573925,0x00,, +3.175834625,0x2C,, +3.175930125,0x58,, +3.176025625,0x00,, +3.176121125,0x5A,, +3.176216625,0x88,, +3.176312,0x00,, +3.1764075,0xD5,, +3.176503,0x42,, +3.176598375,0xAB,, +3.176693875,0x80,, +3.176789375,0x0D,, +3.176884875,0x54,, +3.17698025,0x80,, +3.17707575,0x08,, +3.17717125,0x00,, +3.17726675,0x2A,, +3.177362125,0x62,, +3.177457625,0xA6,, +3.177553125,0x7C,, +3.195179875,0x55,, +3.195275375,0x55,, +3.19537075,0x18,, +3.19546625,0x00,, +3.19556175,0x53,, +3.195657125,0x9B,, +3.195752625,0xDC,, +3.195848125,0x00,, +3.195943625,0x2C,, +3.196039,0x68,, +3.1961345,0x00,, +3.19623,0x4A,, +3.1963255,0xA8,, +3.196420875,0x00,, +3.196516375,0xD5,, +3.196611875,0x42,, +3.196707375,0xAB,, +3.19680275,0x80,, +3.19689825,0x0D,, +3.19699375,0x54,, +3.19708925,0x80,, +3.197184625,0x08,, +3.197280125,0x00,, +3.197375625,0x2A,, +3.197471,0x62,, +3.1975665,0xA6,, +3.197662,0x6B,, +3.215670625,0x55,, +3.215766125,0x55,, +3.2158615,0x2B,, +3.215957,0x03,, +3.2160525,0x54,, +3.216148,0x61,, +3.216243375,0xDD,, +3.216338875,0x00,, +3.216434375,0x2C,, +3.21652975,0x48,, +3.21662525,0x00,, +3.21672075,0x38,, +3.21681625,0xA8,, +3.21691175,0x00,, +3.217007125,0xD5,, +3.217102625,0x42,, +3.217198125,0xAB,, +3.217293625,0x80,, +3.217389,0x0D,, +3.2174845,0x54,, +3.21758,0x80,, +3.217675375,0x08,, +3.217770875,0x00,, +3.217866375,0x2A,, +3.217961875,0x62,, +3.21805725,0xA6,, +3.21815275,0x00,, +3.21824825,0x00,, +3.21834375,0x00,, +3.218439125,0x00,, +3.218534625,0x00,, +3.218630125,0x00,, +3.2187255,0x00,, +3.218821,0x00,, +3.2189165,0x00,, +3.219012,0x00,, +3.2191075,0x00,, +3.219202875,0x00,, +3.219298375,0x00,, +3.219393875,0x00,, +3.21948925,0x00,, +3.21958475,0x00,, +3.21968025,0x00,, +3.21977575,0x00,, +3.219871125,0x00,, +3.219966625,0x93,, +3.235371625,0x55,, +3.235467125,0x55,, +3.2355625,0x18,, +3.235658,0x00,, +3.2357535,0x55,, +3.235848875,0x27,, +3.235944375,0xDC,, +3.236039875,0x00,, +3.236135375,0x2C,, +3.236230875,0x67,, +3.23632625,0xF8,, +3.23642175,0x2A,, +3.23651725,0xB8,, +3.236612625,0x00,, +3.236708125,0xD5,, +3.236803625,0x42,, +3.236899125,0xAB,, +3.2369945,0x80,, +3.23709,0x0D,, +3.2371855,0x54,, +3.237281,0x80,, +3.237376375,0x08,, +3.237471875,0x00,, +3.237567375,0x2A,, +3.237662875,0x62,, +3.23775825,0xA6,, +3.23785375,0xB8,, +3.255471875,0x55,, +3.25556725,0x55,, +3.25566275,0x18,, +3.25575825,0x00,, +3.25585375,0x55,, +3.255949125,0xED,, +3.256044625,0xDE,, +3.256140125,0x00,, +3.256235625,0x2C,, +3.256331,0x67,, +3.2564265,0x7B,, +3.256522,0x2A,, +3.256617375,0xB8,, +3.256712875,0x00,, +3.256808375,0xD5,, +3.256903875,0x42,, +3.25699925,0xAB,, +3.25709475,0x80,, +3.25719025,0x0D,, +3.25728575,0x54,, +3.257381125,0x80,, +3.257476625,0x08,, +3.257572125,0x00,, +3.257667625,0x2A,, +3.257763,0x62,, +3.2578585,0xA6,, +3.257954,0xE2,, +3.275563375,0x55,, +3.275658875,0x55,, +3.275754375,0x18,, +3.27584975,0x00,, +3.27594525,0x56,, +3.27604075,0xB3,, +3.276136125,0xDD,, +3.276231625,0x00,, +3.276327125,0x2C,, +3.276422625,0x66,, +3.276518,0xEE,, +3.2766135,0x2A,, +3.276709,0xB8,, +3.2768045,0x00,, +3.276899875,0xD5,, +3.276995375,0x42,, +3.277090875,0xAB,, +3.27718625,0x80,, +3.27728175,0x0D,, +3.27737725,0x54,, +3.27747275,0x80,, +3.27756825,0x08,, +3.277663625,0x00,, +3.277759125,0x2A,, +3.277854625,0x62,, +3.27795,0xA6,, +3.2780455,0x14,, +3.29567225,0x55,, +3.29576775,0x55,, +3.29586325,0x18,, +3.295958625,0x00,, +3.296054125,0x57,, +3.296149625,0x7A,, +3.296245125,0xDE,, +3.2963405,0x00,, +3.296436,0x2C,, +3.2965315,0x66,, +3.296627,0x78,, +3.296722375,0x2A,, +3.296817875,0xB8,, +3.296913375,0x00,, +3.29700875,0xD5,, +3.29710425,0x42,, +3.29719975,0xAB,, +3.29729525,0x80,, +3.297390625,0x0D,, +3.297486125,0x54,, +3.297581625,0x80,, +3.297677125,0x08,, +3.2977725,0x00,, +3.297868,0x2A,, +3.2979635,0x62,, +3.298058875,0xA6,, +3.298154375,0x51,, +3.31576375,0x55,, +3.31585925,0x55,, +3.31595475,0x18,, +3.31605025,0x00,, +3.316145625,0x58,, +3.316241125,0x40,, +3.316336625,0xDE,, +3.316432125,0x00,, +3.3165275,0x2C,, +3.316623,0x66,, +3.3167185,0x2C,, +3.316814,0x2A,, +3.316909375,0xB8,, +3.317004875,0x00,, +3.317100375,0xD5,, +3.317195875,0x42,, +3.31729125,0xAB,, +3.31738675,0x80,, +3.31748225,0x0D,, +3.317577625,0x54,, +3.317673125,0x80,, +3.317768625,0x08,, +3.317864125,0x00,, +3.3179595,0x2A,, +3.318055,0x62,, +3.3181505,0xA6,, +3.318246,0xCF,, +3.33587275,0x55,, +3.335968125,0x55,, +3.336063625,0x18,, +3.336159125,0x00,, +3.336254625,0x59,, +3.33635,0x06,, +3.3364455,0xDC,, +3.336541,0x00,, +3.336636375,0x2C,, +3.336731875,0x76,, +3.336827375,0x0A,, +3.336922875,0x2A,, +3.337018375,0xB8,, +3.33711375,0x00,, +3.33720925,0xD5,, +3.33730475,0x42,, +3.33740025,0xAB,, +3.337495625,0x80,, +3.337591125,0x0D,, +3.337686625,0x54,, +3.337782,0x80,, +3.3378775,0x08,, +3.337973,0x00,, +3.3380685,0x2A,, +3.338163875,0x62,, +3.338259375,0xA6,, +3.338354875,0x68,, +3.35596425,0x55,, +3.35605975,0x55,, +3.356155125,0x18,, +3.356250625,0x00,, +3.356346125,0x59,, +3.356441625,0xCD,, +3.356537,0xDD,, +3.3566325,0x00,, +3.356728,0x2C,, +3.3568235,0x65,, +3.356918875,0xF6,, +3.357014375,0x2A,, +3.357109875,0xB8,, +3.35720525,0x00,, +3.35730075,0xD5,, +3.35739625,0x42,, +3.35749175,0xAB,, +3.35758725,0x80,, +3.357682625,0x0D,, +3.357778125,0x54,, +3.357873625,0x80,, +3.357969,0x08,, +3.3580645,0x00,, +3.35816,0x2A,, +3.3582555,0x62,, +3.358350875,0xA6,, +3.358446375,0x8E,, +3.3760645,0x55,, +3.376159875,0x55,, +3.376255375,0x18,, +3.376350875,0x00,, +3.376446375,0x5A,, +3.376541875,0x94,, +3.37663725,0xDD,, +3.37673275,0x00,, +3.37682825,0x2C,, +3.376923625,0x65,, +3.377019125,0xE8,, +3.377114625,0x2A,, +3.377210125,0xB8,, +3.3773055,0x00,, +3.377401,0xD5,, +3.3774965,0x42,, +3.377592,0xAB,, +3.377687375,0x80,, +3.377782875,0x0D,, +3.377878375,0x54,, +3.37797375,0x80,, +3.37806925,0x08,, +3.37816475,0x00,, +3.37826025,0x2A,, +3.37835575,0x62,, +3.378451125,0xA6,, +3.378546625,0x41,, +3.396156,0x55,, +3.3962515,0x55,, +3.396346875,0x18,, +3.396442375,0x00,, +3.396537875,0x5B,, +3.396633375,0x5B,, +3.39672875,0xDE,, +3.39682425,0x00,, +3.39691975,0x2C,, +3.39701525,0x65,, +3.39711075,0xDE,, +3.397206125,0x2A,, +3.397301625,0xB8,, +3.397397125,0x00,, +3.3974925,0xD5,, +3.397588,0x42,, +3.3976835,0xAB,, +3.397779,0x80,, +3.397874375,0x0D,, +3.397969875,0x54,, +3.398065375,0x80,, +3.398160875,0x08,, +3.39825625,0x00,, +3.39835175,0x2A,, +3.39844725,0x62,, +3.398542625,0xA6,, +3.398638125,0xD5,, +3.4166555,0x55,, +3.416750875,0x55,, +3.416846375,0x2B,, +3.416941875,0x03,, +3.417037375,0x5C,, +3.41713275,0x20,, +3.41722825,0xDE,, +3.41732375,0x00,, +3.41741925,0x2C,, +3.417514625,0x85,, +3.417610125,0xE1,, +3.417705625,0x2A,, +3.417801,0xB8,, +3.4178965,0x00,, +3.417992,0xD5,, +3.4180875,0x42,, +3.418183,0xAB,, +3.418278375,0x80,, +3.418373875,0x0D,, +3.418469375,0x54,, +3.41856475,0x80,, +3.41866025,0x08,, +3.41875575,0x00,, +3.41885125,0x2A,, +3.418946625,0x62,, +3.419042125,0xA6,, +3.419137625,0x00,, +3.419233125,0x00,, +3.4193285,0x00,, +3.419424,0x00,, +3.4195195,0x00,, +3.419614875,0x00,, +3.419710375,0x00,, +3.419805875,0x00,, +3.419901375,0x00,, +3.41999675,0x00,, +3.42009225,0x00,, +3.42018775,0x00,, +3.42028325,0x00,, +3.42037875,0x00,, +3.420474125,0x00,, +3.420569625,0x00,, +3.420665125,0x00,, +3.4207605,0x00,, +3.420856,0x00,, +3.4209515,0xCD,, +3.4363565,0x55,, +3.436451875,0x55,, +3.436547375,0x18,, +3.436642875,0x00,, +3.436738375,0x5C,, +3.43683375,0xE7,, +3.43692925,0xDC,, +3.43702475,0x00,, +3.437120125,0x2C,, +3.437215625,0x45,, +3.437311125,0xF1,, +3.437406625,0x2A,, +3.437502125,0xB8,, +3.4375975,0x00,, +3.437693,0xD5,, +3.4377885,0x42,, +3.437883875,0xAB,, +3.437979375,0x80,, +3.438074875,0x0D,, +3.438170375,0x54,, +3.43826575,0x80,, +3.43836125,0x08,, +3.43845675,0x00,, +3.43855225,0x2A,, +3.438647625,0x62,, +3.438743125,0xA6,, +3.438838625,0x03,, +3.456456625,0x55,, +3.456552125,0x55,, +3.456647625,0x18,, +3.456743,0x00,, +3.4568385,0x5D,, +3.456934,0xAE,, +3.4570295,0xDC,, +3.457125,0x00,, +3.457220375,0x2C,, +3.457315875,0x66,, +3.457411375,0x1B,, +3.45750675,0x2A,, +3.45760225,0xB8,, +3.45769775,0x00,, +3.45779325,0xD5,, +3.457888625,0x42,, +3.457984125,0xAB,, +3.458079625,0x80,, +3.458175125,0x0D,, +3.4582705,0x54,, +3.458366,0x80,, +3.4584615,0x08,, +3.458557,0x00,, +3.458652375,0x2A,, +3.458747875,0x62,, +3.458843375,0xA6,, +3.458938875,0x36,, +3.47654825,0x55,, +3.476643625,0x55,, +3.476739125,0x18,, +3.476834625,0x00,, +3.476930125,0x5E,, +3.4770255,0x74,, +3.477121,0xDE,, +3.4772165,0x00,, +3.477311875,0x2C,, +3.477407375,0x66,, +3.477502875,0x85,, +3.477598375,0x2A,, +3.477693875,0xB8,, +3.47778925,0x00,, +3.47788475,0xD5,, +3.47798025,0x42,, +3.478075625,0xAB,, +3.478171125,0x80,, +3.478266625,0x0D,, +3.478362125,0x54,, +3.4784575,0x80,, +3.478553,0x08,, +3.4786485,0x00,, +3.478744,0x2A,, +3.478839375,0x62,, +3.478934875,0xA6,, +3.479030375,0xC4,, +3.4966485,0x55,, +3.496743875,0x55,, +3.496839375,0x18,, +3.496934875,0x00,, +3.49703025,0x5F,, +3.49712575,0x3B,, +3.49722125,0xDE,, +3.49731675,0x00,, +3.497412125,0x2C,, +3.497507625,0x67,, +3.497603125,0x00,, +3.497698625,0x2A,, +3.497794,0xB8,, +3.4978895,0x00,, +3.497985,0xD5,, +3.498080375,0x42,, +3.498175875,0xAB,, +3.498271375,0x80,, +3.498366875,0x0D,, +3.498462375,0x54,, +3.49855775,0x80,, +3.49865325,0x08,, +3.49874875,0x00,, +3.498844125,0x2A,, +3.498939625,0x62,, +3.499035125,0xA6,, +3.499130625,0x62,, +3.51674,0x55,, +3.516835375,0x55,, +3.516930875,0x18,, +3.517026375,0x00,, +3.517121875,0x60,, +3.51721725,0x01,, +3.51731275,0xDC,, +3.51740825,0x00,, +3.51750375,0x2C,, +3.517599125,0x47,, +3.517694625,0x1E,, +3.517790125,0x2F,, +3.517885625,0x28,, +3.517981,0x00,, +3.5180765,0xD5,, +3.518172,0x42,, +3.5182675,0xAB,, +3.518362875,0x80,, +3.518458375,0x0D,, +3.518553875,0x54,, +3.51864925,0x80,, +3.51874475,0x08,, +3.51884025,0x00,, +3.51893575,0x2A,, +3.51903125,0x62,, +3.519126625,0xA6,, +3.519222125,0x76,, +3.536848875,0x55,, +3.536944375,0x55,, +3.53703975,0x18,, +3.53713525,0x00,, +3.53723075,0x60,, +3.53732625,0xC7,, +3.537421625,0xDE,, +3.537517125,0x00,, +3.537612625,0x2C,, +3.537708125,0x47,, +3.5378035,0x1E,, +3.537899,0x2F,, +3.5379945,0x28,, +3.53809,0x00,, +3.538185375,0xD5,, +3.538280875,0x42,, +3.538376375,0xAB,, +3.53847175,0x80,, +3.53856725,0x0D,, +3.53866275,0x54,, +3.53875825,0x80,, +3.538853625,0x08,, +3.538949125,0x00,, +3.539044625,0x2A,, +3.539140125,0x62,, +3.5392355,0xA6,, +3.539331,0x06,, +3.556940375,0x55,, +3.557035875,0x55,, +3.557131375,0x18,, +3.55722675,0x00,, +3.55732225,0x61,, +3.55741775,0x8D,, +3.55751325,0xDE,, +3.55760875,0x00,, +3.557704125,0x2C,, +3.557799625,0x46,, +3.557895125,0xFC,, +3.5579905,0x54,, +3.558086,0xD8,, +3.5581815,0x00,, +3.558277,0xD5,, +3.558372375,0x42,, +3.558467875,0xAB,, +3.558563375,0x80,, +3.558658875,0x0D,, +3.55875425,0x54,, +3.55884975,0x80,, +3.55894525,0x08,, +3.559040625,0x00,, +3.559136125,0x2A,, +3.559231625,0x62,, +3.559327125,0xA6,, +3.559422625,0x40,, +3.577040625,0x55,, +3.577136125,0x55,, +3.577231625,0x18,, +3.577327,0x00,, +3.5774225,0x62,, +3.577518,0x53,, +3.577613375,0xDC,, +3.577708875,0x00,, +3.577804375,0x2C,, +3.577899875,0x57,, +3.57799525,0x10,, +3.57809075,0x6B,, +3.57818625,0x38,, +3.57828175,0x00,, +3.578377125,0xD5,, +3.578472625,0x42,, +3.578568125,0xAB,, +3.5786635,0x80,, +3.578759,0x0D,, +3.5788545,0x54,, +3.57895,0x80,, +3.5790455,0x08,, +3.579140875,0x00,, +3.579236375,0x2A,, +3.579331875,0x62,, +3.579427375,0xA6,, +3.57952275,0x71,, +3.597140875,0x55,, +3.597236375,0x55,, +3.59733175,0x18,, +3.59742725,0x00,, +3.59752275,0x63,, +3.597618125,0x1A,, +3.597713625,0xDE,, +3.597809125,0x00,, +3.597904625,0x2C,, +3.598,0x67,, +3.5980955,0x62,, +3.598191,0x80,, +3.5982865,0xB8,, +3.598381875,0x00,, +3.598477375,0xD5,, +3.598572875,0x42,, +3.598668375,0xAB,, +3.59876375,0x80,, +3.59885925,0x0D,, +3.59895475,0x54,, +3.59905025,0x80,, +3.599145625,0x08,, +3.599241125,0x00,, +3.599336625,0x2A,, +3.599432,0x62,, +3.5995275,0xA6,, +3.599623,0x2B,, +3.61764025,0x55,, +3.61773575,0x55,, +3.61783125,0x2B,, +3.617926625,0x03,, +3.618022125,0x63,, +3.618117625,0xE0,, +3.618213125,0xDE,, +3.618308625,0x00,, +3.618404,0x2C,, +3.6184995,0x77,, +3.618595,0xBF,, +3.618690375,0x95,, +3.618785875,0xD8,, +3.618881375,0x00,, +3.618976875,0xD5,, +3.61907225,0x42,, +3.61916775,0xAB,, +3.61926325,0x80,, +3.61935875,0x0D,, +3.619454125,0x54,, +3.619549625,0x80,, +3.619645125,0x08,, +3.619740625,0x00,, +3.619836,0x2A,, +3.6199315,0x62,, +3.620027,0xA6,, +3.6201225,0x00,, +3.620217875,0x00,, +3.620313375,0x00,, +3.620408875,0x00,, +3.620504375,0x00,, +3.62059975,0x00,, +3.62069525,0x00,, +3.62079075,0x00,, +3.620886125,0x00,, +3.620981625,0x00,, +3.621077125,0x00,, +3.621172625,0x00,, +3.621268,0x00,, +3.6213635,0x00,, +3.621459,0x00,, +3.6215545,0x00,, +3.621649875,0x00,, +3.621745375,0x00,, +3.621840875,0x00,, +3.621936375,0xFC,, +3.637332625,0x55,, +3.637428125,0x55,, +3.6375235,0x18,, +3.637619,0x00,, +3.6377145,0x64,, +3.637809875,0xA7,, +3.637905375,0xDD,, +3.638000875,0x00,, +3.638096375,0x2C,, +3.638191875,0x48,, +3.63828725,0x00,, +3.63838275,0xAC,, +3.63847825,0xA8,, +3.638573625,0x00,, +3.638669125,0xD5,, +3.638764625,0x42,, +3.638860125,0xAB,, +3.6389555,0x80,, +3.639051,0x0D,, +3.6391465,0x54,, +3.639242,0x80,, +3.639337375,0x08,, +3.639432875,0x00,, +3.639528375,0x2A,, +3.639623875,0x62,, +3.63971925,0xA6,, +3.63981475,0x04,, +3.657432875,0x55,, +3.65752825,0x55,, +3.65762375,0x18,, +3.65771925,0x00,, +3.65781475,0x65,, +3.657910125,0x6C,, +3.658005625,0xDD,, +3.658101125,0x00,, +3.658196625,0x2C,, +3.658292,0x78,, +3.6583875,0x0A,, +3.658483,0xC1,, +3.658578375,0x88,, +3.658673875,0x00,, +3.658769375,0xD5,, +3.658864875,0x42,, +3.65896025,0xAB,, +3.65905575,0x80,, +3.65915125,0x0D,, +3.65924675,0x54,, +3.659342125,0x80,, +3.659437625,0x08,, +3.659533125,0x00,, +3.659628625,0x2A,, +3.659724,0x62,, +3.6598195,0xA6,, +3.659915,0xCB,, +3.677524375,0x55,, +3.677619875,0x55,, +3.677715375,0x18,, +3.67781075,0x00,, +3.67790625,0x66,, +3.67800175,0x31,, +3.678097125,0xDC,, +3.678192625,0x00,, +3.678288125,0x2C,, +3.678383625,0x48,, +3.678479,0x23,, +3.6785745,0xD4,, +3.67867,0x88,, +3.6787655,0x00,, +3.678860875,0xD5,, +3.678956375,0x42,, +3.679051875,0xAB,, +3.67914725,0x80,, +3.67924275,0x0D,, +3.67933825,0x54,, +3.67943375,0x80,, +3.67952925,0x08,, +3.679624625,0x00,, +3.679720125,0x2A,, +3.679815625,0x62,, +3.679911,0xA6,, +3.6800065,0x76,, +3.697624625,0x55,, +3.69772,0x55,, +3.6978155,0x18,, +3.697911,0x00,, +3.6980065,0x66,, +3.698101875,0xF8,, +3.698197375,0xDC,, +3.698292875,0x00,, +3.698388375,0x2C,, +3.69848375,0x48,, +3.69857925,0x00,, +3.69867475,0xD5,, +3.698770125,0x08,, +3.698865625,0x00,, +3.698961125,0xD5,, +3.699056625,0x42,, +3.699152125,0xAB,, +3.6992475,0x80,, +3.699343,0x0D,, +3.6994385,0x54,, +3.699534,0x80,, +3.699629375,0x08,, +3.699724875,0x00,, +3.699820375,0x2A,, +3.69991575,0x62,, +3.70001125,0xA6,, +3.70010675,0x14,, +3.71772475,0x55,, +3.71782025,0x55,, +3.71791575,0x18,, +3.71801125,0x00,, +3.718106625,0x67,, +3.718202125,0xBE,, +3.718297625,0xDD,, +3.718393125,0x00,, +3.7184885,0x2C,, +3.718584,0x77,, +3.7186795,0xDA,, +3.718775,0xD5,, +3.718870375,0x08,, +3.718965875,0x00,, +3.719061375,0xD5,, +3.719156875,0x42,, +3.71925225,0xAB,, +3.71934775,0x80,, +3.71944325,0x0D,, +3.719538625,0x54,, +3.719634125,0x80,, +3.719729625,0x08,, +3.719825125,0x00,, +3.7199205,0x2A,, +3.720016,0x62,, +3.7201115,0xA6,, +3.720207,0xA5,, +3.737825,0x55,, +3.7379205,0x55,, +3.738016,0x18,, +3.738111375,0x00,, +3.738206875,0x68,, +3.738302375,0x84,, +3.738397875,0xDD,, +3.73849325,0x00,, +3.73858875,0x2C,, +3.73868425,0x47,, +3.73877975,0xC2,, +3.738875125,0xD5,, +3.738970625,0x08,, +3.739066125,0x00,, +3.739161625,0xD5,, +3.739257,0x42,, +3.7393525,0xAB,, +3.739448,0x80,, +3.739543375,0x0D,, +3.739638875,0x54,, +3.739734375,0x80,, +3.739829875,0x08,, +3.739925375,0x00,, +3.74002075,0x2A,, +3.74011625,0x62,, +3.74021175,0xA6,, +3.740307125,0x84,, +3.7579165,0x55,, +3.758012,0x55,, +3.7581075,0x18,, +3.758203,0x00,, +3.7582985,0x69,, +3.758393875,0x4A,, +3.758489375,0xDC,, +3.758584875,0x00,, +3.75868025,0x2C,, +3.75877575,0x47,, +3.75887125,0xC2,, +3.75896675,0xD4,, +3.759062125,0xF8,, +3.759157625,0x00,, +3.759253125,0xD5,, +3.759348625,0x42,, +3.759444,0xAB,, +3.7595395,0x80,, +3.759635,0x0D,, +3.759730375,0x54,, +3.759825875,0x80,, +3.759921375,0x08,, +3.760016875,0x00,, +3.760112375,0x2A,, +3.76020775,0x62,, +3.76030325,0xA6,, +3.76039875,0xED,, +3.77801675,0x55,, +3.77811225,0x55,, +3.77820775,0x18,, +3.77830325,0x00,, +3.778398625,0x6A,, +3.778494125,0x0F,, +3.778589625,0xDE,, +3.778685,0x00,, +3.7787805,0x2C,, +3.778876,0x57,, +3.7789715,0xD1,, +3.779066875,0xD5,, +3.779162375,0x08,, +3.779257875,0x00,, +3.779353375,0xD5,, +3.77944875,0x42,, +3.77954425,0xAB,, +3.77963975,0x80,, +3.77973525,0x0D,, +3.779830625,0x54,, +3.779926125,0x80,, +3.780021625,0x08,, +3.780117125,0x00,, +3.7802125,0x2A,, +3.780308,0x62,, +3.7804035,0xA6,, +3.780498875,0xC2,, +3.798117,0x55,, +3.7982125,0x55,, +3.798307875,0x18,, +3.798403375,0x00,, +3.798498875,0x6A,, +3.798594375,0xD6,, +3.79868975,0xDC,, +3.79878525,0x00,, +3.79888075,0x2C,, +3.79897625,0x67,, +3.79907175,0xDC,, +3.799167125,0xD4,, +3.799262625,0xE8,, +3.799358125,0x00,, +3.7994535,0xD5,, +3.799549,0x42,, +3.7996445,0xAB,, +3.79974,0x80,, +3.799835375,0x0D,, +3.799930875,0x54,, +3.800026375,0x80,, +3.800121875,0x08,, +3.80021725,0x00,, +3.80031275,0x2A,, +3.80040825,0x62,, +3.800503625,0xA6,, +3.800599125,0xFC,, +3.818616375,0x55,, +3.818711875,0x55,, +3.818807375,0x2B,, +3.818902875,0x03,, +3.818998375,0x6B,, +3.81909375,0x9D,, +3.81918925,0xDE,, +3.81928475,0x00,, +3.81938025,0x2C,, +3.819475625,0x57,, +3.819571125,0xDC,, +3.819666625,0xD4,, +3.819762,0xF8,, +3.8198575,0x00,, +3.819953,0xD5,, +3.8200485,0x42,, +3.820143875,0xAB,, +3.820239375,0x80,, +3.820334875,0x0D,, +3.820430375,0x54,, +3.82052575,0x80,, +3.82062125,0x08,, +3.82071675,0x00,, +3.82081225,0x2A,, +3.820907625,0x62,, +3.821003125,0xA6,, +3.821098625,0x00,, +3.821194125,0x00,, +3.8212895,0x00,, +3.821385,0x00,, +3.8214805,0x00,, +3.821575875,0x00,, +3.821671375,0x00,, +3.821766875,0x00,, +3.821862375,0x00,, +3.82195775,0x00,, +3.82205325,0x00,, +3.82214875,0x00,, +3.82224425,0x00,, +3.82233975,0x00,, +3.822435125,0x00,, +3.822530625,0x00,, +3.822626125,0x00,, +3.8227215,0x00,, +3.822817,0x00,, +3.8229125,0x9E,, +3.8383175,0x55,, +3.838412875,0x55,, +3.838508375,0x18,, +3.838603875,0x00,, +3.838699375,0x6C,, +3.83879475,0x63,, +3.83889025,0xDC,, +3.83898575,0x00,, +3.839081125,0x2C,, +3.839176625,0x77,, +3.839272125,0xE2,, +3.839367625,0xD4,, +3.839463125,0xE8,, +3.8395585,0x00,, +3.839654,0xD5,, +3.8397495,0x42,, +3.839844875,0xAB,, +3.839940375,0x80,, +3.840035875,0x0D,, +3.840131375,0x54,, +3.84022675,0x80,, +3.84032225,0x08,, +3.84041775,0x00,, +3.84051325,0x2A,, +3.840608625,0x62,, +3.840704125,0xA6,, +3.840799625,0xD3,, +3.858409,0x55,, +3.8585045,0x55,, +3.858599875,0x18,, +3.858695375,0x00,, +3.858790875,0x6D,, +3.858886375,0x29,, +3.85898175,0xDB,, +3.85907725,0x00,, +3.85917275,0x2C,, +3.859268125,0x67,, +3.859363625,0xED,, +3.859459125,0xD4,, +3.859554625,0xE8,, +3.85965,0x00,, +3.8597455,0xD5,, +3.859841,0x42,, +3.8599365,0xAB,, +3.860032,0x80,, +3.860127375,0x0D,, +3.860222875,0x54,, +3.860318375,0x80,, +3.86041375,0x08,, +3.86050925,0x00,, +3.86060475,0x2A,, +3.86070025,0x62,, +3.860795625,0xA6,, +3.860891125,0x91,, +3.87850925,0x55,, +3.878604625,0x55,, +3.878700125,0x18,, +3.878795625,0x00,, +3.878891125,0x6D,, +3.8789865,0xEE,, +3.879082,0xDE,, +3.8791775,0x00,, +3.879272875,0x2C,, +3.879368375,0x67,, +3.879463875,0xF2,, +3.879559375,0xD4,, +3.879654875,0x88,, +3.87975025,0x00,, +3.87984575,0xD5,, +3.87994125,0x42,, +3.880036625,0xAB,, +3.880132125,0x80,, +3.880227625,0x0D,, +3.880323125,0x54,, +3.8804185,0x80,, +3.880514,0x08,, +3.8806095,0x00,, +3.880705,0x2A,, +3.880800375,0x62,, +3.880895875,0xA6,, +3.880991375,0xC4,, +3.8986095,0x55,, +3.898704875,0x55,, +3.898800375,0x18,, +3.898895875,0x00,, +3.89899125,0x6E,, +3.89908675,0xB4,, +3.89918225,0xDD,, +3.89927775,0x00,, +3.899373125,0x2C,, +3.899468625,0x67,, +3.899564125,0xD5,, +3.899659625,0xD0,, +3.899755,0xC8,, +3.8998505,0x00,, +3.899946,0xD5,, +3.900041375,0x42,, +3.900136875,0xAB,, +3.900232375,0x80,, +3.900327875,0x0D,, +3.90042325,0x54,, +3.90051875,0x80,, +3.90061425,0x08,, +3.90070975,0x00,, +3.900805125,0x2A,, +3.900900625,0x62,, +3.900996125,0xA6,, +3.901091625,0x73,, +3.918709625,0x55,, +3.918805125,0x55,, +3.918900625,0x18,, +3.918996,0x00,, +3.9190915,0x6F,, +3.919187,0x7A,, +3.9192825,0xDD,, +3.919377875,0x00,, +3.919473375,0x2C,, +3.919568875,0x67,, +3.91966425,0x6F,, +3.91975975,0xC8,, +3.91985525,0x78,, +3.91995075,0x00,, +3.92004625,0xD5,, +3.920141625,0x42,, +3.920237125,0xAB,, +3.920332625,0x80,, +3.920428125,0x0D,, +3.9205235,0x54,, +3.920619,0x80,, +3.9207145,0x08,, +3.920809875,0x00,, +3.920905375,0x2A,, +3.921000875,0x62,, +3.921096375,0xA6,, +3.92119175,0xCD,, +3.938809875,0x55,, +3.938905375,0x55,, +3.93900075,0x18,, +3.93909625,0x00,, +3.93919175,0x70,, +3.93928725,0x40,, +3.939382625,0xDE,, +3.939478125,0x00,, +3.939573625,0x2C,, +3.939669125,0x66,, +3.9397645,0xCB,, +3.93986,0xBB,, +3.9399555,0x38,, +3.940051,0x00,, +3.940146375,0xD5,, +3.940241875,0x42,, +3.940337375,0xAB,, +3.94043275,0x80,, +3.94052825,0x0D,, +3.94062375,0x54,, +3.94071925,0x80,, +3.940814625,0x08,, +3.940910125,0x00,, +3.941005625,0x2A,, +3.941101125,0x62,, +3.9411965,0xA6,, +3.941292,0xE2,, +3.958910125,0x55,, +3.9590055,0x55,, +3.959101,0x18,, +3.9591965,0x00,, +3.959292,0x71,, +3.959387375,0x07,, +3.959482875,0xDC,, +3.959578375,0x00,, +3.959673875,0x2C,, +3.95976925,0x86,, +3.95986475,0x06,, +3.95996025,0xAA,, +3.96005575,0x58,, +3.960151125,0x00,, +3.960246625,0xD5,, +3.960342125,0x42,, +3.9604375,0xAB,, +3.960533,0x80,, +3.9606285,0x0D,, +3.960724,0x54,, +3.9608195,0x80,, +3.960914875,0x08,, +3.961010375,0x00,, +3.961105875,0x2A,, +3.96120125,0x62,, +3.96129675,0xA6,, +3.96139225,0x24,, +3.979001625,0x55,, +3.979097125,0x55,, +3.979192625,0x18,, +3.979288,0x00,, +3.9793835,0x71,, +3.979479,0xCE,, +3.979574375,0xDD,, +3.979669875,0x00,, +3.979765375,0x2C,, +3.979860875,0x65,, +3.97995625,0x3C,, +3.98005175,0x99,, +3.98014725,0x38,, +3.98024275,0x00,, +3.980338125,0xD5,, +3.980433625,0x42,, +3.980529125,0xAB,, +3.9806245,0x80,, +3.98072,0x0D,, +3.9808155,0x54,, +3.980911,0x80,, +3.9810065,0x08,, +3.981101875,0x00,, +3.981197375,0x2A,, +3.981292875,0x62,, +3.981388375,0xA6,, +3.98148375,0x1D,, +3.999101875,0x55,, +3.999197375,0x55,, +3.99929275,0x18,, +3.99938825,0x00,, +3.99948375,0x72,, +3.999579125,0x94,, +3.999674625,0xDC,, +3.999770125,0x00,, +3.999865625,0x2C,, +3.999961,0x34,, +4.0000565,0x97,, +4.000152,0x8A,, +4.0002475,0x98,, +4.000342875,0x00,, +4.000438375,0xD5,, +4.000533875,0x42,, +4.000629375,0xAB,, +4.00072475,0x80,, +4.00082025,0x0D,, +4.00091575,0x54,, +4.00101125,0x80,, +4.001106625,0x08,, +4.001202125,0x00,, +4.001297625,0x2A,, +4.001393,0x62,, +4.0014885,0xA6,, +4.001584,0x2B,, +4.01960125,0x55,, +4.01969675,0x55,, +4.01979225,0x2B,, +4.019887625,0x03,, +4.019983125,0x73,, +4.020078625,0x5B,, +4.020174125,0xDC,, +4.020269625,0x00,, +4.020365,0x2C,, +4.0204605,0x74,, +4.020556,0x06,, +4.020651375,0x80,, +4.020746875,0x38,, +4.020842375,0x00,, +4.020937875,0xD5,, +4.02103325,0x42,, +4.02112875,0xAB,, +4.02122425,0x80,, +4.02131975,0x0D,, +4.021415125,0x54,, +4.021510625,0x80,, +4.021606125,0x08,, +4.021701625,0x00,, +4.021797,0x2A,, +4.0218925,0x62,, +4.021988,0xA6,, +4.0220835,0x00,, +4.022178875,0x00,, +4.022274375,0x00,, +4.022369875,0x00,, +4.022465375,0x00,, +4.02256075,0x00,, +4.02265625,0x00,, +4.02275175,0x00,, +4.022847125,0x00,, +4.022942625,0x00,, +4.023038125,0x00,, +4.023133625,0x00,, +4.023229,0x00,, +4.0233245,0x00,, +4.02342,0x00,, +4.0235155,0x00,, +4.023610875,0x00,, +4.023706375,0x00,, +4.023801875,0x00,, +4.02389725,0x68,, +4.039293625,0x55,, +4.039389125,0x55,, +4.0394845,0x18,, +4.03958,0x00,, +4.0396755,0x74,, +4.039770875,0x21,, +4.039866375,0xDD,, +4.039961875,0x00,, +4.040057375,0x2C,, +4.040152875,0x63,, +4.04024825,0x5E,, +4.04034375,0x7E,, +4.04043925,0xE8,, +4.040534625,0x00,, +4.040630125,0xD5,, +4.040725625,0x42,, +4.040821125,0xAB,, +4.0409165,0x80,, +4.041012,0x0D,, +4.0411075,0x54,, +4.041203,0x80,, +4.041298375,0x08,, +4.041393875,0x00,, +4.041489375,0x2A,, +4.041584875,0x62,, +4.04168025,0xA6,, +4.04177575,0xF8,, +4.059393875,0x55,, +4.05948925,0x55,, +4.05958475,0x18,, +4.05968025,0x00,, +4.05977575,0x74,, +4.059871125,0xE7,, +4.059966625,0xDD,, +4.060062125,0x00,, +4.060157625,0x2C,, +4.060253,0x42,, +4.0603485,0xC3,, +4.060444,0x7C,, +4.060539375,0xF8,, +4.060634875,0x00,, +4.060730375,0xD5,, +4.060825875,0x42,, +4.06092125,0xAB,, +4.06101675,0x80,, +4.06111225,0x0D,, +4.06120775,0x54,, +4.061303125,0x80,, +4.061398625,0x08,, +4.061494125,0x00,, +4.0615895,0x2A,, +4.061685,0x62,, +4.0617805,0xA6,, +4.061876,0x33,, +4.079494,0x55,, +4.0795895,0x55,, +4.079685,0x18,, +4.0797805,0x00,, +4.079875875,0x75,, +4.079971375,0xAC,, +4.080066875,0xDD,, +4.08016225,0x00,, +4.08025775,0x2C,, +4.08035325,0x42,, +4.08044875,0xAB,, +4.080544125,0x86,, +4.080639625,0x08,, +4.080735125,0x00,, +4.080830625,0xD5,, +4.080926125,0x42,, +4.0810215,0xAB,, +4.081117,0x80,, +4.0812125,0x0D,, +4.081307875,0x54,, +4.081403375,0x80,, +4.081498875,0x08,, +4.081594375,0x00,, +4.08168975,0x2A,, +4.08178525,0x62,, +4.08188075,0xA6,, +4.08197625,0x18,, +4.09959425,0x55,, +4.09968975,0x55,, +4.09978525,0x18,, +4.099880625,0x00,, +4.099976125,0x76,, +4.100071625,0x73,, +4.100167,0xDC,, +4.1002625,0x00,, +4.100358,0x2C,, +4.1004535,0x62,, +4.100549,0xAB,, +4.100644375,0x9B,, +4.100739875,0xC8,, +4.100835375,0x00,, +4.10093075,0xD5,, +4.10102625,0x42,, +4.10112175,0xAB,, +4.10121725,0x80,, +4.101312625,0x0D,, +4.101408125,0x54,, +4.101503625,0x80,, +4.101599125,0x08,, +4.1016945,0x00,, +4.10179,0x2A,, +4.1018855,0x62,, +4.101980875,0xA6,, +4.102076375,0x34,, +4.11968575,0x55,, +4.11978125,0x55,, +4.11987675,0x18,, +4.11997225,0x00,, +4.120067625,0x77,, +4.120163125,0x39,, +4.120258625,0xDD,, +4.120354125,0x00,, +4.1204495,0x2C,, +4.120545,0x52,, +4.1206405,0xAB,, +4.120736,0xB1,, +4.120831375,0x68,, +4.120926875,0x00,, +4.121022375,0xD5,, +4.121117875,0x42,, +4.12121325,0xAB,, +4.12130875,0x80,, +4.12140425,0x0D,, +4.121499625,0x54,, +4.121595125,0x80,, +4.121690625,0x08,, +4.121786125,0x00,, +4.1218815,0x2A,, +4.121977,0x62,, +4.1220725,0xA6,, +4.122168,0x76,, +4.139786,0x55,, +4.1398815,0x55,, +4.139977,0x18,, +4.140072375,0x00,, +4.140167875,0x78,, +4.140263375,0x00,, +4.140358875,0xDD,, +4.14045425,0x00,, +4.14054975,0x2C,, +4.14064525,0x62,, +4.14074075,0xAB,, +4.140836125,0xC6,, +4.140931625,0x18,, +4.141027125,0x00,, +4.141122625,0xD5,, +4.141218,0x42,, +4.1413135,0xAB,, +4.141409,0x80,, +4.141504375,0x0D,, +4.141599875,0x54,, +4.141695375,0x80,, +4.141790875,0x08,, +4.141886375,0x00,, +4.14198175,0x2A,, +4.14207725,0x62,, +4.14217275,0xA6,, +4.142268125,0x6F,, +4.160120625,0x55,, +4.160216,0x55,, +4.1603115,0x18,, +4.160407,0x00,, +4.160502375,0x78,, +4.160597875,0xC7,, +4.160693375,0xDD,, +4.160788875,0x00,, +4.160884375,0x2C,, +4.16097975,0x42,, +4.16107525,0xB6,, +4.16117075,0xD4,, +4.161266125,0x18,, +4.161361625,0x00,, +4.161457125,0xD5,, +4.161552625,0x42,, +4.161648,0xAB,, +4.1617435,0x80,, +4.161839,0x0D,, +4.1619345,0x54,, +4.162029875,0x80,, +4.162125375,0x08,, +4.162220875,0x00,, +4.162316375,0x2A,, +4.16241175,0x62,, +4.16250725,0xA6,, +4.16260275,0x72,, +4.18016,0x55,, +4.1802555,0x55,, +4.180351,0x18,, +4.180446375,0x00,, +4.180541875,0x79,, +4.180637375,0x8B,, +4.180732875,0xDC,, +4.180828375,0x00,, +4.18092375,0x2C,, +4.18101925,0x62,, +4.18111475,0xB6,, +4.181210125,0xD4,, +4.181305625,0x58,, +4.181401125,0x00,, +4.181496625,0xD5,, +4.181592,0x42,, +4.1816875,0xAB,, +4.181783,0x80,, +4.1818785,0x0D,, +4.181973875,0x54,, +4.182069375,0x80,, +4.182164875,0x08,, +4.18226025,0x00,, +4.18235575,0x2A,, +4.18245125,0x62,, +4.18254675,0xA6,, +4.18264225,0x98,, +4.201223625,0x55,, +4.201319125,0x55,, +4.2014145,0x18,, +4.20151,0x00,, +4.2016055,0x7A,, +4.201701,0x5B,, +4.201796375,0xDC,, +4.201891875,0x00,, +4.201987375,0x2C,, +4.202082875,0x42,, +4.20217825,0xD7,, +4.20227375,0xD4,, +4.20236925,0x48,, +4.202464625,0x00,, +4.202560125,0xD5,, +4.202655625,0x42,, +4.202751125,0xAB,, +4.202846625,0x80,, +4.202942,0x0D,, +4.2030375,0x54,, +4.203133,0x80,, +4.203228375,0x08,, +4.203323875,0x00,, +4.203419375,0x2A,, +4.203514875,0x62,, +4.20361025,0xA6,, +4.20370575,0xCB,, +4.221289125,0x55,, +4.221384625,0x55,, +4.22148,0x2B,, +4.2215755,0x03,, +4.221671,0x7B,, +4.2217665,0x1D,, +4.221861875,0xDE,, +4.221957375,0x00,, +4.222052875,0x2C,, +4.222148375,0x63,, +4.22224375,0x4E,, +4.22233925,0xD4,, +4.22243475,0x28,, +4.22253025,0x00,, +4.222625625,0xD5,, +4.222721125,0x42,, +4.222816625,0xAB,, +4.222912,0x80,, +4.2230075,0x0D,, +4.223103,0x54,, +4.2231985,0x80,, +4.223293875,0x08,, +4.223389375,0x00,, +4.223484875,0x2A,, +4.223580375,0x62,, +4.223675875,0xA6,, +4.22377125,0x00,, +4.22386675,0x00,, +4.22396225,0x00,, +4.224057625,0x00,, +4.224153125,0x00,, +4.224248625,0x00,, +4.224344125,0x00,, +4.2244395,0x00,, +4.224535,0x00,, +4.2246305,0x00,, +4.224726,0x00,, +4.224821375,0x00,, +4.224916875,0x00,, +4.225012375,0x00,, +4.22510775,0x00,, +4.22520325,0x00,, +4.22529875,0x00,, +4.22539425,0x00,, +4.225489625,0x00,, +4.225585125,0xB8,, +4.240408625,0x55,, +4.240504125,0x55,, +4.240599625,0x18,, +4.240695,0x00,, +4.2407905,0x7B,, +4.240886,0xDD,, +4.240981375,0xCD,, +4.241076875,0x00,, +4.241172375,0x2C,, +4.241267875,0x63,, +4.24136325,0x4E,, +4.24145875,0xD4,, +4.24155425,0x28,, +4.24164975,0x00,, +4.241745125,0xD5,, +4.241840625,0x42,, +4.241936125,0xAB,, +4.2420315,0x80,, +4.242127,0x0D,, +4.2422225,0x54,, +4.242318,0x80,, +4.2424135,0x08,, +4.242508875,0x00,, +4.242604375,0x2A,, +4.242699875,0x62,, +4.24279525,0xA6,, +4.24289075,0x23,, +4.260543625,0x55,, +4.260639,0x55,, +4.2607345,0x18,, +4.26083,0x00,, +4.260925375,0x7C,, +4.261020875,0xA4,, +4.261116375,0xCD,, +4.261211875,0x00,, +4.26130725,0x2C,, +4.26140275,0x63,, +4.26149825,0xEE,, +4.26159375,0xD4,, +4.261689125,0x58,, +4.261784625,0x00,, +4.261880125,0xD5,, +4.261975625,0x42,, +4.262071,0xAB,, +4.2621665,0x80,, +4.262262,0x0D,, +4.2623575,0x54,, +4.262452875,0x80,, +4.262548375,0x08,, +4.262643875,0x00,, +4.26273925,0x2A,, +4.26283475,0x62,, +4.26293025,0xA6,, +4.26302575,0x71,, +4.280513625,0x55,, +4.280609,0x55,, +4.2807045,0x18,, +4.2808,0x00,, +4.2808955,0x7D,, +4.280991,0x6B,, +4.281086375,0xCD,, +4.281181875,0x00,, +4.281277375,0x2C,, +4.28137275,0x64,, +4.28146825,0xB3,, +4.28156375,0xD3,, +4.28165925,0xD8,, +4.281754625,0x00,, +4.281850125,0xD5,, +4.281945625,0x42,, +4.282041125,0xAB,, +4.2821365,0x80,, +4.282232,0x0D,, +4.2823275,0x54,, +4.282422875,0x80,, +4.282518375,0x08,, +4.282613875,0x00,, +4.282709375,0x2A,, +4.282804875,0x62,, +4.28290025,0xA6,, +4.28299575,0xB6,, +4.300700625,0x55,, +4.300796125,0x55,, +4.3008915,0x18,, +4.300987,0x00,, +4.3010825,0x7E,, +4.301177875,0x31,, +4.301273375,0xBB,, +4.301368875,0x00,, +4.301464375,0x2C,, +4.301559875,0x65,, +4.30165525,0xB8,, +4.30175075,0xCB,, +4.30184625,0xE8,, +4.301941625,0x00,, +4.302037125,0xD5,, +4.302132625,0x42,, +4.302228125,0xAB,, +4.3023235,0x80,, +4.302419,0x0D,, +4.3025145,0x54,, +4.30261,0x80,, +4.302705375,0x08,, +4.302800875,0x00,, +4.302896375,0x2A,, +4.30299175,0x62,, +4.30308725,0xA6,, +4.30318275,0xC6,, +4.320670625,0x55,, +4.320766125,0x55,, +4.320861625,0x18,, +4.320957,0x00,, +4.3210525,0x7E,, +4.321148,0xF8,, +4.3212435,0xCA,, +4.321338875,0x00,, +4.321434375,0x2C,, +4.321529875,0x46,, +4.32162525,0xB6,, +4.32172075,0xC5,, +4.32181625,0x98,, +4.32191175,0x00,, +4.322007125,0xD5,, +4.322102625,0x42,, +4.322198125,0xAB,, +4.322293625,0x80,, +4.322389125,0x0D,, +4.3224845,0x54,, +4.32258,0x80,, +4.3226755,0x08,, +4.322770875,0x00,, +4.322866375,0x2A,, +4.322961875,0x62,, +4.323057375,0xA6,, +4.32315275,0x6A,, +4.340892375,0x55,, +4.340987875,0x55,, +4.34108325,0x18,, +4.34117875,0x00,, +4.34127425,0x7F,, +4.34136975,0xBF,, +4.341465125,0xC1,, +4.341560625,0x00,, +4.341656125,0x2C,, +4.341751625,0x58,, +4.341847,0x00,, +4.3419425,0xBD,, +4.342038,0x78,, +4.342133375,0x00,, +4.342228875,0xD5,, +4.342324375,0x42,, +4.342419875,0xAB,, +4.34251525,0x80,, +4.34261075,0x0D,, +4.34270625,0x54,, +4.34280175,0x80,, +4.34289725,0x08,, +4.342992625,0x00,, +4.343088125,0x2A,, +4.343183625,0x62,, +4.343279,0xA6,, +4.3433745,0x60,, +4.361634875,0x55,, +4.36173025,0x55,, +4.36182575,0x18,, +4.36192125,0x00,, +4.36201675,0x80,, +4.362112125,0x8C,, +4.362207625,0xDC,, +4.362303125,0x00,, +4.3623985,0x2C,, +4.362494,0x89,, +4.3625895,0x36,, +4.362685,0xB3,, +4.3627805,0xD8,, +4.362875875,0x00,, +4.362971375,0xD5,, +4.363066875,0x42,, +4.36316225,0xAB,, +4.36325775,0x80,, +4.36335325,0x0D,, +4.36344875,0x54,, +4.363544125,0x80,, +4.363639625,0x08,, +4.363735125,0x00,, +4.363830625,0x2A,, +4.363926,0x62,, +4.3640215,0xA6,, +4.364117,0xDC,, +4.381934625,0x55,, +4.382030125,0x55,, +4.382125625,0x18,, +4.382221,0x00,, +4.3823165,0x81,, +4.382412,0x55,, +4.3825075,0xDB,, +4.382602875,0x00,, +4.382698375,0x2C,, +4.382793875,0x7B,, +4.382889375,0xAA,, +4.38298475,0xA4,, +4.38308025,0x88,, +4.38317575,0x00,, +4.383271125,0xD5,, +4.383366625,0x42,, +4.383462125,0xAB,, +4.383557625,0x80,, +4.383653125,0x0D,, +4.3837485,0x54,, +4.383844,0x80,, +4.3839395,0x08,, +4.384035,0x00,, +4.384130375,0x2A,, +4.384225875,0x62,, +4.384321375,0xA6,, +4.38441675,0x08,, +4.40210425,0x55,, +4.40219975,0x55,, +4.40229525,0x18,, +4.40239075,0x00,, +4.402486125,0x82,, +4.402581625,0x1D,, +4.402677125,0xDB,, +4.402772625,0x00,, +4.402868,0x2C,, +4.4029635,0x5C,, +4.403059,0xF4,, +4.403154375,0xA4,, +4.403249875,0x78,, +4.403345375,0x00,, +4.403440875,0xD5,, +4.403536375,0x42,, +4.40363175,0xAB,, +4.40372725,0x80,, +4.40382275,0x0D,, +4.40391825,0x54,, +4.404013625,0x80,, +4.404109125,0x08,, +4.404204625,0x00,, +4.4043,0x2A,, +4.4043955,0x62,, +4.404491,0xA6,, +4.4045865,0xB2,, +4.42177925,0x55,, +4.42187475,0x55,, +4.42197025,0x2B,, +4.422065625,0x03,, +4.422161125,0x82,, +4.422256625,0xDB,, +4.422352125,0xDB,, +4.4224475,0x00,, +4.422543,0x2C,, +4.4226385,0x6D,, +4.422733875,0x53,, +4.422829375,0xA0,, +4.422924875,0x28,, +4.423020375,0x00,, +4.42311575,0xD5,, +4.42321125,0x42,, +4.42330675,0xAB,, +4.42340225,0x80,, +4.423497625,0x0D,, +4.423593125,0x54,, +4.423688625,0x80,, +4.423784125,0x08,, +4.4238795,0x00,, +4.423975,0x2A,, +4.4240705,0x62,, +4.424166,0xA6,, +4.424261375,0x00,, +4.424356875,0x00,, +4.424452375,0x00,, +4.42454775,0x00,, +4.42464325,0x00,, +4.42473875,0x00,, +4.42483425,0x00,, +4.424929625,0x00,, +4.425025125,0x00,, +4.425120625,0x00,, +4.425216125,0x00,, +4.425311625,0x00,, +4.425407,0x00,, +4.4255025,0x00,, +4.425598,0x00,, +4.425693375,0x00,, +4.425788875,0x00,, +4.425884375,0x00,, +4.425979875,0x00,, +4.42607525,0x46,, +4.441254625,0x55,, +4.441350125,0x55,, +4.4414455,0x18,, +4.441541,0x00,, +4.4416365,0x83,, +4.441731875,0x9F,, +4.441827375,0xDD,, +4.441922875,0x00,, +4.442018375,0x2C,, +4.44211375,0x7D,, +4.44220925,0x49,, +4.44230475,0x90,, +4.44240025,0x18,, +4.442495625,0x00,, +4.442591125,0xD5,, +4.442686625,0x42,, +4.442782125,0xAB,, +4.4428775,0x80,, +4.442973,0x0D,, +4.4430685,0x54,, +4.443164,0x80,, +4.443259375,0x08,, +4.443354875,0x00,, +4.443450375,0x2A,, +4.443545875,0x62,, +4.44364125,0xA6,, +4.44373675,0x78,, +4.461346125,0x55,, +4.461441625,0x55,, +4.461537125,0x18,, +4.4616325,0x00,, +4.461728,0x84,, +4.4618235,0x65,, +4.461919,0xDB,, +4.462014375,0x00,, +4.462109875,0x2C,, +4.462205375,0x7D,, +4.46230075,0x49,, +4.46239625,0x90,, +4.46249175,0x18,, +4.46258725,0x00,, +4.46268275,0xD5,, +4.462778125,0x42,, +4.462873625,0xAB,, +4.462969125,0x80,, +4.4630645,0x0D,, +4.46316,0x54,, +4.4632555,0x80,, +4.463351,0x08,, +4.463446375,0x00,, +4.463541875,0x2A,, +4.463637375,0x62,, +4.463732875,0xA6,, +4.46382825,0x86,, +4.481446375,0x55,, +4.481541875,0x55,, +4.48163725,0x18,, +4.48173275,0x00,, +4.48182825,0x85,, +4.481923625,0x2B,, +4.482019125,0xDB,, +4.482114625,0x00,, +4.482210125,0x2C,, +4.482305625,0x6D,, +4.482401,0x3A,, +4.4824965,0x7D,, +4.482592,0xE8,, +4.4826875,0x00,, +4.482782875,0xD5,, +4.482878375,0x42,, +4.482973875,0xAB,, +4.48306925,0x80,, +4.48316475,0x0D,, +4.48326025,0x54,, +4.48335575,0x80,, +4.483451125,0x08,, +4.483546625,0x00,, +4.483642125,0x2A,, +4.483737625,0x62,, +4.483833,0xA6,, +4.4839285,0xC5,, +4.501807,0x55,, +4.501902375,0x55,, +4.501997875,0x18,, +4.502093375,0x00,, +4.502188875,0x85,, +4.50228425,0xF5,, +4.50237975,0xDD,, +4.50247525,0x00,, +4.50257075,0x2C,, +4.502666125,0x6D,, +4.502761625,0x3E,, +4.502857125,0x70,, +4.5029525,0xE8,, +4.503048,0x00,, +4.5031435,0xD5,, +4.503239,0x42,, +4.503334375,0xAB,, +4.503429875,0x80,, +4.503525375,0x0D,, +4.503620875,0x54,, +4.50371625,0x80,, +4.50381175,0x08,, +4.50390725,0x00,, +4.504002625,0x2A,, +4.504098125,0x62,, +4.504193625,0xA6,, +4.504289125,0x87,, +4.522306375,0x55,, +4.522401875,0x55,, +4.522497375,0x18,, +4.52259275,0x00,, +4.52268825,0x86,, +4.52278375,0xBF,, +4.52287925,0xDC,, +4.522974625,0x00,, +4.523070125,0x2C,, +4.523165625,0x7D,, +4.523261,0x42,, +4.5233565,0x5C,, +4.523452,0xB8,, +4.5235475,0x00,, +4.523643,0xD5,, +4.523738375,0x42,, +4.523833875,0xAB,, +4.523929375,0x80,, +4.524024875,0x0D,, +4.52412025,0x54,, +4.52421575,0x80,, +4.52431125,0x08,, +4.524406625,0x00,, +4.524502125,0x2A,, +4.524597625,0x62,, +4.524693125,0xA6,, +4.5247885,0x51,, +4.541885875,0x55,, +4.541981375,0x55,, +4.542076875,0x18,, +4.54217225,0x00,, +4.54226775,0x87,, +4.54236325,0x80,, +4.54245875,0xD0,, +4.542554125,0x00,, +4.542649625,0x2C,, +4.542745125,0x7D,, +4.5428405,0x42,, +4.542936,0x5C,, +4.5430315,0xB8,, +4.543127,0x00,, +4.543222375,0xD5,, +4.543317875,0x42,, +4.543413375,0xAB,, +4.543508875,0x80,, +4.54360425,0x0D,, +4.54369975,0x54,, +4.54379525,0x80,, +4.54389075,0x08,, +4.543986125,0x00,, +4.544081625,0x2A,, +4.544177125,0x62,, +4.544272625,0xA6,, +4.544368,0x18,, +4.5618385,0x55,, +4.561934,0x55,, +4.5620295,0x18,, +4.562125,0x00,, +4.562220375,0x88,, +4.562315875,0x46,, +4.562411375,0xCC,, +4.562506875,0x00,, +4.56260225,0x2C,, +4.56269775,0x6D,, +4.56279325,0x42,, +4.56288875,0x42,, +4.562984125,0xE8,, +4.563079625,0x00,, +4.563175125,0xD5,, +4.563270625,0x42,, +4.563366,0xAB,, +4.5634615,0x80,, +4.563557,0x0D,, +4.563652375,0x54,, +4.563747875,0x80,, +4.563843375,0x08,, +4.563938875,0x00,, +4.56403425,0x2A,, +4.56412975,0x62,, +4.56422525,0xA6,, +4.56432075,0x1E,, +4.58193875,0x55,, +4.58203425,0x55,, +4.58212975,0x18,, +4.58222525,0x00,, +4.582320625,0x89,, +4.582416125,0x0B,, +4.582511625,0xCD,, +4.582607,0x00,, +4.5827025,0x2C,, +4.582798,0x8D,, +4.5828935,0x41,, +4.582988875,0x2D,, +4.583084375,0xC8,, +4.583179875,0x00,, +4.583275375,0xD5,, +4.58337075,0x42,, +4.58346625,0xAB,, +4.58356175,0x80,, +4.583657125,0x0D,, +4.583752625,0x54,, +4.583848125,0x80,, +4.583943625,0x08,, +4.584039125,0x00,, +4.5841345,0x2A,, +4.58423,0x62,, +4.5843255,0xA6,, +4.584420875,0x41,, +4.602039,0x55,, +4.6021345,0x55,, +4.602229875,0x18,, +4.602325375,0x00,, +4.602420875,0x89,, +4.602516375,0xD1,, +4.60261175,0xD0,, +4.60270725,0x00,, +4.60280275,0x2C,, +4.60289825,0x4D,, +4.60299375,0x48,, +4.603089125,0x2A,, +4.603184625,0xB8,, +4.603280125,0x00,, +4.6033755,0xD5,, +4.603471,0x42,, +4.6035665,0xAB,, +4.603662,0x80,, +4.603757375,0x0D,, +4.603852875,0x54,, +4.603948375,0x80,, +4.604043875,0x08,, +4.60413925,0x00,, +4.60423475,0x2A,, +4.60433025,0x62,, +4.604425625,0xA6,, +4.604521125,0xFE,, +4.622538375,0x55,, +4.622633875,0x55,, +4.622729375,0x2B,, +4.622824875,0x03,, +4.622920375,0x8A,, +4.62301575,0x98,, +4.62311125,0xC0,, +4.62320675,0x00,, +4.62330225,0x2C,, +4.623397625,0x5C,, +4.623493125,0xDE,, +4.623588625,0x2A,, +4.623684,0xB8,, +4.6237795,0x00,, +4.623875,0xD5,, +4.6239705,0x42,, +4.624065875,0xAB,, +4.624161375,0x80,, +4.624256875,0x0D,, +4.624352375,0x54,, +4.62444775,0x80,, +4.62454325,0x08,, +4.62463875,0x00,, +4.62473425,0x2A,, +4.624829625,0x62,, +4.624925125,0xA6,, +4.625020625,0x00,, +4.625116125,0x00,, +4.6252115,0x00,, +4.625307,0x00,, +4.6254025,0x00,, +4.625497875,0x00,, +4.625593375,0x00,, +4.625688875,0x00,, +4.625784375,0x00,, +4.62587975,0x00,, +4.62597525,0x00,, +4.62607075,0x00,, +4.62616625,0x00,, +4.62626175,0x00,, +4.626357125,0x00,, +4.626452625,0x00,, +4.626548125,0x00,, +4.6266435,0x00,, +4.626739,0x00,, +4.6268345,0x34,, +4.6422395,0x55,, +4.642334875,0x55,, +4.642430375,0x18,, +4.642525875,0x00,, +4.642621375,0x8B,, +4.64271675,0x5F,, +4.64281225,0xC4,, +4.64290775,0x00,, +4.643003125,0x2C,, +4.643098625,0x6C,, +4.643194125,0x1A,, +4.643289625,0x2A,, +4.643385,0xB8,, +4.6434805,0x00,, +4.643576,0xD5,, +4.6436715,0x42,, +4.643766875,0xAB,, +4.643862375,0x80,, +4.643957875,0x0D,, +4.644053375,0x54,, +4.64414875,0x80,, +4.64424425,0x08,, +4.64433975,0x00,, +4.64443525,0x2A,, +4.644530625,0x62,, +4.644626125,0xA6,, +4.644721625,0xC4,, +4.662339625,0x55,, +4.662435125,0x55,, +4.662530625,0x18,, +4.662626,0x00,, +4.6627215,0x8C,, +4.662817,0x26,, +4.6629125,0xC0,, +4.663007875,0x00,, +4.663103375,0x2C,, +4.663198875,0x6B,, +4.663294375,0x5C,, +4.66338975,0x2E,, +4.66348525,0x38,, +4.66358075,0x00,, +4.66367625,0xD5,, +4.663771625,0x42,, +4.663867125,0xAB,, +4.663962625,0x80,, +4.664058125,0x0D,, +4.6641535,0x54,, +4.664249,0x80,, +4.6643445,0x08,, +4.66444,0x00,, +4.664535375,0x2A,, +4.664630875,0x62,, +4.664726375,0xA6,, +4.66482175,0xA5,, +4.683299125,0x55,, +4.6833945,0x55,, +4.68349,0x18,, +4.6835855,0x00,, +4.683681,0x8C,, +4.683776375,0xF3,, +4.683871875,0xCC,, +4.683967375,0x00,, +4.684062875,0x2C,, +4.68415825,0x4A,, +4.68425375,0xA8,, +4.68434925,0x38,, +4.684444625,0xA8,, +4.684540125,0x00,, +4.684635625,0xD5,, +4.684731125,0x42,, +4.684826625,0xAB,, +4.684922,0x80,, +4.6850175,0x0D,, +4.685113,0x54,, +4.685208375,0x80,, +4.685303875,0x08,, +4.685399375,0x00,, +4.685494875,0x2A,, +4.68559025,0x62,, +4.68568575,0xA6,, +4.68578125,0x26,, +4.702531375,0x55,, +4.702626875,0x55,, +4.702722375,0x18,, +4.70281775,0x00,, +4.70291325,0x8D,, +4.70300875,0xB1,, +4.70310425,0xC4,, +4.70319975,0x00,, +4.703295125,0x2C,, +4.703390625,0x69,, +4.703486125,0xBA,, +4.703581625,0x42,, +4.703677,0xC8,, +4.7037725,0x00,, +4.703868,0xD5,, +4.703963375,0x42,, +4.704058875,0xAB,, +4.704154375,0x80,, +4.704249875,0x0D,, +4.70434525,0x54,, +4.70444075,0x80,, +4.70453625,0x08,, +4.70463175,0x00,, +4.704727125,0x2A,, +4.704822625,0x62,, +4.704918125,0xA6,, +4.705013625,0x1F,, +4.722631625,0x55,, +4.722727125,0x55,, +4.722822625,0x18,, +4.722918,0x00,, +4.7230135,0x8E,, +4.723109,0x78,, +4.7232045,0xCC,, +4.723299875,0x00,, +4.723395375,0x2C,, +4.723490875,0x68,, +4.72358625,0xEB,, +4.72368175,0x4A,, +4.72377725,0x68,, +4.72387275,0x00,, +4.723968125,0xD5,, +4.724063625,0x42,, +4.724159125,0xAB,, +4.724254625,0x80,, +4.724350125,0x0D,, +4.7244455,0x54,, +4.724541,0x80,, +4.7246365,0x08,, +4.724731875,0x00,, +4.724827375,0x2A,, +4.724922875,0x62,, +4.725018375,0xA6,, +4.72511375,0xF4,, +4.742731875,0x55,, +4.742827375,0x55,, +4.74292275,0x18,, +4.74301825,0x00,, +4.74311375,0x8F,, +4.74320925,0x3F,, +4.743304625,0xC7,, +4.743400125,0x00,, +4.743495625,0x2C,, +4.743591,0x78,, +4.7436865,0x23,, +4.743782,0x51,, +4.7438775,0x18,, +4.743973,0x00,, +4.744068375,0xD5,, +4.744163875,0x42,, +4.744259375,0xAB,, +4.74435475,0x80,, +4.74445025,0x0D,, +4.74454575,0x54,, +4.74464125,0x80,, +4.744736625,0x08,, +4.744832125,0x00,, +4.744927625,0x2A,, +4.745023125,0x62,, +4.7451185,0xA6,, +4.745214,0x6C,, +4.762823375,0x55,, +4.762918875,0x55,, +4.763014375,0x18,, +4.76310975,0x00,, +4.76320525,0x90,, +4.76330075,0x06,, +4.76339625,0xCA,, +4.763491625,0x00,, +4.763587125,0x2C,, +4.763682625,0x67,, +4.763778125,0xAC,, +4.7638735,0x58,, +4.763969,0x88,, +4.7640645,0x00,, +4.76416,0xD5,, +4.764255375,0x42,, +4.764350875,0xAB,, +4.764446375,0x80,, +4.764541875,0x0D,, +4.76463725,0x54,, +4.76473275,0x80,, +4.76482825,0x08,, +4.764923625,0x00,, +4.765019125,0x2A,, +4.765114625,0x62,, +4.765210125,0xA6,, +4.7653055,0xD7,, +4.782923625,0x55,, +4.783019125,0x55,, +4.7831145,0x18,, +4.78321,0x00,, +4.7833055,0x90,, +4.783401,0xCC,, +4.783496375,0xCC,, +4.783591875,0x00,, +4.783687375,0x2C,, +4.783782875,0x57,, +4.78387825,0x03,, +4.78397375,0x61,, +4.78406925,0xE8,, +4.78416475,0x00,, +4.784260125,0xD5,, +4.784355625,0x42,, +4.784451125,0xAB,, +4.7845465,0x80,, +4.784642,0x0D,, +4.7847375,0x54,, +4.784833,0x80,, +4.784928375,0x08,, +4.785023875,0x00,, +4.785119375,0x2A,, +4.785214875,0x62,, +4.785310375,0xA6,, +4.78540575,0xFC,, +4.803023875,0x55,, +4.803119375,0x55,, +4.80321475,0x18,, +4.80331025,0x00,, +4.80340575,0x91,, +4.803501125,0x92,, +4.803596625,0xC9,, +4.803692125,0x00,, +4.803787625,0x2C,, +4.803883,0x66,, +4.8039785,0x6D,, +4.804074,0x6D,, +4.8041695,0x88,, +4.804264875,0x00,, +4.804360375,0xD5,, +4.804455875,0x42,, +4.80455125,0xAB,, +4.80464675,0x80,, +4.80474225,0x0D,, +4.80483775,0x54,, +4.80493325,0x80,, +4.805028625,0x08,, +4.805124125,0x00,, +4.805219625,0x2A,, +4.805315,0x62,, +4.8054105,0xA6,, +4.805506,0xE2,, +4.82352325,0x55,, +4.82361875,0x55,, +4.82371425,0x2B,, +4.823809625,0x03,, +4.823905125,0x92,, +4.824000625,0x59,, +4.824096125,0xCF,, +4.824191625,0x00,, +4.824287,0x2C,, +4.8243825,0x75,, +4.824478,0xFC,, +4.824573375,0x7C,, +4.824668875,0x28,, +4.824764375,0x00,, +4.824859875,0xD5,, +4.82495525,0x42,, +4.82505075,0xAB,, +4.82514625,0x80,, +4.82524175,0x0D,, +4.825337125,0x54,, +4.825432625,0x80,, +4.825528125,0x08,, +4.825623625,0x00,, +4.825719,0x2A,, +4.8258145,0x62,, +4.82591,0xA6,, +4.826005375,0x00,, +4.826100875,0x00,, +4.826196375,0x00,, +4.826291875,0x00,, +4.826387375,0x00,, +4.82648275,0x00,, +4.82657825,0x00,, +4.82667375,0x00,, +4.826769125,0x00,, +4.826864625,0x00,, +4.826960125,0x00,, +4.827055625,0x00,, +4.827151,0x00,, +4.8272465,0x00,, +4.827342,0x00,, +4.8274375,0x00,, +4.827532875,0x00,, +4.827628375,0x00,, +4.827723875,0x00,, +4.82781925,0xD6,, +4.843215625,0x55,, +4.843311125,0x55,, +4.8434065,0x18,, +4.843502,0x00,, +4.8435975,0x93,, +4.843692875,0x1F,, +4.843788375,0xDC,, +4.843883875,0x00,, +4.843979375,0x2C,, +4.84407475,0x65,, +4.84417025,0x98,, +4.84426575,0x83,, +4.84436125,0x38,, +4.844456625,0x00,, +4.844552125,0xD5,, +4.844647625,0x42,, +4.844743125,0xAB,, +4.8448385,0x80,, +4.844934,0x0D,, +4.8450295,0x54,, +4.845125,0x80,, +4.845220375,0x08,, +4.845315875,0x00,, +4.845411375,0x2A,, +4.845506875,0x62,, +4.84560225,0xA6,, +4.84569775,0x3E,, +4.863446,0x55,, +4.8635415,0x55,, +4.863636875,0x18,, +4.863732375,0x00,, +4.863827875,0x93,, +4.863923375,0xE5,, +4.864018875,0xCA,, +4.86411425,0x00,, +4.86420975,0x2C,, +4.86430525,0x45,, +4.864400625,0x6A,, +4.864496125,0x8B,, +4.864591625,0xB8,, +4.864687125,0x00,, +4.8647825,0xD5,, +4.864878,0x42,, +4.8649735,0xAB,, +4.865069,0x80,, +4.865164375,0x0D,, +4.865259875,0x54,, +4.865355375,0x80,, +4.86545075,0x08,, +4.86554625,0x00,, +4.86564175,0x2A,, +4.86573725,0x62,, +4.865832625,0xA6,, +4.865928125,0xF9,, +4.883416,0x55,, +4.8835115,0x55,, +4.883607,0x18,, +4.8837025,0x00,, +4.883797875,0x94,, +4.883893375,0xAB,, +4.883988875,0xCD,, +4.88408425,0x00,, +4.88417975,0x2C,, +4.88427525,0x55,, +4.88437075,0x62,, +4.884466125,0x8F,, +4.884561625,0xD8,, +4.884657125,0x00,, +4.884752625,0xD5,, +4.884848,0x42,, +4.8849435,0xAB,, +4.885039,0x80,, +4.8851345,0x0D,, +4.885229875,0x54,, +4.885325375,0x80,, +4.885420875,0x08,, +4.885516375,0x00,, +4.88561175,0x2A,, +4.88570725,0x62,, +4.88580275,0xA6,, +4.88589825,0x86,, +4.90351625,0x55,, +4.90361175,0x55,, +4.90370725,0x18,, +4.903802625,0x00,, +4.903898125,0x95,, +4.903993625,0x71,, +4.904089,0xCA,, +4.9041845,0x00,, +4.90428,0x2C,, +4.9043755,0x65,, +4.904471,0x62,, +4.904566375,0x8E,, +4.904661875,0x38,, +4.904757375,0x00,, +4.90485275,0xD5,, +4.90494825,0x42,, +4.90504375,0xAB,, +4.90513925,0x80,, +4.905234625,0x0D,, +4.905330125,0x54,, +4.905425625,0x80,, +4.905521125,0x08,, +4.9056165,0x00,, +4.905712,0x2A,, +4.9058075,0x62,, +4.905902875,0xA6,, +4.905998375,0x27,, +4.92360775,0x55,, +4.92370325,0x55,, +4.92379875,0x18,, +4.92389425,0x00,, +4.923989625,0x96,, +4.924085125,0x37,, +4.924180625,0xCB,, +4.924276125,0x00,, +4.9243715,0x2C,, +4.924467,0x55,, +4.9245625,0x6E,, +4.924657875,0x84,, +4.924753375,0x98,, +4.924848875,0x00,, +4.924944375,0xD5,, +4.925039875,0x42,, +4.92513525,0xAB,, +4.92523075,0x80,, +4.92532625,0x0D,, +4.925421625,0x54,, +4.925517125,0x80,, +4.925612625,0x08,, +4.925708125,0x00,, +4.9258035,0x2A,, +4.925899,0x62,, +4.9259945,0xA6,, +4.92609,0x30,, +4.943708,0x55,, +4.9438035,0x55,, +4.943899,0x18,, +4.943994375,0x00,, +4.944089875,0x96,, +4.944185375,0xFD,, +4.94428075,0xCC,, +4.94437625,0x00,, +4.94447175,0x2C,, +4.94456725,0x65,, +4.94466275,0xBE,, +4.944758125,0x7F,, +4.944853625,0x28,, +4.944949125,0x00,, +4.945044625,0xD5,, +4.94514,0x42,, +4.9452355,0xAB,, +4.945331,0x80,, +4.945426375,0x0D,, +4.945521875,0x54,, +4.945617375,0x80,, +4.945712875,0x08,, +4.94580825,0x00,, +4.94590375,0x2A,, +4.94599925,0x62,, +4.94609475,0xA6,, +4.946190125,0x6A,, +4.96380825,0x55,, +4.96390375,0x55,, +4.963999125,0x18,, +4.964094625,0x00,, +4.964190125,0x97,, +4.964285625,0xC4,, +4.964381,0xCC,, +4.9644765,0x00,, +4.964572,0x2C,, +4.9646675,0x66,, +4.964762875,0x2C,, +4.964858375,0x7D,, +4.964953875,0x88,, +4.96504925,0x00,, +4.96514475,0xD5,, +4.96524025,0x42,, +4.96533575,0xAB,, +4.96543125,0x80,, +4.965526625,0x0D,, +4.965622125,0x54,, +4.965717625,0x80,, +4.965813,0x08,, +4.9659085,0x00,, +4.966004,0x2A,, +4.9660995,0x62,, +4.966194875,0xA6,, +4.966290375,0x13,, +4.9839085,0x55,, +4.984003875,0x55,, +4.984099375,0x18,, +4.984194875,0x00,, +4.984290375,0x98,, +4.98438575,0x8B,, +4.98448125,0xD1,, +4.98457675,0x00,, +4.98467225,0x2C,, +4.984767625,0x66,, +4.984863125,0xAE,, +4.984958625,0x80,, +4.985054125,0x08,, +4.9851495,0x00,, +4.985245,0xD5,, +4.9853405,0x42,, +4.985436,0xAB,, +4.985531375,0x80,, +4.985626875,0x0D,, +4.985722375,0x54,, +4.98581775,0x80,, +4.98591325,0x08,, +4.98600875,0x00,, +4.98610425,0x2A,, +4.986199625,0x62,, +4.986295125,0xA6,, +4.986390625,0xD1,, +5.004,0x55,, +5.0040955,0x55,, +5.004190875,0x18,, +5.004286375,0x00,, +5.004381875,0x99,, +5.004477375,0x51,, +5.00457275,0xCC,, +5.00466825,0x00,, +5.00476375,0x2C,, +5.00485925,0x78,, +5.004954625,0x81,, +5.005050125,0x80,, +5.005145625,0x08,, +5.005241,0x00,, +5.0053365,0xD5,, +5.005432,0x42,, +5.0055275,0xAB,, +5.005623,0x80,, +5.005718375,0x0D,, +5.005813875,0x54,, +5.005909375,0x80,, +5.006004875,0x08,, +5.00610025,0x00,, +5.00619575,0x2A,, +5.00629125,0x62,, +5.006386625,0xA6,, +5.006482125,0x3B,, +5.024499375,0x55,, +5.024594875,0x55,, +5.024690375,0x2B,, +5.024785875,0x03,, +5.024881375,0x9A,, +5.02497675,0x18,, +5.02507225,0xC9,, +5.02516775,0x00,, +5.02526325,0x2C,, +5.025358625,0x67,, +5.025454125,0xA3,, +5.025549625,0x80,, +5.025645,0x08,, +5.0257405,0x00,, +5.025836,0xD5,, +5.0259315,0x42,, +5.026026875,0xAB,, +5.026122375,0x80,, +5.026217875,0x0D,, +5.026313375,0x54,, +5.02640875,0x80,, +5.02650425,0x08,, +5.02659975,0x00,, +5.026695125,0x2A,, +5.026790625,0x62,, +5.026886125,0xA6,, +5.026981625,0x00,, +5.027077125,0x00,, +5.0271725,0x00,, +5.027268,0x00,, +5.0273635,0x00,, +5.027458875,0x00,, +5.027554375,0x00,, +5.027649875,0x00,, +5.027745375,0x00,, +5.02784075,0x00,, +5.02793625,0x00,, +5.02803175,0x00,, +5.02812725,0x00,, +5.028222625,0x00,, +5.028318125,0x00,, +5.028413625,0x00,, +5.028509125,0x00,, +5.0286045,0x00,, +5.0287,0x00,, +5.0287955,0x57,, +5.0442005,0x55,, +5.044295875,0x55,, +5.044391375,0x18,, +5.044486875,0x00,, +5.044582375,0x9A,, +5.04467775,0xDD,, +5.04477325,0xCA,, +5.04486875,0x00,, +5.044964125,0x2C,, +5.045059625,0x78,, +5.045155125,0x23,, +5.045250625,0x80,, +5.045346,0x08,, +5.0454415,0x00,, +5.045537,0xD5,, +5.0456325,0x42,, +5.045727875,0xAB,, +5.045823375,0x80,, +5.045918875,0x0D,, +5.046014375,0x54,, +5.04610975,0x80,, +5.04620525,0x08,, +5.04630075,0x00,, +5.04639625,0x2A,, +5.046491625,0x62,, +5.046587125,0xA6,, +5.046682625,0x64,, +5.064292,0x55,, +5.064387375,0x55,, +5.064482875,0x18,, +5.064578375,0x00,, +5.064673875,0x9B,, +5.064769375,0xA4,, +5.06486475,0xD1,, +5.06496025,0x00,, +5.06505575,0x2C,, +5.065151125,0x58,, +5.065246625,0x2E,, +5.065342125,0x80,, +5.065437625,0x08,, +5.065533,0x00,, +5.0656285,0xD5,, +5.065724,0x42,, +5.0658195,0xAB,, +5.065914875,0x80,, +5.066010375,0x0D,, +5.066105875,0x54,, +5.066201375,0x80,, +5.06629675,0x08,, +5.06639225,0x00,, +5.06648775,0x2A,, +5.06658325,0x62,, +5.066678625,0xA6,, +5.066774125,0xFB,, +5.08439225,0x55,, +5.084487625,0x55,, +5.084583125,0x18,, +5.084678625,0x00,, +5.084774125,0x9C,, +5.0848695,0x6A,, +5.084965,0xDD,, +5.0850605,0x00,, +5.085155875,0x2C,, +5.085251375,0x68,, +5.085346875,0x00,, +5.085442375,0x80,, +5.085537875,0x08,, +5.08563325,0x00,, +5.08572875,0xD5,, +5.08582425,0x42,, +5.085919625,0xAB,, +5.086015125,0x80,, +5.086110625,0x0D,, +5.086206125,0x54,, +5.0863015,0x80,, +5.086397,0x08,, +5.0864925,0x00,, +5.086588,0x2A,, +5.086683375,0x62,, +5.086778875,0xA6,, +5.086874375,0xF4,, +5.104492375,0x55,, +5.104587875,0x55,, +5.104683375,0x18,, +5.10477875,0x00,, +5.10487425,0x9D,, +5.10496975,0x30,, +5.10506525,0xB9,, +5.10516075,0x00,, +5.105256125,0x2C,, +5.105351625,0x68,, +5.105447125,0x00,, +5.105542625,0x80,, +5.105638,0x08,, +5.1057335,0x00,, +5.105829,0xD5,, +5.105924375,0x42,, +5.106019875,0xAB,, +5.106115375,0x80,, +5.106210875,0x0D,, +5.10630625,0x54,, +5.10640175,0x80,, +5.10649725,0x08,, +5.10659275,0x00,, +5.106688125,0x2A,, +5.106783625,0x62,, +5.106879125,0xA6,, +5.1069745,0xF2,, +5.124592625,0x55,, +5.124688125,0x55,, +5.124783625,0x18,, +5.124879,0x00,, +5.1249745,0x9D,, +5.12507,0xF5,, +5.1251655,0xD1,, +5.125260875,0x00,, +5.125356375,0x2C,, +5.125451875,0x58,, +5.12554725,0x00,, +5.12564275,0x80,, +5.12573825,0x08,, +5.12583375,0x00,, +5.125929125,0xD5,, +5.126024625,0x42,, +5.126120125,0xAB,, +5.126215625,0x80,, +5.126311125,0x0D,, +5.1264065,0x54,, +5.126502,0x80,, +5.1265975,0x08,, +5.126692875,0x00,, +5.126788375,0x2A,, +5.126883875,0x62,, +5.126979375,0xA6,, +5.12707475,0x7B,, +5.14468425,0x55,, +5.144779625,0x55,, +5.144875125,0x18,, +5.144970625,0x00,, +5.145066,0x9E,, +5.1451615,0xBC,, +5.145257,0xB8,, +5.1453525,0x00,, +5.145447875,0x2C,, +5.145543375,0x68,, +5.145638875,0x00,, +5.145734375,0x80,, +5.14582975,0x08,, +5.14592525,0x00,, +5.14602075,0xD5,, +5.146116125,0x42,, +5.146211625,0xAB,, +5.146307125,0x80,, +5.146402625,0x0D,, +5.146498,0x54,, +5.1465935,0x80,, +5.146689,0x08,, +5.1467845,0x00,, +5.146879875,0x2A,, +5.146975375,0x62,, +5.147070875,0xA6,, +5.147166375,0x2A,, +5.164784375,0x55,, +5.164879875,0x55,, +5.164975375,0x18,, +5.16507075,0x00,, +5.16516625,0x9F,, +5.16526175,0x83,, +5.16535725,0xD0,, +5.165452625,0x00,, +5.165548125,0x2C,, +5.165643625,0x58,, +5.165739125,0x00,, +5.1658345,0x80,, +5.16593,0x08,, +5.1660255,0x00,, +5.166121,0xD5,, +5.166216375,0x42,, +5.166311875,0xAB,, +5.166407375,0x80,, +5.166502875,0x0D,, +5.16659825,0x54,, +5.16669375,0x80,, +5.16678925,0x08,, +5.166884625,0x00,, +5.166980125,0x2A,, +5.167075625,0x62,, +5.167171125,0xA6,, +5.1672665,0x5E,, +5.184876,0x55,, +5.184971375,0x55,, +5.185066875,0x18,, +5.185162375,0x00,, +5.18525775,0xA0,, +5.18535325,0x49,, +5.18544875,0xDC,, +5.18554425,0x00,, +5.185639625,0x2C,, +5.185735125,0x48,, +5.185830625,0x00,, +5.185926125,0x80,, +5.1860215,0x08,, +5.186117,0x00,, +5.1862125,0xD5,, +5.186307875,0x42,, +5.186403375,0xAB,, +5.186498875,0x80,, +5.186594375,0x0D,, +5.186689875,0x54,, +5.18678525,0x80,, +5.18688075,0x08,, +5.18697625,0x00,, +5.18707175,0x2A,, +5.187167125,0x62,, +5.187262625,0xA6,, +5.187358125,0x00,, +5.205566375,0x55,, +5.20566175,0x55,, +5.20575725,0x18,, +5.20585275,0x00,, +5.20594825,0xA1,, +5.206043625,0x16,, +5.206139125,0xC0,, +5.206234625,0x00,, +5.206330125,0x2C,, +5.2064255,0x68,, +5.206521,0x00,, +5.2066165,0x80,, +5.206711875,0x08,, +5.206807375,0x00,, +5.206902875,0xD5,, +5.206998375,0x42,, +5.20709375,0xAB,, +5.20718925,0x80,, +5.20728475,0x0D,, +5.20738025,0x54,, +5.207475625,0x80,, +5.207571125,0x08,, +5.207666625,0x00,, +5.207762,0x2A,, +5.2078575,0x62,, +5.207953,0xA6,, +5.2080485,0x1F,, +5.225475625,0x55,, +5.225571125,0x55,, +5.2256665,0x2B,, +5.225762,0x03,, +5.2258575,0xA1,, +5.225953,0xD6,, +5.226048375,0xC2,, +5.226143875,0x00,, +5.226239375,0x2C,, +5.22633475,0x68,, +5.22643025,0x00,, +5.22652575,0x80,, +5.22662125,0x08,, +5.226716625,0x00,, +5.226812125,0xD5,, +5.226907625,0x42,, +5.227003125,0xAB,, +5.227098625,0x80,, +5.227194,0x0D,, +5.2272895,0x54,, +5.227385,0x80,, +5.227480375,0x08,, +5.227575875,0x00,, +5.227671375,0x2A,, +5.227766875,0x62,, +5.22786225,0xA6,, +5.22795775,0x00,, +5.22805325,0x00,, +5.22814875,0x00,, +5.228244125,0x00,, +5.228339625,0x00,, +5.228435125,0x00,, +5.2285305,0x00,, +5.228626,0x00,, +5.2287215,0x00,, +5.228817,0x00,, +5.228912375,0x00,, +5.229007875,0x00,, +5.229103375,0x00,, +5.229198875,0x00,, +5.22929425,0x00,, +5.22938975,0x00,, +5.22948525,0x00,, +5.22958075,0x00,, +5.229676125,0x00,, +5.229771625,0xBE,, +5.245176625,0x55,, +5.245272125,0x55,, +5.2453675,0x18,, +5.245463,0x00,, +5.2455585,0xA2,, +5.245653875,0x9D,, +5.245749375,0xCF,, +5.245844875,0x00,, +5.245940375,0x2C,, +5.24603575,0x68,, +5.24613125,0x00,, +5.24622675,0x80,, +5.24632225,0x08,, +5.246417625,0x00,, +5.246513125,0xD5,, +5.246608625,0x42,, +5.246704125,0xAB,, +5.2467995,0x80,, +5.246895,0x0D,, +5.2469905,0x54,, +5.247086,0x80,, +5.247181375,0x08,, +5.247276875,0x00,, +5.247372375,0x2A,, +5.247467875,0x62,, +5.24756325,0xA6,, +5.24765875,0x1A,, +5.265276875,0x55,, +5.26537225,0x55,, +5.26546775,0x18,, +5.26556325,0x00,, +5.265658625,0xA3,, +5.265754125,0x62,, +5.265849625,0xC5,, +5.265945125,0x00,, +5.266040625,0x2C,, +5.266136,0x58,, +5.2662315,0x00,, +5.266327,0x80,, +5.266422375,0x08,, +5.266517875,0x00,, +5.266613375,0xD5,, +5.266708875,0x42,, +5.26680425,0xAB,, +5.26689975,0x80,, +5.26699525,0x0D,, +5.26709075,0x54,, +5.267186125,0x80,, +5.267281625,0x08,, +5.267377125,0x00,, +5.2674725,0x2A,, +5.267568,0x62,, +5.2676635,0xA6,, +5.267759,0x22,, +5.285368375,0x55,, +5.285463875,0x55,, +5.28555925,0x18,, +5.28565475,0x00,, +5.28575025,0xA4,, +5.285845625,0x28,, +5.285941125,0xCF,, +5.286036625,0x00,, +5.286132125,0x2C,, +5.286227625,0x68,, +5.286323,0x00,, +5.2864185,0x80,, +5.286514,0x08,, +5.2866095,0x00,, +5.286704875,0xD5,, +5.286800375,0x42,, +5.286895875,0xAB,, +5.28699125,0x80,, +5.28708675,0x0D,, +5.28718225,0x54,, +5.28727775,0x80,, +5.287373125,0x08,, +5.287468625,0x00,, +5.287564125,0x2A,, +5.287659625,0x62,, +5.287755,0xA6,, +5.2878505,0xA9,, +5.306458,0x55,, +5.3065535,0x55,, +5.306648875,0x18,, +5.306744375,0x00,, +5.306839875,0xA4,, +5.30693525,0xF7,, +5.30703075,0xD0,, +5.30712625,0x00,, +5.30722175,0x2C,, +5.307317125,0x48,, +5.307412625,0x00,, +5.307508125,0x80,, +5.307603625,0x08,, +5.307699,0x00,, +5.3077945,0xD5,, +5.30789,0x42,, +5.3079855,0xAB,, +5.308080875,0x80,, +5.308176375,0x0D,, +5.308271875,0x54,, +5.308367375,0x80,, +5.30846275,0x08,, +5.30855825,0x00,, +5.30865375,0x2A,, +5.308749125,0x62,, +5.308844625,0xA6,, +5.308940125,0xB9,, +5.325777125,0x55,, +5.3258725,0x55,, +5.325968,0x18,, +5.3260635,0x00,, +5.326159,0xA5,, +5.326254375,0xB5,, +5.326349875,0xC4,, +5.326445375,0x00,, +5.326540875,0x2C,, +5.32663625,0x68,, +5.32673175,0x00,, +5.32682725,0x80,, +5.32692275,0x08,, +5.327018125,0x00,, +5.327113625,0xD5,, +5.327209125,0x42,, +5.327304625,0xAB,, +5.3274,0x80,, +5.3274955,0x0D,, +5.327591,0x54,, +5.3276865,0x80,, +5.327781875,0x08,, +5.327877375,0x00,, +5.327972875,0x2A,, +5.32806825,0x62,, +5.32816375,0xA6,, +5.32825925,0x4A,, +5.346597625,0x55,, +5.346693125,0x55,, +5.346788625,0x18,, +5.346884,0x00,, +5.3469795,0xA6,, +5.347075,0x82,, +5.3471705,0xD0,, +5.347265875,0x00,, +5.347361375,0x2C,, +5.347456875,0x68,, +5.347552375,0x00,, +5.34764775,0x80,, +5.34774325,0x08,, +5.34783875,0x00,, +5.34793425,0xD5,, +5.348029625,0x42,, +5.348125125,0xAB,, +5.348220625,0x80,, +5.348316,0x0D,, +5.3484115,0x54,, +5.348507,0x80,, +5.3486025,0x08,, +5.348697875,0x00,, +5.348793375,0x2A,, +5.348888875,0x62,, +5.348984375,0xA6,, +5.349079875,0x3C,, +5.3666285,0x55,, +5.366723875,0x55,, +5.366819375,0x18,, +5.366914875,0x00,, +5.36701025,0xA7,, +5.36710575,0x48,, +5.36720125,0xD0,, +5.36729675,0x00,, +5.367392125,0x2C,, +5.367487625,0x68,, +5.367583125,0x00,, +5.367678625,0x80,, +5.367774,0x08,, +5.3678695,0x00,, +5.367965,0xD5,, +5.3680605,0x42,, +5.368155875,0xAB,, +5.368251375,0x80,, +5.368346875,0x0D,, +5.368442375,0x54,, +5.36853775,0x80,, +5.36863325,0x08,, +5.36872875,0x00,, +5.36882425,0x2A,, +5.368919625,0x62,, +5.369015125,0xA6,, +5.369110625,0x6D,, +5.385991,0x55,, +5.386086375,0x55,, +5.386181875,0x18,, +5.386277375,0x00,, +5.386372875,0xA8,, +5.38646825,0x06,, +5.38656375,0xDB,, +5.38665925,0x00,, +5.38675475,0x2C,, +5.386850125,0x48,, +5.386945625,0x00,, +5.387041125,0x80,, +5.387136625,0x08,, +5.387232,0x00,, +5.3873275,0xD5,, +5.387423,0x42,, +5.387518375,0xAB,, +5.387613875,0x80,, +5.387709375,0x0D,, +5.387804875,0x54,, +5.38790025,0x80,, +5.38799575,0x08,, +5.38809125,0x00,, +5.38818675,0x2A,, +5.38828225,0x62,, +5.388377625,0xA6,, +5.388473125,0xAD,, +5.405961,0x55,, +5.4060565,0x55,, +5.406151875,0x18,, +5.406247375,0x00,, +5.406342875,0xA8,, +5.406438375,0xCC,, +5.40653375,0xDB,, +5.40662925,0x00,, +5.40672475,0x2C,, +5.40682025,0x68,, +5.406915625,0x00,, +5.407011125,0x80,, +5.407106625,0x08,, +5.407202,0x00,, +5.4072975,0xD5,, +5.407393,0x42,, +5.4074885,0xAB,, +5.407584,0x80,, +5.407679375,0x0D,, +5.407774875,0x54,, +5.407870375,0x80,, +5.407965875,0x08,, +5.40806125,0x00,, +5.40815675,0x2A,, +5.40825225,0x62,, +5.408347625,0xA6,, +5.408443125,0x69,, +5.426460375,0x55,, +5.426555875,0x55,, +5.426651375,0x2B,, +5.426746875,0x03,, +5.42684225,0xA9,, +5.42693775,0x91,, +5.42703325,0xDC,, +5.42712875,0x00,, +5.42722425,0x2C,, +5.427319625,0x68,, +5.427415125,0x00,, +5.427510625,0x80,, +5.427606,0x08,, +5.4277015,0x00,, +5.427797,0xD5,, +5.4278925,0x42,, +5.427987875,0xAB,, +5.428083375,0x80,, +5.428178875,0x0D,, +5.428274375,0x54,, +5.42836975,0x80,, +5.42846525,0x08,, +5.42856075,0x00,, +5.428656125,0x2A,, +5.428751625,0x62,, +5.428847125,0xA6,, +5.428942625,0x00,, +5.429038125,0x00,, +5.4291335,0x00,, +5.429229,0x00,, +5.4293245,0x00,, +5.429419875,0x00,, +5.429515375,0x00,, +5.429610875,0x00,, +5.429706375,0x00,, +5.42980175,0x00,, +5.42989725,0x00,, +5.42999275,0x00,, +5.43008825,0x00,, +5.430183625,0x00,, +5.430279125,0x00,, +5.430374625,0x00,, +5.430470125,0x00,, +5.4305655,0x00,, +5.430661,0x00,, +5.4307565,0xF4,, +5.4461615,0x55,, +5.446256875,0x55,, +5.446352375,0x18,, +5.446447875,0x00,, +5.446543375,0xAA,, +5.44663875,0x57,, +5.44673425,0xDC,, +5.44682975,0x00,, +5.446925125,0x2C,, +5.447020625,0x58,, +5.447116125,0x00,, +5.447211625,0x80,, +5.447307,0x08,, +5.4474025,0x00,, +5.447498,0xD5,, +5.4475935,0x42,, +5.447688875,0xAB,, +5.447784375,0x80,, +5.447879875,0x0D,, +5.44797525,0x54,, +5.44807075,0x80,, +5.44816625,0x08,, +5.44826175,0x00,, +5.44835725,0x2A,, +5.448452625,0x62,, +5.448548125,0xA6,, +5.448643625,0x1A,, +5.466253,0x55,, +5.466348375,0x55,, +5.466443875,0x18,, +5.466539375,0x00,, +5.466634875,0xAB,, +5.466730375,0x1E,, +5.46682575,0xDC,, +5.46692125,0x00,, +5.46701675,0x2C,, +5.467112125,0x68,, +5.467207625,0x00,, +5.467303125,0x80,, +5.467398625,0x08,, +5.467494,0x00,, +5.4675895,0xD5,, +5.467685,0x42,, +5.4677805,0xAB,, +5.467875875,0x80,, +5.467971375,0x0D,, +5.468066875,0x54,, +5.468162375,0x80,, +5.46825775,0x08,, +5.46835325,0x00,, +5.46844875,0x2A,, +5.46854425,0x62,, +5.468639625,0xA6,, +5.468735125,0x0E,, +5.48635325,0x55,, +5.486448625,0x55,, +5.486544125,0x18,, +5.486639625,0x00,, +5.486735125,0xAB,, +5.4868305,0xE4,, +5.486926,0xDA,, +5.4870215,0x00,, +5.487116875,0x2C,, +5.487212375,0x68,, +5.487307875,0x00,, +5.487403375,0x80,, +5.48749875,0x08,, +5.48759425,0x00,, +5.48768975,0xD5,, +5.48778525,0x42,, +5.487880625,0xAB,, +5.487976125,0x80,, +5.488071625,0x0D,, +5.488167125,0x54,, +5.4882625,0x80,, +5.488358,0x08,, +5.4884535,0x00,, +5.488549,0x2A,, +5.488644375,0x62,, +5.488739875,0xA6,, +5.488835375,0x49,, +5.506453375,0x55,, +5.506548875,0x55,, +5.506644375,0x18,, +5.50673975,0x00,, +5.50683525,0xAC,, +5.50693075,0xA9,, +5.50702625,0xDB,, +5.50712175,0x00,, +5.507217125,0x2C,, +5.507312625,0x58,, +5.507408125,0x00,, +5.507503625,0x80,, +5.507599,0x08,, +5.5076945,0x00,, +5.50779,0xD5,, +5.507885375,0x42,, +5.507980875,0xAB,, +5.508076375,0x80,, +5.508171875,0x0D,, +5.50826725,0x54,, +5.50836275,0x80,, +5.50845825,0x08,, +5.50855375,0x00,, +5.508649125,0x2A,, +5.508744625,0x62,, +5.508840125,0xA6,, +5.5089355,0x83,, +5.526553625,0x55,, +5.526649125,0x55,, +5.5267445,0x18,, +5.52684,0x00,, +5.5269355,0xAD,, +5.527031,0x6E,, +5.5271265,0xDC,, +5.527221875,0x00,, +5.527317375,0x2C,, +5.527412875,0x58,, +5.52750825,0x00,, +5.52760375,0x80,, +5.52769925,0x08,, +5.52779475,0x00,, +5.527890125,0xD5,, +5.527985625,0x42,, +5.528081125,0xAB,, +5.528176625,0x80,, +5.528272125,0x0D,, +5.5283675,0x54,, +5.528463,0x80,, +5.528558375,0x08,, +5.528653875,0x00,, +5.528749375,0x2A,, +5.528844875,0x62,, +5.528940375,0xA6,, +5.52903575,0xF9,, +5.546653875,0x55,, +5.546749375,0x55,, +5.54684475,0x18,, +5.54694025,0x00,, +5.54703575,0xAE,, +5.54713125,0x35,, +5.547226625,0xDB,, +5.547322125,0x00,, +5.547417625,0x2C,, +5.547513,0x58,, +5.5476085,0x00,, +5.547704,0x80,, +5.5477995,0x08,, +5.547895,0x00,, +5.547990375,0xD5,, +5.548085875,0x42,, +5.548181375,0xAB,, +5.54827675,0x80,, +5.54837225,0x0D,, +5.54846775,0x54,, +5.54856325,0x80,, +5.548658625,0x08,, +5.548754125,0x00,, +5.548849625,0x2A,, +5.548945125,0x62,, +5.5490405,0xA6,, +5.549136,0x41,, +5.566745375,0x55,, +5.566840875,0x55,, +5.566936375,0x18,, +5.56703175,0x00,, +5.56712725,0xAE,, +5.56722275,0xFC,, +5.56731825,0xDC,, +5.567413625,0x00,, +5.567509125,0x2C,, +5.567604625,0x48,, +5.567700125,0x00,, +5.5677955,0x80,, +5.567891,0x08,, +5.5679865,0x00,, +5.568081875,0x80,, +5.568177375,0x02,, +5.568272875,0xAB,, +5.568368375,0x80,, +5.568463875,0x0D,, +5.56855925,0x54,, +5.56865475,0x80,, +5.56875025,0x08,, +5.568845625,0x00,, +5.568941125,0x2A,, +5.569036625,0x62,, +5.569132125,0xA6,, +5.5692275,0x38,, +5.586845625,0x55,, +5.586941125,0x55,, +5.5870365,0x18,, +5.587132,0x00,, +5.5872275,0xAF,, +5.587323,0xC2,, +5.587418375,0xDB,, +5.587513875,0x00,, +5.587609375,0x2C,, +5.58770475,0x48,, +5.58780025,0x00,, +5.58789575,0x80,, +5.58799125,0x08,, +5.58808675,0x00,, +5.588182125,0x80,, +5.588277625,0x02,, +5.588373125,0xAB,, +5.5884685,0x80,, +5.588564,0x0D,, +5.5886595,0x54,, +5.588755,0x80,, +5.588850375,0x08,, +5.588945875,0x00,, +5.589041375,0x2A,, +5.589136875,0x62,, +5.58923225,0xA6,, +5.58932775,0xA6,, +5.606937125,0x55,, +5.607032625,0x55,, +5.607128125,0x18,, +5.6072235,0x00,, +5.607319,0xB0,, +5.6074145,0x88,, +5.60751,0xDC,, +5.607605375,0x00,, +5.607700875,0x2C,, +5.607796375,0x68,, +5.607891875,0x00,, +5.60798725,0x80,, +5.60808275,0x08,, +5.60817825,0x00,, +5.60827375,0x80,, +5.608369125,0x02,, +5.608464625,0xAB,, +5.608560125,0x80,, +5.608655625,0x0D,, +5.608751,0x54,, +5.6088465,0x80,, +5.608942,0x08,, +5.609037375,0x00,, +5.609132875,0x2A,, +5.609228375,0x62,, +5.609323875,0xA6,, +5.60941925,0x93,, +5.62744525,0x55,, +5.62754075,0x55,, +5.62763625,0x2B,, +5.627731625,0x03,, +5.627827125,0xB1,, +5.627922625,0x4F,, +5.628018125,0xDC,, +5.6281135,0x00,, +5.628209,0x2C,, +5.6283045,0x68,, +5.6284,0x00,, +5.628495375,0x80,, +5.628590875,0x08,, +5.628686375,0x00,, +5.628781875,0x2A,, +5.62887725,0xB2,, +5.62897275,0xAB,, +5.62906825,0x80,, +5.62916375,0x0D,, +5.629259125,0x54,, +5.629354625,0x80,, +5.629450125,0x08,, +5.629545625,0x00,, +5.629641,0x2A,, +5.6297365,0x62,, +5.629832,0xA6,, +5.629927375,0x00,, +5.630022875,0x00,, +5.630118375,0x00,, +5.630213875,0x00,, +5.630309375,0x00,, +5.63040475,0x00,, +5.63050025,0x00,, +5.63059575,0x00,, +5.630691125,0x00,, +5.630786625,0x00,, +5.630882125,0x00,, +5.630977625,0x00,, +5.631073,0x00,, +5.6311685,0x00,, +5.631264,0x00,, +5.6313595,0x00,, +5.631454875,0x00,, +5.631550375,0x00,, +5.631645875,0x00,, +5.63174125,0x25,, +5.647137625,0x55,, +5.647233125,0x55,, +5.6473285,0x18,, +5.647424,0x00,, +5.6475195,0xB2,, +5.647614875,0x15,, +5.647710375,0xDC,, +5.647805875,0x00,, +5.647901375,0x2C,, +5.64799675,0x68,, +5.64809225,0x00,, +5.64818775,0x80,, +5.64828325,0x08,, +5.648378625,0x00,, +5.648474125,0x2A,, +5.648569625,0xB2,, +5.648665,0xAB,, +5.6487605,0x80,, +5.648856,0x0D,, +5.6489515,0x54,, +5.649047,0x80,, +5.649142375,0x08,, +5.649237875,0x00,, +5.649333375,0x2A,, +5.649428875,0x62,, +5.64952425,0xA6,, +5.64961975,0xA1,, +5.667237875,0x55,, +5.66733325,0x55,, +5.66742875,0x18,, +5.66752425,0x00,, +5.667619625,0xB2,, +5.667715125,0xDB,, +5.667810625,0xDD,, +5.667906125,0x00,, +5.668001625,0x2C,, +5.668097,0x48,, +5.6681925,0x00,, +5.668288,0x80,, +5.668383375,0x08,, +5.668478875,0x00,, +5.668574375,0x2A,, +5.668669875,0xB2,, +5.66876525,0xAB,, +5.66886075,0x80,, +5.66895625,0x0D,, +5.66905175,0x54,, +5.669147125,0x80,, +5.669242625,0x08,, +5.669338125,0x00,, +5.6694335,0x2A,, +5.669529,0x62,, +5.6696245,0xA6,, +5.66972,0x50,, +5.687338,0x55,, +5.6874335,0x55,, +5.687529,0x18,, +5.6876245,0x00,, +5.687719875,0xB3,, +5.687815375,0xA1,, +5.687910875,0xDB,, +5.68800625,0x00,, +5.68810175,0x2C,, +5.68819725,0x78,, +5.68829275,0x00,, +5.688388125,0x80,, +5.688483625,0x08,, +5.688579125,0x00,, +5.688674625,0x2A,, +5.68877,0xB2,, +5.6888655,0xAB,, +5.688961,0x80,, +5.6890565,0x0D,, +5.689151875,0x54,, +5.689247375,0x80,, +5.689342875,0x08,, +5.689438375,0x00,, +5.68953375,0x2A,, +5.68962925,0x62,, +5.68972475,0xA6,, +5.68982025,0x4B,, +5.70743825,0x55,, +5.70753375,0x55,, +5.70762925,0x18,, +5.707724625,0x00,, +5.707820125,0xB4,, +5.707915625,0x67,, +5.708011,0xDB,, +5.7081065,0x00,, +5.708202,0x2C,, +5.7082975,0x68,, +5.708392875,0x00,, +5.708488375,0x80,, +5.708583875,0x08,, +5.708679375,0x00,, +5.70877475,0x2A,, +5.70887025,0xB2,, +5.70896575,0xAB,, +5.70906125,0x80,, +5.709156625,0x0D,, +5.709252125,0x54,, +5.709347625,0x80,, +5.709443125,0x08,, +5.7095385,0x00,, +5.709634,0x2A,, +5.7097295,0x62,, +5.709824875,0xA6,, +5.709920375,0x3A,, +5.72752975,0x55,, +5.72762525,0x55,, +5.72772075,0x18,, +5.72781625,0x00,, +5.727911625,0xB5,, +5.728007125,0x2C,, +5.728102625,0xDB,, +5.728198125,0x00,, +5.7282935,0x2C,, +5.728389,0x68,, +5.7284845,0x00,, +5.728579875,0x80,, +5.728675375,0x08,, +5.728770875,0x00,, +5.728866375,0x2A,, +5.728961875,0xB2,, +5.72905725,0xAB,, +5.72915275,0x80,, +5.72924825,0x0D,, +5.729343625,0x54,, +5.729439125,0x80,, +5.729534625,0x08,, +5.729630125,0x00,, +5.7297255,0x2A,, +5.729821,0x62,, +5.7299165,0xA6,, +5.730012,0x86,, +5.74763,0x55,, +5.7477255,0x55,, +5.747821,0x18,, +5.747916375,0x00,, +5.748011875,0xB5,, +5.748107375,0xF2,, +5.74820275,0xDC,, +5.74829825,0x00,, +5.74839375,0x2C,, +5.74848925,0x68,, +5.74858475,0x00,, +5.748680125,0x80,, +5.748775625,0x08,, +5.748871125,0x00,, +5.748966625,0x2A,, +5.749062,0xB2,, +5.7491575,0xAB,, +5.749253,0x80,, +5.749348375,0x0D,, +5.749443875,0x54,, +5.749539375,0x80,, +5.749634875,0x08,, +5.74973025,0x00,, +5.74982575,0x2A,, +5.74992125,0x62,, +5.75001675,0xA6,, +5.750112125,0x3C,, +5.76773025,0x55,, +5.76782575,0x55,, +5.767921125,0x18,, +5.768016625,0x00,, +5.768112125,0xB6,, +5.768207625,0xB9,, +5.768303,0xDB,, +5.7683985,0x00,, +5.768494,0x2C,, +5.7685895,0x68,, +5.768684875,0x00,, +5.768780375,0x80,, +5.768875875,0x08,, +5.76897125,0x00,, +5.76906675,0x2A,, +5.76916225,0xB2,, +5.76925775,0xAB,, +5.769353125,0x80,, +5.769448625,0x0D,, +5.769544125,0x54,, +5.769639625,0x80,, +5.769735,0x08,, +5.7698305,0x00,, +5.769926,0x2A,, +5.770021375,0x62,, +5.770116875,0xA6,, +5.770212375,0xE0,, +5.78782175,0x55,, +5.78791725,0x55,, +5.78801275,0x18,, +5.78810825,0x00,, +5.788203625,0xB7,, +5.788299125,0x7F,, +5.788394625,0xDB,, +5.78849,0x00,, +5.7885855,0x2C,, +5.788681,0x68,, +5.7887765,0x00,, +5.788871875,0x80,, +5.788967375,0x08,, +5.789062875,0x00,, +5.789158375,0x2A,, +5.78925375,0xB2,, +5.78934925,0xAB,, +5.78944475,0x80,, +5.789540125,0x0D,, +5.789635625,0x54,, +5.789731125,0x80,, +5.789826625,0x08,, +5.789922,0x00,, +5.7900175,0x2A,, +5.790113,0x62,, +5.7902085,0xA6,, +5.790303875,0x9A,, +5.807922,0x55,, +5.8080175,0x55,, +5.808112875,0x18,, +5.808208375,0x00,, +5.808303875,0xB8,, +5.808399375,0x46,, +5.80849475,0xDD,, +5.80859025,0x00,, +5.80868575,0x2C,, +5.80878125,0x68,, +5.808876625,0x00,, +5.808972125,0x80,, +5.809067625,0x08,, +5.809163,0x00,, +5.8092585,0x2A,, +5.809354,0xB2,, +5.8094495,0xAB,, +5.809545,0x80,, +5.809640375,0x0D,, +5.809735875,0x54,, +5.809831375,0x80,, +5.809926875,0x08,, +5.81002225,0x00,, +5.81011775,0x2A,, +5.81021325,0x62,, +5.810308625,0xA6,, +5.810404125,0x0F,, +5.828421375,0x55,, +5.828516875,0x55,, +5.828612375,0x2B,, +5.828707875,0x03,, +5.82880325,0xB9,, +5.82889875,0x0C,, +5.82899425,0xDC,, +5.82908975,0x00,, +5.82918525,0x2C,, +5.829280625,0x48,, +5.829376125,0x00,, +5.829471625,0x80,, +5.829567,0x08,, +5.8296625,0x00,, +5.829758,0x2A,, +5.8298535,0xB2,, +5.829948875,0xAB,, +5.830044375,0x80,, +5.830139875,0x0D,, +5.830235375,0x54,, +5.83033075,0x80,, +5.83042625,0x08,, +5.83052175,0x00,, +5.830617125,0x2A,, +5.830712625,0x62,, +5.830808125,0xA6,, +5.830903625,0x00,, +5.830999125,0x00,, +5.8310945,0x00,, +5.83119,0x00,, +5.8312855,0x00,, +5.831380875,0x00,, +5.831476375,0x00,, +5.831571875,0x00,, +5.831667375,0x00,, +5.83176275,0x00,, +5.83185825,0x00,, +5.83195375,0x00,, +5.83204925,0x00,, +5.832144625,0x00,, +5.832240125,0x00,, +5.832335625,0x00,, +5.832431125,0x00,, +5.8325265,0x00,, +5.832622,0x00,, +5.8327175,0xB4,, +5.84811375,0x55,, +5.84820925,0x55,, +5.84830475,0x18,, +5.848400125,0x00,, +5.848495625,0xB9,, +5.848591125,0xD3,, +5.8486865,0xDD,, +5.848782,0x00,, +5.8488775,0x2C,, +5.848973,0x68,, +5.849068375,0x00,, +5.849163875,0x80,, +5.849259375,0x08,, +5.849354875,0x00,, +5.84945025,0x2A,, +5.84954575,0xB2,, +5.84964125,0xAB,, +5.84973675,0x80,, +5.849832125,0x0D,, +5.849927625,0x54,, +5.850023125,0x80,, +5.850118625,0x08,, +5.850214,0x00,, +5.8503095,0x2A,, +5.850405,0x62,, +5.850500375,0xA6,, +5.850595875,0xCD,, +5.868214,0x55,, +5.868309375,0x55,, +5.868404875,0x18,, +5.868500375,0x00,, +5.868595875,0xBA,, +5.868691375,0x9A,, +5.86878675,0xDC,, +5.86888225,0x00,, +5.86897775,0x2C,, +5.869073125,0x48,, +5.869168625,0x00,, +5.869264125,0x80,, +5.869359625,0x08,, +5.869455,0x00,, +5.8695505,0x80,, +5.869646,0x02,, +5.8697415,0xAB,, +5.869836875,0x80,, +5.869932375,0x0D,, +5.870027875,0x54,, +5.870123375,0x80,, +5.87021875,0x08,, +5.87031425,0x00,, +5.87040975,0x2A,, +5.870505125,0x62,, +5.870600625,0xA6,, +5.870696125,0x85,, +5.8883055,0x55,, +5.888401,0x55,, +5.8884965,0x18,, +5.888591875,0x00,, +5.888687375,0xBB,, +5.888782875,0x60,, +5.88887825,0xDC,, +5.88897375,0x00,, +5.88906925,0x2C,, +5.88916475,0x68,, +5.88926025,0x00,, +5.889355625,0x80,, +5.889451125,0x08,, +5.889546625,0x00,, +5.889642,0x80,, +5.8897375,0x02,, +5.889833,0xAB,, +5.8899285,0x80,, +5.890023875,0x0D,, +5.890119375,0x54,, +5.890214875,0x80,, +5.890310375,0x08,, +5.89040575,0x00,, +5.89050125,0x2A,, +5.89059675,0x62,, +5.89069225,0xA6,, +5.890787625,0xBF,, +5.908414375,0x55,, +5.908509875,0x55,, +5.908605375,0x18,, +5.90870075,0x00,, +5.90879625,0xBC,, +5.90889175,0x26,, +5.90898725,0xDC,, +5.909082625,0x00,, +5.909178125,0x2C,, +5.909273625,0x68,, +5.909369125,0x00,, +5.909464625,0x80,, +5.90956,0x08,, +5.9096555,0x00,, +5.909751,0xD5,, +5.909846375,0x42,, +5.909941875,0xAB,, +5.910037375,0x80,, +5.910132875,0x0D,, +5.91022825,0x54,, +5.91032375,0x80,, +5.91041925,0x08,, +5.91051475,0x00,, +5.910610125,0x2A,, +5.910705625,0x62,, +5.910801125,0xA6,, +5.9108965,0x12,, +5.928514625,0x55,, +5.928610125,0x55,, +5.9287055,0x18,, +5.928801,0x00,, +5.9288965,0xBC,, +5.928992,0xED,, +5.9290875,0xDA,, +5.929182875,0x00,, +5.929278375,0x2C,, +5.929373875,0x68,, +5.92946925,0x00,, +5.92956475,0x80,, +5.92966025,0x08,, +5.92975575,0x00,, +5.929851125,0xD5,, +5.929946625,0x42,, +5.930042125,0xAB,, +5.930137625,0x80,, +5.930233,0x0D,, +5.9303285,0x54,, +5.930424,0x80,, +5.930519375,0x08,, +5.930614875,0x00,, +5.930710375,0x2A,, +5.930805875,0x62,, +5.930901375,0xA6,, +5.93099675,0x3D,, +5.948606125,0x55,, +5.948701625,0x55,, +5.948797125,0x18,, +5.948892625,0x00,, +5.948988,0xBD,, +5.9490835,0xB2,, +5.949179,0xDC,, +5.9492745,0x00,, +5.949369875,0x2C,, +5.949465375,0x68,, +5.949560875,0x00,, +5.949656375,0x80,, +5.94975175,0x08,, +5.94984725,0x00,, +5.94994275,0xD5,, +5.950038125,0x42,, +5.950133625,0xAB,, +5.950229125,0x80,, +5.950324625,0x0D,, +5.95042,0x54,, +5.9505155,0x80,, +5.950611,0x08,, +5.9507065,0x00,, +5.950801875,0x2A,, +5.950897375,0x62,, +5.950992875,0xA6,, +5.95108825,0x14,, +5.968706375,0x55,, +5.968801875,0x55,, +5.968897375,0x18,, +5.96899275,0x00,, +5.96908825,0xBE,, +5.96918375,0x79,, +5.96927925,0xDB,, +5.969374625,0x00,, +5.969470125,0x2C,, +5.969565625,0x58,, +5.969661125,0x00,, +5.9697565,0x80,, +5.969852,0x08,, +5.9699475,0x00,, +5.970042875,0xD5,, +5.970138375,0x42,, +5.970233875,0xAB,, +5.970329375,0x80,, +5.970424875,0x0D,, +5.97052025,0x54,, +5.97061575,0x80,, +5.97071125,0x08,, +5.970806625,0x00,, +5.970902125,0x2A,, +5.970997625,0x62,, +5.971093125,0xA6,, +5.9711885,0xC6,, +5.988806625,0x55,, +5.988902125,0x55,, +5.9889975,0x18,, +5.989093,0x00,, +5.9891885,0xBF,, +5.989284,0x3F,, +5.989379375,0xDC,, +5.989474875,0x00,, +5.989570375,0x2C,, +5.98966575,0x68,, +5.98976125,0x00,, +5.98985675,0x80,, +5.98995225,0x08,, +5.99004775,0x00,, +5.990143125,0xD5,, +5.990238625,0x42,, +5.990334125,0xAB,, +5.9904295,0x80,, +5.990525,0x0D,, +5.9906205,0x54,, +5.990716,0x80,, +5.990811375,0x08,, +5.990906875,0x00,, +5.991002375,0x2A,, +5.991097875,0x62,, +5.99119325,0xA6,, +5.99128875,0x76,, +6.008898125,0x55,, +6.008993625,0x55,, +6.009089125,0x18,, +6.0091845,0x00,, +6.00928,0xC0,, +6.0093755,0x06,, +6.009471,0xDC,, +6.009566375,0x00,, +6.009661875,0x2C,, +6.009757375,0x68,, +6.009852875,0x00,, +6.00994825,0x80,, +6.01004375,0x08,, +6.01013925,0x00,, +6.01023475,0xD5,, +6.010330125,0x42,, +6.010425625,0xAB,, +6.010521125,0x80,, +6.010616625,0x0D,, +6.010712,0x54,, +6.0108075,0x80,, +6.010903,0x08,, +6.010998375,0x00,, +6.011093875,0x2A,, +6.011189375,0x62,, +6.011284875,0xA6,, +6.01138025,0xAA,, +6.02940625,0x55,, +6.02950175,0x55,, +6.02959725,0x2B,, +6.029692625,0x03,, +6.029788125,0xC0,, +6.029883625,0xCC,, +6.029979125,0xDB,, +6.0300745,0x00,, +6.03017,0x2C,, +6.0302655,0x68,, +6.030361,0x00,, +6.030456375,0x80,, +6.030551875,0x08,, +6.030647375,0x00,, +6.03074275,0xD5,, +6.03083825,0x42,, +6.03093375,0xAB,, +6.03102925,0x80,, +6.03112475,0x0D,, +6.031220125,0x54,, +6.031315625,0x80,, +6.031411125,0x08,, +6.031506625,0x00,, +6.031602,0x2A,, +6.0316975,0x62,, +6.031793,0xA6,, +6.031888375,0x00,, +6.031983875,0x00,, +6.032079375,0x00,, +6.032174875,0x00,, +6.03227025,0x00,, +6.03236575,0x00,, +6.03246125,0x00,, +6.03255675,0x00,, +6.032652125,0x00,, +6.032747625,0x00,, +6.032843125,0x00,, +6.032938625,0x00,, +6.033034,0x00,, +6.0331295,0x00,, +6.033225,0x00,, +6.0333205,0x00,, +6.033415875,0x00,, +6.033511375,0x00,, +6.033606875,0x00,, +6.03370225,0xBA,, +6.049098625,0x55,, +6.049194125,0x55,, +6.0492895,0x18,, +6.049385,0x00,, +6.0494805,0xC1,, +6.049575875,0x92,, +6.049671375,0xDA,, +6.049766875,0x00,, +6.049862375,0x2C,, +6.04995775,0x68,, +6.05005325,0x00,, +6.05014875,0x80,, +6.05024425,0x08,, +6.050339625,0x00,, +6.050435125,0xD5,, +6.050530625,0x42,, +6.050626,0xAB,, +6.0507215,0x80,, +6.050817,0x0D,, +6.0509125,0x54,, +6.051008,0x80,, +6.051103375,0x08,, +6.051198875,0x00,, +6.051294375,0x2A,, +6.051389875,0x62,, +6.05148525,0xA6,, +6.05158075,0x44,, +6.069198875,0x55,, +6.06929425,0x55,, +6.06938975,0x18,, +6.06948525,0x00,, +6.069580625,0xC2,, +6.069676125,0x59,, +6.069771625,0xDC,, +6.069867125,0x00,, +6.0699625,0x2C,, +6.070058,0x68,, +6.0701535,0x00,, +6.070249,0x80,, +6.070344375,0x08,, +6.070439875,0x00,, +6.070535375,0xD5,, +6.070630875,0x42,, +6.07072625,0xAB,, +6.07082175,0x80,, +6.07091725,0x0D,, +6.07101275,0x54,, +6.071108125,0x80,, +6.071203625,0x08,, +6.071299125,0x00,, +6.0713945,0x2A,, +6.07149,0x62,, +6.0715855,0xA6,, +6.071681,0x9D,, +6.089299,0x55,, +6.0893945,0x55,, +6.08949,0x18,, +6.089585375,0x00,, +6.089680875,0xC3,, +6.089776375,0x1F,, +6.089871875,0xDC,, +6.08996725,0x00,, +6.09006275,0x2C,, +6.09015825,0x68,, +6.09025375,0x00,, +6.090349125,0x80,, +6.090444625,0x08,, +6.090540125,0x00,, +6.090635625,0xD5,, +6.090731,0x42,, +6.0908265,0xAB,, +6.090922,0x80,, +6.0910175,0x0D,, +6.091112875,0x54,, +6.091208375,0x80,, +6.091303875,0x08,, +6.09139925,0x00,, +6.09149475,0x2A,, +6.09159025,0x62,, +6.09168575,0xA6,, +6.09178125,0xCE,, +6.109390625,0x55,, +6.109486,0x55,, +6.1095815,0x18,, +6.109677,0x00,, +6.109772375,0xC3,, +6.109867875,0xE6,, +6.109963375,0xDB,, +6.110058875,0x00,, +6.110154375,0x2C,, +6.11024975,0x48,, +6.11034525,0x00,, +6.11044075,0x80,, +6.110536125,0x08,, +6.110631625,0x00,, +6.110727125,0xD5,, +6.110822625,0x42,, +6.110918,0xAB,, +6.1110135,0x80,, +6.111109,0x0D,, +6.1112045,0x54,, +6.111299875,0x80,, +6.111395375,0x08,, +6.111490875,0x00,, +6.11158625,0x2A,, +6.11168175,0x62,, +6.11177725,0xA6,, +6.11187275,0x29,, +6.12949075,0x55,, +6.12958625,0x55,, +6.12968175,0x18,, +6.12977725,0x00,, +6.129872625,0xC4,, +6.129968125,0xAD,, +6.130063625,0xDD,, +6.130159125,0x00,, +6.1302545,0x2C,, +6.13035,0x48,, +6.1304455,0x00,, +6.130540875,0x80,, +6.130636375,0x08,, +6.130731875,0x00,, +6.130827375,0xD5,, +6.13092275,0x42,, +6.13101825,0xAB,, +6.13111375,0x80,, +6.13120925,0x0D,, +6.131304625,0x54,, +6.131400125,0x80,, +6.131495625,0x08,, +6.131591125,0x00,, +6.1316865,0x2A,, +6.131782,0x62,, +6.1318775,0xA6,, +6.131973,0x96,, +6.149591,0x55,, +6.1496865,0x55,, +6.149782,0x18,, +6.149877375,0x00,, +6.149972875,0xC5,, +6.150068375,0x73,, +6.15016375,0xDD,, +6.15025925,0x00,, +6.15035475,0x2C,, +6.15045025,0x68,, +6.150545625,0x00,, +6.150641125,0x80,, +6.150736625,0x08,, +6.150832125,0x00,, +6.150927625,0xD5,, +6.151023,0x42,, +6.1511185,0xAB,, +6.151214,0x80,, +6.151309375,0x0D,, +6.151404875,0x54,, +6.151500375,0x80,, +6.151595875,0x08,, +6.15169125,0x00,, +6.15178675,0x2A,, +6.15188225,0x62,, +6.15197775,0xA6,, +6.152073125,0x7D,, +6.16969125,0x55,, +6.16978675,0x55,, +6.169882125,0x18,, +6.169977625,0x00,, +6.170073125,0xC6,, +6.170168625,0x39,, +6.170264,0xDB,, +6.1703595,0x00,, +6.170455,0x2C,, +6.1705505,0x68,, +6.170645875,0x00,, +6.170741375,0x80,, +6.170836875,0x08,, +6.17093225,0x00,, +6.17102775,0xD5,, +6.17112325,0x42,, +6.17121875,0xAB,, +6.171314125,0x80,, +6.171409625,0x0D,, +6.171505125,0x54,, +6.171600625,0x80,, +6.171696,0x08,, +6.1717915,0x00,, +6.171887,0x2A,, +6.171982375,0x62,, +6.172077875,0xA6,, +6.172173375,0x49,, +6.18978275,0x55,, +6.18987825,0x55,, +6.18997375,0x18,, +6.190069125,0x00,, +6.190164625,0xC6,, +6.190260125,0xFF,, +6.190355625,0xDD,, +6.190451,0x00,, +6.1905465,0x2C,, +6.190642,0x48,, +6.1907375,0x00,, +6.190832875,0x80,, +6.190928375,0x08,, +6.191023875,0x00,, +6.191119375,0xD5,, +6.19121475,0x42,, +6.19131025,0xAB,, +6.19140575,0x80,, +6.191501125,0x0D,, +6.191596625,0x54,, +6.191692125,0x80,, +6.191787625,0x08,, +6.191883,0x00,, +6.1919785,0x2A,, +6.192074,0x62,, +6.1921695,0xA6,, +6.192264875,0x4E,, +6.209883,0x55,, +6.2099785,0x55,, +6.210073875,0x18,, +6.210169375,0x00,, +6.210264875,0xC7,, +6.210360375,0xC5,, +6.21045575,0xDD,, +6.21055125,0x00,, +6.21064675,0x2C,, +6.21074225,0x58,, +6.210837625,0x00,, +6.210933125,0x80,, +6.211028625,0x08,, +6.211124,0x00,, +6.2112195,0xD5,, +6.211315,0x42,, +6.2114105,0xAB,, +6.211505875,0x80,, +6.211601375,0x0D,, +6.211696875,0x54,, +6.211792375,0x80,, +6.211887875,0x08,, +6.21198325,0x00,, +6.21207875,0x2A,, +6.21217425,0x62,, +6.212269625,0xA6,, +6.212365125,0xED,, +6.230382375,0x55,, +6.230477875,0x55,, +6.230573375,0x2B,, +6.230668875,0x03,, +6.23076425,0xC8,, +6.23085975,0x8B,, +6.23095525,0xDC,, +6.23105075,0x00,, +6.23114625,0x2C,, +6.231241625,0x68,, +6.231337125,0x00,, +6.231432625,0x80,, +6.231528,0x08,, +6.2316235,0x00,, +6.231719,0xD5,, +6.2318145,0x4D,, +6.231909875,0x38,, +6.232005375,0x80,, +6.232100875,0x0D,, +6.232196375,0x54,, +6.23229175,0x80,, +6.23238725,0x08,, +6.23248275,0x00,, +6.232578125,0x2A,, +6.232673625,0x62,, +6.232769125,0xA6,, +6.232864625,0x00,, +6.23296,0x00,, +6.2330555,0x00,, +6.233151,0x00,, +6.2332465,0x00,, +6.233341875,0x00,, +6.233437375,0x00,, +6.233532875,0x00,, +6.233628375,0x00,, +6.23372375,0x00,, +6.23381925,0x00,, +6.23391475,0x00,, +6.23401025,0x00,, +6.234105625,0x00,, +6.234201125,0x00,, +6.234296625,0x00,, +6.234392125,0x00,, +6.2344875,0x00,, +6.234583,0x00,, +6.2346785,0x1C,, +6.250083375,0x55,, +6.250178875,0x55,, +6.250274375,0x18,, +6.250369875,0x00,, +6.250465375,0xC9,, +6.25056075,0x50,, +6.25065625,0xDC,, +6.250751625,0x00,, +6.250847125,0x2C,, +6.250942625,0x78,, +6.251038125,0x00,, +6.251133625,0x80,, +6.251229,0x08,, +6.2513245,0x00,, +6.25142,0xD5,, +6.2515155,0x4F,, +6.251610875,0xFF,, +6.251706375,0x80,, +6.251801875,0x0D,, +6.25189725,0x54,, +6.25199275,0x80,, +6.25208825,0x08,, +6.25218375,0x00,, +6.252279125,0x2A,, +6.252374625,0x62,, +6.252470125,0xA6,, +6.252565625,0x0B,, +6.270183625,0x55,, +6.270279125,0x55,, +6.270374625,0x18,, +6.27047,0x00,, +6.2705655,0xCA,, +6.270661,0x17,, +6.2707565,0xDB,, +6.270851875,0x00,, +6.270947375,0x2C,, +6.271042875,0x68,, +6.271138375,0x00,, +6.27123375,0x80,, +6.27132925,0x08,, +6.27142475,0x00,, +6.271520125,0xD5,, +6.271615625,0x4F,, +6.271711125,0xFF,, +6.271806625,0x80,, +6.271902125,0x0D,, +6.2719975,0x54,, +6.272093,0x80,, +6.2721885,0x08,, +6.272284,0x00,, +6.272379375,0x2A,, +6.272474875,0x62,, +6.272570375,0xA6,, +6.27266575,0x1C,, +6.290283875,0x55,, +6.290379375,0x55,, +6.29047475,0x18,, +6.29057025,0x00,, +6.29066575,0xCA,, +6.29076125,0xDE,, +6.290856625,0xDB,, +6.290952125,0x00,, +6.291047625,0x2C,, +6.291143125,0x68,, +6.2912385,0x00,, +6.291334,0x80,, +6.2914295,0x08,, +6.291525,0x00,, +6.291620375,0xD5,, +6.291715875,0x4F,, +6.291811375,0xFF,, +6.291906875,0x80,, +6.29200225,0x0D,, +6.29209775,0x54,, +6.29219325,0x80,, +6.292288625,0x08,, +6.292384125,0x00,, +6.292479625,0x2A,, +6.292575125,0x62,, +6.2926705,0xA6,, +6.292766,0x54,, +6.310375375,0x55,, +6.310470875,0x55,, +6.310566375,0x18,, +6.31066175,0x00,, +6.31075725,0xCB,, +6.31085275,0xA5,, +6.31094825,0xDC,, +6.311043625,0x00,, +6.311139125,0x2C,, +6.311234625,0x68,, +6.311330125,0x00,, +6.311425625,0x80,, +6.311521,0x08,, +6.3116165,0x00,, +6.311712,0xD5,, +6.311807375,0x4F,, +6.311902875,0xFF,, +6.311998375,0x80,, +6.312093875,0x0D,, +6.31218925,0x54,, +6.31228475,0x80,, +6.31238025,0x08,, +6.31247575,0x00,, +6.312571125,0x2A,, +6.312666625,0x62,, +6.312762125,0xA6,, +6.3128575,0x80,, +6.330475625,0x55,, +6.330571125,0x55,, +6.3306665,0x18,, +6.330762,0x00,, +6.3308575,0xCC,, +6.330953,0x6B,, +6.3310485,0xDB,, +6.331143875,0x00,, +6.331239375,0x2C,, +6.331334875,0x68,, +6.33143025,0x00,, +6.33152575,0x80,, +6.33162125,0x08,, +6.33171675,0x00,, +6.331812125,0xD5,, +6.331907625,0x4F,, +6.332003125,0xFF,, +6.332098625,0x80,, +6.332194,0x0D,, +6.3322895,0x54,, +6.332385,0x80,, +6.332480375,0x08,, +6.332575875,0x00,, +6.332671375,0x2A,, +6.332766875,0x62,, +6.33286225,0xA6,, +6.33295775,0xE7,, +6.350575875,0x55,, +6.350671375,0x55,, +6.35076675,0x18,, +6.35086225,0x00,, +6.35095775,0xCD,, +6.35105325,0x30,, +6.351148625,0xDB,, +6.351244125,0x00,, +6.351339625,0x2C,, +6.351435,0x58,, +6.3515305,0x00,, +6.351626,0x80,, +6.3517215,0x08,, +6.351816875,0x00,, +6.351912375,0xD5,, +6.352007875,0x4F,, +6.352103375,0xFF,, +6.35219875,0x80,, +6.35229425,0x0D,, +6.35238975,0x54,, +6.35248525,0x80,, +6.352580625,0x08,, +6.352676125,0x00,, +6.352771625,0x2A,, +6.352867125,0x62,, +6.3529625,0xA6,, +6.353058,0x18,, +6.370676125,0x55,, +6.3707715,0x55,, +6.370867,0x18,, +6.3709625,0x00,, +6.371057875,0xCD,, +6.371153375,0xF5,, +6.371248875,0xDC,, +6.371344375,0x00,, +6.37143975,0x2C,, +6.37153525,0x78,, +6.37163075,0x00,, +6.37172625,0x80,, +6.37182175,0x08,, +6.371917125,0x00,, +6.372012625,0xD5,, +6.372108125,0x4F,, +6.3722035,0xFF,, +6.372299,0x80,, +6.3723945,0x0D,, +6.37249,0x54,, +6.372585375,0x80,, +6.372680875,0x08,, +6.372776375,0x00,, +6.372871875,0x2A,, +6.37296725,0x62,, +6.37306275,0xA6,, +6.37315825,0x78,, +6.390767625,0x55,, +6.390863125,0x55,, +6.3909585,0x18,, +6.391054,0x00,, +6.3911495,0xCE,, +6.391245,0xBB,, +6.391340375,0xDB,, +6.391435875,0x00,, +6.391531375,0x2C,, +6.39162675,0x78,, +6.39172225,0x00,, +6.39181775,0x80,, +6.39191325,0x08,, +6.39200875,0x00,, +6.392104125,0xD5,, +6.392199625,0x4F,, +6.392295125,0xFF,, +6.3923905,0x80,, +6.392486,0x0D,, +6.3925815,0x54,, +6.392677,0x80,, +6.392772375,0x08,, +6.392867875,0x00,, +6.392963375,0x2A,, +6.393058875,0x62,, +6.39315425,0xA6,, +6.39324975,0x79,, +6.410867875,0x55,, +6.41096325,0x55,, +6.41105875,0x18,, +6.41115425,0x00,, +6.41124975,0xCF,, +6.411345125,0x81,, +6.411440625,0xDD,, +6.411536125,0x00,, +6.411631625,0x2C,, +6.411727,0x78,, +6.4118225,0x00,, +6.411918,0x80,, +6.4120135,0x08,, +6.412108875,0x00,, +6.412204375,0xD5,, +6.412299875,0x4F,, +6.41239525,0xFF,, +6.41249075,0x80,, +6.41258625,0x0D,, +6.41268175,0x54,, +6.412777125,0x80,, +6.412872625,0x08,, +6.412968125,0x00,, +6.413063625,0x2A,, +6.413159,0x62,, +6.4132545,0xA6,, +6.41335,0xD2,, +6.43136725,0x55,, +6.43146275,0x55,, +6.43155825,0x2B,, +6.431653625,0x03,, +6.431749125,0xD0,, +6.431844625,0x47,, +6.431940125,0xDA,, +6.4320355,0x00,, +6.432131,0x2C,, +6.4322265,0x78,, +6.432322,0x00,, +6.432417375,0x80,, +6.432512875,0x08,, +6.432608375,0x00,, +6.43270375,0xD5,, +6.43279925,0x4F,, +6.43289475,0xFF,, +6.43299025,0x80,, +6.43308575,0x0D,, +6.433181125,0x54,, +6.433276625,0x80,, +6.433372125,0x08,, +6.433467625,0x00,, +6.433563,0x2A,, +6.4336585,0x62,, +6.433754,0xA6,, +6.433849375,0x00,, +6.433944875,0x00,, +6.434040375,0x00,, +6.434135875,0x00,, +6.43423125,0x00,, +6.43432675,0x00,, +6.43442225,0x00,, +6.43451775,0x00,, +6.434613125,0x00,, +6.434708625,0x00,, +6.434804125,0x00,, +6.4348995,0x00,, +6.434995,0x00,, +6.4350905,0x00,, +6.435186,0x00,, +6.4352815,0x00,, +6.435376875,0x00,, +6.435472375,0x00,, +6.435567875,0x00,, +6.43566325,0x3F,, +6.45106825,0x55,, +6.45116375,0x55,, +6.45125925,0x18,, +6.451354625,0x00,, +6.451450125,0xD1,, +6.451545625,0x0E,, +6.451641125,0xDA,, +6.4517365,0x00,, +6.451832,0x2C,, +6.4519275,0x68,, +6.452022875,0x00,, +6.452118375,0x80,, +6.452213875,0x08,, +6.452309375,0x00,, +6.452404875,0xD5,, +6.45250025,0x4F,, +6.45259575,0xFF,, +6.45269125,0x80,, +6.452786625,0x0D,, +6.452882125,0x54,, +6.452977625,0x80,, +6.453073125,0x08,, +6.4531685,0x00,, +6.453264,0x2A,, +6.4533595,0x62,, +6.453455,0xA6,, +6.453550375,0xF1,, +6.471159875,0x55,, +6.47125525,0x55,, +6.47135075,0x18,, +6.47144625,0x00,, +6.471541625,0xD1,, +6.471637125,0xD3,, +6.471732625,0xDC,, +6.471828125,0x00,, +6.4719235,0x2C,, +6.472019,0x58,, +6.4721145,0x00,, +6.47221,0x80,, +6.472305375,0x08,, +6.472400875,0x00,, +6.472496375,0xD5,, +6.472591875,0x4F,, +6.47268725,0xFF,, +6.47278275,0x80,, +6.47287825,0x0D,, +6.47297375,0x54,, +6.473069125,0x80,, +6.473164625,0x08,, +6.473260125,0x00,, +6.4733555,0x2A,, +6.473451,0x62,, +6.4735465,0xA6,, +6.473642,0x0B,, +6.49126,0x55,, +6.4913555,0x55,, +6.491451,0x18,, +6.491546375,0x00,, +6.491641875,0xD2,, +6.491737375,0x99,, +6.491832875,0xDB,, +6.49192825,0x00,, +6.49202375,0x2C,, +6.49211925,0x48,, +6.49221475,0x00,, +6.492310125,0x80,, +6.492405625,0x08,, +6.492501125,0x00,, +6.492596625,0xD5,, +6.492692,0x4F,, +6.4927875,0xFF,, +6.492883,0x80,, +6.4929785,0x0D,, +6.493073875,0x54,, +6.493169375,0x80,, +6.493264875,0x08,, +6.49336025,0x00,, +6.49345575,0x2A,, +6.49355125,0x62,, +6.49364675,0xA6,, +6.49374225,0xF3,, +6.511351625,0x55,, +6.511447,0x55,, +6.5115425,0x18,, +6.511638,0x00,, +6.511733375,0xD3,, +6.511828875,0x5F,, +6.511924375,0xDC,, +6.512019875,0x00,, +6.512115375,0x2C,, +6.51221075,0x68,, +6.51230625,0x00,, +6.51240175,0x80,, +6.512497125,0x08,, +6.512592625,0x00,, +6.512688125,0xD5,, +6.512783625,0x4D,, +6.512879,0x38,, +6.5129745,0x80,, +6.51307,0x0D,, +6.5131655,0x54,, +6.513260875,0x80,, +6.513356375,0x08,, +6.513451875,0x00,, +6.51354725,0x2A,, +6.51364275,0x62,, +6.51373825,0xA6,, +6.51383375,0x61,, +6.53145175,0x55,, +6.53154725,0x55,, +6.53164275,0x18,, +6.53173825,0x00,, +6.531833625,0xD4,, +6.531929125,0x25,, +6.532024625,0xDD,, +6.532120125,0x00,, +6.5322155,0x2C,, +6.532311,0x48,, +6.5324065,0x00,, +6.532501875,0x80,, +6.532597375,0x08,, +6.532692875,0x00,, +6.532788375,0xD5,, +6.53288375,0x42,, +6.53297925,0xAB,, +6.53307475,0x80,, +6.53317025,0x0D,, +6.533265625,0x54,, +6.533361125,0x80,, +6.533456625,0x08,, +6.533552125,0x00,, +6.5336475,0x2A,, +6.533743,0x62,, +6.5338385,0xA6,, +6.533934,0xB6,, +6.551552,0x55,, +6.5516475,0x55,, +6.551743,0x18,, +6.551838375,0x00,, +6.551933875,0xD4,, +6.552029375,0xEC,, +6.55212475,0xDB,, +6.55222025,0x00,, +6.55231575,0x2C,, +6.55241125,0x58,, +6.552506625,0x00,, +6.552602125,0x80,, +6.552697625,0x08,, +6.552793125,0x00,, +6.552888625,0xD5,, +6.552984,0x42,, +6.5530795,0xAB,, +6.553175,0x80,, +6.553270375,0x0D,, +6.553365875,0x54,, +6.553461375,0x80,, +6.553556875,0x08,, +6.55365225,0x00,, +6.55374775,0x2A,, +6.55384325,0x62,, +6.55393875,0xA6,, +6.554034125,0xF6,, +6.5716435,0x55,, +6.571739,0x55,, +6.5718345,0x18,, +6.57193,0x00,, +6.572025375,0xD5,, +6.572120875,0xB3,, +6.572216375,0xDB,, +6.572311875,0x00,, +6.57240725,0x2C,, +6.57250275,0x68,, +6.57259825,0x00,, +6.572693625,0x80,, +6.572789125,0x08,, +6.572884625,0x00,, +6.572980125,0xD5,, +6.5730755,0x42,, +6.573171,0xAB,, +6.5732665,0x80,, +6.573362,0x0D,, +6.573457375,0x54,, +6.573552875,0x80,, +6.573648375,0x08,, +6.573743875,0x00,, +6.57383925,0x2A,, +6.57393475,0x62,, +6.57403025,0xA6,, +6.57412575,0x10,, +6.59174375,0x55,, +6.59183925,0x55,, +6.59193475,0x18,, +6.592030125,0x00,, +6.592125625,0xD6,, +6.592221125,0x79,, +6.592316625,0xDB,, +6.592412,0x00,, +6.5925075,0x2C,, +6.592603,0x78,, +6.5926985,0x00,, +6.592793875,0x80,, +6.592889375,0x08,, +6.592984875,0x00,, +6.593080375,0xD5,, +6.59317575,0x42,, +6.59327125,0xAB,, +6.59336675,0x80,, +6.593462125,0x0D,, +6.593557625,0x54,, +6.593653125,0x80,, +6.593748625,0x08,, +6.593844,0x00,, +6.5939395,0x2A,, +6.594035,0x62,, +6.5941305,0xA6,, +6.594225875,0x05,, +6.611844,0x55,, +6.6119395,0x55,, +6.612034875,0x18,, +6.612130375,0x00,, +6.612225875,0xD7,, +6.612321375,0x40,, +6.61241675,0xDA,, +6.61251225,0x00,, +6.61260775,0x2C,, +6.61270325,0x68,, +6.612798625,0x00,, +6.612894125,0x80,, +6.612989625,0x08,, +6.613085,0x00,, +6.6131805,0xD5,, +6.613276,0x42,, +6.6133715,0xAB,, +6.613466875,0x80,, +6.613562375,0x0D,, +6.613657875,0x54,, +6.613753375,0x80,, +6.613848875,0x08,, +6.61394425,0x00,, +6.61403975,0x2A,, +6.61413525,0x62,, +6.614230625,0xA6,, +6.614326125,0xC1,, +6.63233475,0x55,, +6.63243025,0x55,, +6.632525625,0x2B,, +6.632621125,0x03,, +6.632716625,0xD8,, +6.632812125,0x06,, +6.632907625,0xDB,, +6.633003,0x00,, +6.6330985,0x2C,, +6.633194,0x68,, +6.633289375,0x00,, +6.633384875,0x80,, +6.633480375,0x08,, +6.633575875,0x00,, +6.63367125,0xD5,, +6.63376675,0x42,, +6.63386225,0xAB,, +6.63395775,0x80,, +6.634053125,0x0D,, +6.634148625,0x54,, +6.634244125,0x80,, +6.6343395,0x08,, +6.634435,0x00,, +6.6345305,0x2A,, +6.634626,0x62,, +6.6347215,0xA6,, +6.634816875,0x00,, +6.634912375,0x00,, +6.635007875,0x00,, +6.63510325,0x00,, +6.63519875,0x00,, +6.63529425,0x00,, +6.63538975,0x00,, +6.635485125,0x00,, +6.635580625,0x00,, +6.635676125,0x00,, +6.635771625,0x00,, +6.635867,0x00,, +6.6359625,0x00,, +6.636058,0x00,, +6.6361535,0x00,, +6.636248875,0x00,, +6.636344375,0x00,, +6.636439875,0x00,, +6.636535375,0x00,, +6.63663075,0xD0,, +6.652044375,0x55,, +6.652139875,0x55,, +6.652235375,0x18,, +6.652330875,0x00,, +6.65242625,0xD8,, +6.65252175,0xCB,, +6.65261725,0xDC,, +6.652712625,0x00,, +6.652808125,0x2C,, +6.652903625,0x48,, +6.652999125,0x00,, +6.653094625,0x80,, +6.65319,0x08,, +6.6532855,0x00,, +6.653381,0xD5,, +6.6534765,0x42,, +6.653571875,0xAB,, +6.653667375,0x80,, +6.653762875,0x0D,, +6.65385825,0x54,, +6.65395375,0x80,, +6.65404925,0x08,, +6.65414475,0x00,, +6.654240125,0x2A,, +6.654335625,0x62,, +6.654431125,0xA6,, +6.654526625,0x99,, +6.672136,0x55,, +6.672231375,0x55,, +6.672326875,0x18,, +6.672422375,0x00,, +6.672517875,0xD9,, +6.67261325,0x90,, +6.67270875,0xDB,, +6.67280425,0x00,, +6.67289975,0x2C,, +6.672995125,0x68,, +6.673090625,0x00,, +6.673186125,0x80,, +6.673281625,0x08,, +6.673377,0x00,, +6.6734725,0xD5,, +6.673568,0x42,, +6.6736635,0xAB,, +6.673758875,0x80,, +6.673854375,0x0D,, +6.673949875,0x54,, +6.674045375,0x80,, +6.67414075,0x08,, +6.67423625,0x00,, +6.67433175,0x2A,, +6.674427125,0x62,, +6.674522625,0xA6,, +6.674618125,0x42,, +6.692236125,0x55,, +6.692331625,0x55,, +6.692427125,0x18,, +6.692522625,0x00,, +6.692618125,0xDA,, +6.6927135,0x57,, +6.692809,0xDB,, +6.6929045,0x00,, +6.692999875,0x2C,, +6.693095375,0x68,, +6.693190875,0x00,, +6.693286375,0x80,, +6.69338175,0x08,, +6.69347725,0x00,, +6.69357275,0xD5,, +6.69366825,0x42,, +6.693763625,0xAB,, +6.693859125,0x80,, +6.693954625,0x0D,, +6.69405,0x54,, +6.6941455,0x80,, +6.694241,0x08,, +6.6943365,0x00,, +6.694432,0x2A,, +6.694527375,0x62,, +6.694622875,0xA6,, +6.694718375,0x58,, +6.712336375,0x55,, +6.712431875,0x55,, +6.712527375,0x18,, +6.71262275,0x00,, +6.71271825,0xDB,, +6.71281375,0x1D,, +6.71290925,0xDC,, +6.713004625,0x00,, +6.713100125,0x2C,, +6.713195625,0x78,, +6.713291125,0x00,, +6.7133865,0x80,, +6.713482,0x08,, +6.7135775,0x00,, +6.713673,0xD5,, +6.713768375,0x42,, +6.713863875,0xAB,, +6.713959375,0x80,, +6.714054875,0x0D,, +6.71415025,0x54,, +6.71424575,0x80,, +6.71434125,0x08,, +6.71443675,0x00,, +6.714532125,0x2A,, +6.714627625,0x62,, +6.714723125,0xA6,, +6.7148185,0x04,, +6.732436625,0x55,, +6.732532125,0x55,, +6.7326275,0x18,, +6.732723,0x00,, +6.7328185,0xDB,, +6.732914,0xE3,, +6.733009375,0xDB,, +6.733104875,0x00,, +6.733200375,0x2C,, +6.733295875,0x48,, +6.73339125,0x00,, +6.73348675,0x80,, +6.73358225,0x08,, +6.73367775,0x00,, +6.733773125,0xD5,, +6.733868625,0x42,, +6.733964125,0xAB,, +6.734059625,0x80,, +6.734155,0x0D,, +6.7342505,0x54,, +6.734346,0x80,, +6.734441375,0x08,, +6.734536875,0x00,, +6.734632375,0x2A,, +6.734727875,0x62,, +6.73482325,0xA6,, +6.73491875,0x51,, +6.752528125,0x55,, +6.752623625,0x55,, +6.752719125,0x18,, +6.752814625,0x00,, +6.75291,0xDC,, +6.7530055,0xA9,, +6.753101,0xDB,, +6.753196375,0x01,, +6.753291875,0x2C,, +6.753387375,0x48,, +6.753482875,0x00,, +6.753578375,0x80,, +6.75367375,0x08,, +6.75376925,0x00,, +6.75386475,0xD5,, +6.753960125,0x42,, +6.754055625,0xAB,, +6.754151125,0x80,, +6.754246625,0x0D,, +6.754342,0x54,, +6.7544375,0x80,, +6.754533,0x08,, +6.7546285,0x00,, +6.754723875,0x2A,, +6.754819375,0x62,, +6.754914875,0xA6,, +6.75501025,0x14,, +6.772628375,0x55,, +6.772723875,0x55,, +6.77281925,0x18,, +6.77291475,0x00,, +6.77301025,0xDD,, +6.77310575,0x6E,, +6.77320125,0xDB,, +6.773296625,0x00,, +6.773392125,0x2C,, +6.773487625,0x68,, +6.773583125,0x00,, +6.7736785,0x80,, +6.773774,0x08,, +6.7738695,0x00,, +6.773964875,0xD5,, +6.774060375,0x42,, +6.774155875,0xAB,, +6.774251375,0x80,, +6.77434675,0x0D,, +6.77444225,0x54,, +6.77453775,0x80,, +6.77463325,0x08,, +6.774728625,0x00,, +6.774824125,0x2A,, +6.774919625,0x62,, +6.775015125,0xA6,, +6.7751105,0xBB,, +6.792728625,0x55,, +6.792824125,0x55,, +6.7929195,0x18,, +6.793015,0x00,, +6.7931105,0xDE,, +6.793206,0x34,, +6.793301375,0xDC,, +6.793396875,0x00,, +6.793492375,0x2C,, +6.79358775,0x68,, +6.79368325,0x00,, +6.79377875,0x80,, +6.79387425,0x08,, +6.793969625,0x00,, +6.794065125,0xD5,, +6.794160625,0x42,, +6.794256125,0xAB,, +6.7943515,0x80,, +6.794447,0x0D,, +6.7945425,0x54,, +6.794638,0x80,, +6.794733375,0x08,, +6.794828875,0x00,, +6.794924375,0x2A,, +6.795019875,0x62,, +6.79511525,0xA6,, +6.79521075,0xC7,, +6.812828875,0x55,, +6.81292425,0x55,, +6.81301975,0x18,, +6.81311525,0x00,, +6.81321075,0xDE,, +6.813306125,0xFB,, +6.813401625,0xDB,, +6.813497125,0x00,, +6.8135925,0x2C,, +6.813688,0x78,, +6.8137835,0x00,, +6.813879,0x80,, +6.8139745,0x08,, +6.814069875,0x00,, +6.814165375,0xD5,, +6.814260875,0x42,, +6.81435625,0xAB,, +6.81445175,0x80,, +6.81454725,0x0D,, +6.81464275,0x54,, +6.814738125,0x80,, +6.814833625,0x08,, +6.814929125,0x00,, +6.815024625,0x2A,, +6.81512,0x62,, +6.8152155,0xA6,, +6.815311,0x3D,, +6.833319625,0x55,, +6.833415,0x55,, +6.8335105,0x2B,, +6.833606,0x03,, +6.8337015,0xDF,, +6.833796875,0xC2,, +6.833892375,0xDD,, +6.833987875,0x00,, +6.834083375,0x2C,, +6.83417875,0x68,, +6.83427425,0x00,, +6.83436975,0x80,, +6.83446525,0x08,, +6.834560625,0x00,, +6.834656125,0xD5,, +6.834751625,0x42,, +6.834847125,0xAB,, +6.8349425,0x80,, +6.835038,0x0D,, +6.8351335,0x54,, +6.835229,0x80,, +6.835324375,0x08,, +6.835419875,0x00,, +6.835515375,0x2A,, +6.83561075,0x62,, +6.83570625,0xA6,, +6.83580175,0x00,, +6.83589725,0x00,, +6.83599275,0x00,, +6.836088125,0x00,, +6.836183625,0x00,, +6.836279125,0x00,, +6.8363745,0x00,, +6.83647,0x00,, +6.8365655,0x00,, +6.836661,0x00,, +6.836756375,0x00,, +6.836851875,0x00,, +6.836947375,0x00,, +6.837042875,0x00,, +6.83713825,0x00,, +6.83723375,0x00,, +6.83732925,0x00,, +6.837424625,0x00,, +6.837520125,0x00,, +6.837615625,0xF2,, +6.853020625,0x55,, +6.853116,0x55,, +6.8532115,0x18,, +6.853307,0x00,, +6.8534025,0xE0,, +6.853497875,0x88,, +6.853593375,0xDB,, +6.853688875,0x00,, +6.853784375,0x2C,, +6.85387975,0x68,, +6.85397525,0x00,, +6.85407075,0x80,, +6.85416625,0x08,, +6.854261625,0x00,, +6.854357125,0xD5,, +6.854452625,0x42,, +6.854548,0xAB,, +6.8546435,0x80,, +6.854739,0x0D,, +6.8548345,0x54,, +6.854929875,0x80,, +6.855025375,0x08,, +6.855120875,0x00,, +6.855216375,0x2A,, +6.855311875,0x62,, +6.85540725,0xA6,, +6.85550275,0x95,, +6.873120875,0x55,, +6.87321625,0x55,, +6.87331175,0x18,, +6.87340725,0x00,, +6.873502625,0xE1,, +6.873598125,0x4E,, +6.873693625,0xDB,, +6.873789125,0x00,, +6.8738845,0x2C,, +6.87398,0x68,, +6.8740755,0x00,, +6.874171,0x80,, +6.874266375,0x08,, +6.874361875,0x00,, +6.874457375,0xD5,, +6.87455275,0x42,, +6.87464825,0xAB,, +6.87474375,0x80,, +6.87483925,0x0D,, +6.87493475,0x54,, +6.875030125,0x80,, +6.875125625,0x08,, +6.875221125,0x00,, +6.8753165,0x2A,, +6.875412,0x62,, +6.8755075,0xA6,, +6.875603,0xEF,, +6.893221,0x55,, +6.8933165,0x55,, +6.893412,0x18,, +6.893507375,0x00,, +6.893602875,0xE2,, +6.893698375,0x14,, +6.893793875,0xDB,, +6.89388925,0x00,, +6.89398475,0x2C,, +6.89408025,0x48,, +6.894175625,0x00,, +6.894271125,0x80,, +6.894366625,0x08,, +6.894462125,0x00,, +6.894557625,0xD5,, +6.894653,0x42,, +6.8947485,0xAB,, +6.894844,0x80,, +6.8949395,0x0D,, +6.895034875,0x54,, +6.895130375,0x80,, +6.895225875,0x08,, +6.89532125,0x00,, +6.89541675,0x2A,, +6.89551225,0x62,, +6.89560775,0xA6,, +6.895703125,0x90,, +6.913312625,0x55,, +6.913408,0x55,, +6.9135035,0x18,, +6.913599,0x00,, +6.913694375,0xE2,, +6.913789875,0xDA,, +6.913885375,0xDD,, +6.913980875,0x00,, +6.91407625,0x2C,, +6.91417175,0x68,, +6.91426725,0x00,, +6.91436275,0x80,, +6.914458125,0x08,, +6.914553625,0x00,, +6.914649125,0xD5,, +6.914744625,0x42,, +6.91484,0xAB,, +6.9149355,0x80,, +6.915031,0x0D,, +6.9151265,0x54,, +6.915221875,0x80,, +6.915317375,0x08,, +6.915412875,0x00,, +6.91550825,0x2A,, +6.91560375,0x62,, +6.91569925,0xA6,, +6.91579475,0xA5,, +6.93341275,0x55,, +6.93350825,0x55,, +6.93360375,0x18,, +6.933699125,0x00,, +6.933794625,0xE3,, +6.933890125,0xA0,, +6.933985625,0xDB,, +6.934081125,0x00,, +6.9341765,0x2C,, +6.934272,0x58,, +6.9343675,0x00,, +6.934462875,0x80,, +6.934558375,0x08,, +6.934653875,0x00,, +6.934749375,0xD5,, +6.93484475,0x42,, +6.93494025,0xAB,, +6.93503575,0x80,, +6.93513125,0x0D,, +6.935226625,0x54,, +6.935322125,0x80,, +6.935417625,0x08,, +6.935513,0x00,, +6.9356085,0x2A,, +6.935704,0x62,, +6.9357995,0xA6,, +6.935895,0xBE,, +6.953513,0x55,, +6.9536085,0x55,, +6.953704,0x18,, +6.953799375,0x00,, +6.953894875,0xE4,, +6.953990375,0x66,, +6.95408575,0xDC,, +6.95418125,0x00,, +6.95427675,0x2C,, +6.95437225,0x58,, +6.954467625,0x00,, +6.954563125,0x80,, +6.954658625,0x08,, +6.954754125,0x00,, +6.9548495,0xD5,, +6.954945,0x42,, +6.9550405,0xAB,, +6.955135875,0x80,, +6.955231375,0x0D,, +6.955326875,0x54,, +6.955422375,0x80,, +6.955517875,0x08,, +6.95561325,0x00,, +6.95570875,0x2A,, +6.95580425,0x62,, +6.95589975,0xA6,, +6.955995125,0xEB,, +6.97361325,0x55,, +6.97370875,0x55,, +6.973804125,0x18,, +6.973899625,0x00,, +6.973995125,0xE5,, +6.9740905,0x2D,, +6.974186,0xDB,, +6.9742815,0x00,, +6.974377,0x2C,, +6.9744725,0x48,, +6.974567875,0x00,, +6.974663375,0x80,, +6.974758875,0x08,, +6.97485425,0x00,, +6.97494975,0xD5,, +6.97504525,0x42,, +6.97514075,0xAB,, +6.975236125,0x80,, +6.975331625,0x0D,, +6.975427125,0x54,, +6.975522625,0x80,, +6.975618,0x08,, +6.9757135,0x00,, +6.975809,0x2A,, +6.975904375,0x62,, +6.975999875,0xA6,, +6.976095375,0x73,, +6.993713375,0x55,, +6.993808875,0x55,, +6.993904375,0x18,, +6.993999875,0x00,, +6.994095375,0xE5,, +6.99419075,0xF4,, +6.99428625,0xDB,, +6.99438175,0x00,, +6.99447725,0x2C,, +6.994572625,0x58,, +6.994668125,0x00,, +6.994763625,0x80,, +6.994859,0x08,, +6.9949545,0x00,, +6.99505,0xD5,, +6.9951455,0x42,, +6.995240875,0xAB,, +6.995336375,0x80,, +6.995431875,0x0D,, +6.995527375,0x54,, +6.99562275,0x80,, +6.99571825,0x08,, +6.99581375,0x00,, +6.995909125,0x2A,, +6.996004625,0x62,, +6.996100125,0xA6,, +6.996195625,0xBF,, +7.013805,0x55,, +7.0139005,0x55,, +7.013995875,0x18,, +7.014091375,0x00,, +7.014186875,0xE6,, +7.01428225,0xBB,, +7.01437775,0xDC,, +7.01447325,0x00,, +7.01456875,0x2C,, +7.01466425,0x68,, +7.014759625,0x00,, +7.014855125,0x80,, +7.014950625,0x08,, +7.015046,0x00,, +7.0151415,0xD5,, +7.015237,0x42,, +7.0153325,0xAB,, +7.015427875,0x80,, +7.015523375,0x0D,, +7.015618875,0x54,, +7.015714375,0x80,, +7.01580975,0x08,, +7.01590525,0x00,, +7.01600075,0x2A,, +7.01609625,0x62,, +7.016191625,0xA6,, +7.016287125,0x5D,, +7.034304375,0x55,, +7.034399875,0x55,, +7.034495375,0x2B,, +7.034590875,0x03,, +7.03468625,0xE7,, +7.03478175,0x81,, +7.03487725,0xDE,, +7.03497275,0x00,, +7.035068125,0x2C,, +7.035163625,0x68,, +7.035259125,0x00,, +7.035354625,0x80,, +7.03545,0x08,, +7.0355455,0x00,, +7.035641,0xD5,, +7.035736375,0x42,, +7.035831875,0xAB,, +7.035927375,0x80,, +7.036022875,0x0D,, +7.036118375,0x54,, +7.03621375,0x80,, +7.03630925,0x08,, +7.03640475,0x00,, +7.036500125,0x2A,, +7.036595625,0x62,, +7.036691125,0xA6,, +7.036786625,0x00,, +7.036882,0x00,, +7.0369775,0x00,, +7.037073,0x00,, +7.0371685,0x00,, +7.037263875,0x00,, +7.037359375,0x00,, +7.037454875,0x00,, +7.03755025,0x00,, +7.03764575,0x00,, +7.03774125,0x00,, +7.03783675,0x00,, +7.03793225,0x00,, +7.038027625,0x00,, +7.038123125,0x00,, +7.038218625,0x00,, +7.038314125,0x00,, +7.0384095,0x00,, +7.038505,0x00,, +7.0386005,0xE3,, +7.054005375,0x55,, +7.054100875,0x55,, +7.054196375,0x18,, +7.054291875,0x00,, +7.05438725,0xE8,, +7.05448275,0x47,, +7.05457825,0xDC,, +7.054673625,0x00,, +7.054769125,0x2C,, +7.054864625,0x48,, +7.054960125,0x00,, +7.055055625,0x80,, +7.055151,0x08,, +7.0552465,0x00,, +7.055342,0xD5,, +7.0554375,0x42,, +7.055532875,0xAB,, +7.055628375,0x80,, +7.055723875,0x0D,, +7.05581925,0x54,, +7.05591475,0x80,, +7.05601025,0x08,, +7.05610575,0x00,, +7.056201125,0x2A,, +7.056296625,0x62,, +7.056392125,0xA6,, +7.056487625,0xD6,, +7.074105625,0x55,, +7.074201125,0x55,, +7.074296625,0x18,, +7.074392,0x00,, +7.0744875,0xE9,, +7.074583,0x0D,, +7.0746785,0xDA,, +7.074773875,0x00,, +7.074869375,0x2C,, +7.074964875,0x78,, +7.075060375,0x00,, +7.07515575,0x80,, +7.07525125,0x08,, +7.07534675,0x00,, +7.075442125,0xD5,, +7.075537625,0x42,, +7.075633125,0xAB,, +7.075728625,0x80,, +7.075824,0x0D,, +7.0759195,0x54,, +7.076015,0x80,, +7.0761105,0x08,, +7.076206,0x00,, +7.076301375,0x2A,, +7.076396875,0x62,, +7.07649225,0xA6,, +7.07658775,0x61,, +7.094197125,0x55,, +7.094292625,0x55,, +7.094388125,0x18,, +7.094483625,0x00,, +7.094579,0xE9,, +7.0946745,0xD3,, +7.09477,0xDC,, +7.0948655,0x00,, +7.094960875,0x2C,, +7.095056375,0x48,, +7.095151875,0x00,, +7.095247375,0x80,, +7.09534275,0x08,, +7.09543825,0x00,, +7.09553375,0xD5,, +7.09562925,0x42,, +7.095724625,0xAB,, +7.095820125,0x80,, +7.095915625,0x0D,, +7.096011,0x54,, +7.0961065,0x80,, +7.096202,0x08,, +7.0962975,0x00,, +7.096392875,0x2A,, +7.096488375,0x62,, +7.096583875,0xA6,, +7.096679375,0xD0,, +7.11428875,0x55,, +7.114384125,0x55,, +7.114479625,0x18,, +7.114575125,0x00,, +7.114670625,0xEA,, +7.114766,0x9A,, +7.1148615,0xDB,, +7.114957,0x00,, +7.1150525,0x2C,, +7.115148,0x78,, +7.115243375,0x00,, +7.115338875,0x80,, +7.115434375,0x08,, +7.11552975,0x00,, +7.11562525,0xD5,, +7.11572075,0x42,, +7.11581625,0xAB,, +7.115911625,0x80,, +7.116007125,0x0D,, +7.116102625,0x54,, +7.116198125,0x80,, +7.1162935,0x08,, +7.116389,0x00,, +7.1164845,0x2A,, +7.116579875,0x62,, +7.116675375,0xA6,, +7.116770875,0xA4,, +7.134388875,0x55,, +7.134484375,0x55,, +7.134579875,0x18,, +7.134675375,0x00,, +7.134770875,0xEB,, +7.13486625,0x61,, +7.13496175,0xDD,, +7.13505725,0x00,, +7.135152625,0x2C,, +7.135248125,0x68,, +7.135343625,0x00,, +7.135439125,0x80,, +7.1355345,0x08,, +7.13563,0x00,, +7.1357255,0xD5,, +7.135821,0x42,, +7.135916375,0xAB,, +7.136011875,0x80,, +7.136107375,0x0D,, +7.13620275,0x54,, +7.13629825,0x80,, +7.13639375,0x08,, +7.13648925,0x00,, +7.13658475,0x2A,, +7.136680125,0x62,, +7.136775625,0xA6,, +7.136871125,0x95,, +7.154497875,0x55,, +7.15459325,0x55,, +7.15468875,0x18,, +7.15478425,0x00,, +7.15487975,0xEC,, +7.15497525,0x27,, +7.155070625,0xDC,, +7.155166125,0x00,, +7.155261625,0x2C,, +7.155357,0x68,, +7.1554525,0x00,, +7.155548,0x80,, +7.1556435,0x08,, +7.155738875,0x00,, +7.155834375,0xD5,, +7.155929875,0x42,, +7.156025375,0xAB,, +7.15612075,0x80,, +7.15621625,0x0D,, +7.15631175,0x54,, +7.156407125,0x80,, +7.156502625,0x08,, +7.156598125,0x00,, +7.156693625,0x2A,, +7.156789125,0x62,, +7.1568845,0xA6,, +7.15698,0x01,, +7.174589375,0x55,, +7.174684875,0x55,, +7.17478025,0x18,, +7.17487575,0x00,, +7.17497125,0xEC,, +7.17506675,0xEE,, +7.175162125,0xDD,, +7.175257625,0x00,, +7.175353125,0x2C,, +7.175448625,0x58,, +7.175544125,0x00,, +7.1756395,0x80,, +7.175735,0x08,, +7.1758305,0x00,, +7.175925875,0xD5,, +7.176021375,0x42,, +7.176116875,0xAB,, +7.176212375,0x80,, +7.17630775,0x0D,, +7.17640325,0x54,, +7.17649875,0x80,, +7.17659425,0x08,, +7.176689625,0x00,, +7.176785125,0x2A,, +7.176880625,0x62,, +7.176976,0xA6,, +7.1770715,0x42,, +7.194689625,0x55,, +7.194785125,0x55,, +7.1948805,0x18,, +7.194976,0x00,, +7.1950715,0xED,, +7.195167,0xB4,, +7.195262375,0xDD,, +7.195357875,0x00,, +7.195453375,0x2C,, +7.19554875,0x78,, +7.19564425,0x00,, +7.19573975,0x80,, +7.19583525,0x08,, +7.195930625,0x00,, +7.196026125,0xD5,, +7.196121625,0x42,, +7.196217125,0xAB,, +7.1963125,0x80,, +7.196408,0x0D,, +7.1965035,0x54,, +7.196598875,0x80,, +7.196694375,0x08,, +7.196789875,0x00,, +7.196885375,0x2A,, +7.196980875,0x62,, +7.19707625,0xA6,, +7.19717175,0x99,, +7.214789875,0x55,, +7.21488525,0x55,, +7.21498075,0x18,, +7.21507625,0x00,, +7.21517175,0xEE,, +7.215267125,0x7B,, +7.215362625,0xD9,, +7.215458125,0x00,, +7.2155535,0x2C,, +7.215649,0x78,, +7.2157445,0x00,, +7.21584,0x80,, +7.2159355,0x08,, +7.216030875,0x00,, +7.216126375,0xD5,, +7.216221875,0x42,, +7.21631725,0xAB,, +7.21641275,0x80,, +7.21650825,0x0D,, +7.21660375,0x54,, +7.216699125,0x80,, +7.216794625,0x08,, +7.216890125,0x00,, +7.216985625,0x2A,, +7.217081,0x62,, +7.2171765,0xA6,, +7.217272,0x01,, +7.23572325,0x55,, +7.235818625,0x55,, +7.235914125,0x2B,, +7.236009625,0x03,, +7.236105125,0xEF,, +7.2362005,0x46,, +7.236296,0xDD,, +7.2363915,0x00,, +7.236487,0x2C,, +7.236582375,0x78,, +7.236677875,0x00,, +7.236773375,0x80,, +7.236868875,0x08,, +7.23696425,0x00,, +7.23705975,0xD5,, +7.23715525,0x42,, +7.237250625,0xAB,, +7.237346125,0x80,, +7.237441625,0x0D,, +7.237537125,0x54,, +7.2376325,0x80,, +7.237728,0x08,, +7.2378235,0x00,, +7.237919,0x2A,, +7.2380145,0x62,, +7.238109875,0xA6,, +7.238205375,0x00,, +7.238300875,0x00,, +7.23839625,0x00,, +7.23849175,0x00,, +7.23858725,0x00,, +7.23868275,0x00,, +7.238778125,0x00,, +7.238873625,0x00,, +7.238969125,0x00,, +7.239064625,0x00,, +7.23916,0x00,, +7.2392555,0x00,, +7.239351,0x00,, +7.239446375,0x00,, +7.239541875,0x00,, +7.239637375,0x00,, +7.239732875,0x00,, +7.239828375,0x00,, +7.23992375,0x00,, +7.24001925,0x82,, +7.25545025,0x55,, +7.25554575,0x55,, +7.255641125,0x18,, +7.255736625,0x00,, +7.255832125,0xF0,, +7.255927625,0x0E,, +7.256023,0xDB,, +7.2561185,0x00,, +7.256214,0x2C,, +7.256309375,0x58,, +7.256404875,0x00,, +7.256500375,0x80,, +7.256595875,0x08,, +7.256691375,0x00,, +7.25678675,0xD5,, +7.25688225,0x42,, +7.25697775,0xAB,, +7.25707325,0x80,, +7.257168625,0x0D,, +7.257264125,0x54,, +7.257359625,0x80,, +7.257455,0x08,, +7.2575505,0x00,, +7.257646,0x2A,, +7.2577415,0x62,, +7.257836875,0xA6,, +7.257932375,0x36,, +7.27524675,0x55,, +7.275342125,0x55,, +7.275437625,0x18,, +7.275533125,0x00,, +7.2756285,0xF0,, +7.275724,0xD0,, +7.2758195,0xC4,, +7.275915,0x00,, +7.2760105,0x2C,, +7.276105875,0x58,, +7.276201375,0x00,, +7.276296875,0x80,, +7.27639225,0x08,, +7.27648775,0x00,, +7.27658325,0xD5,, +7.27667875,0x42,, +7.276774125,0xAB,, +7.276869625,0x80,, +7.276965125,0x0D,, +7.277060625,0x54,, +7.277156,0x80,, +7.2772515,0x08,, +7.277347,0x00,, +7.2774425,0x2A,, +7.277537875,0x62,, +7.277633375,0xA6,, +7.277728875,0x25,, +7.295859,0x55,, +7.2959545,0x55,, +7.296049875,0x18,, +7.296145375,0x00,, +7.296240875,0xF1,, +7.29633625,0x9D,, +7.29643175,0xDD,, +7.29652725,0x00,, +7.29662275,0x2C,, +7.296718125,0x68,, +7.296813625,0x00,, +7.296909125,0x80,, +7.297004625,0x08,, +7.2971,0x00,, +7.2971955,0xD5,, +7.297291,0x42,, +7.297386375,0xAB,, +7.297481875,0x80,, +7.297577375,0x0D,, +7.297672875,0x54,, +7.297768375,0x80,, +7.29786375,0x08,, +7.29795925,0x00,, +7.29805475,0x2A,, +7.29815025,0x62,, +7.298245625,0xA6,, +7.298341125,0xAD,, +7.31528225,0x55,, +7.31537775,0x55,, +7.315473125,0x18,, +7.315568625,0x00,, +7.315664125,0xF2,, +7.315759625,0x5D,, +7.315855,0xDB,, +7.3159505,0x00,, +7.316046,0x2C,, +7.3161415,0x68,, +7.316236875,0x00,, +7.316332375,0x80,, +7.316427875,0x08,, +7.316523375,0x00,, +7.31661875,0xD5,, +7.31671425,0x42,, +7.31680975,0xAB,, +7.316905125,0x80,, +7.317000625,0x0D,, +7.317096125,0x54,, +7.317191625,0x80,, +7.317287,0x08,, +7.3173825,0x00,, +7.317478,0x2A,, +7.3175735,0x62,, +7.317668875,0xA6,, +7.317764375,0x0D,, +7.33537375,0x55,, +7.33546925,0x55,, +7.33556475,0x18,, +7.335660125,0x00,, +7.335755625,0xF3,, +7.335851125,0x24,, +7.335946625,0xDA,, +7.336042125,0x00,, +7.3361375,0x2C,, +7.336233,0x68,, +7.3363285,0x00,, +7.336423875,0x80,, +7.336519375,0x08,, +7.336614875,0x00,, +7.336710375,0xD5,, +7.33680575,0x42,, +7.33690125,0xAB,, +7.33699675,0x80,, +7.33709225,0x0D,, +7.337187625,0x54,, +7.337283125,0x80,, +7.337378625,0x08,, +7.337474,0x00,, +7.3375695,0x2A,, +7.337665,0x62,, +7.3377605,0xA6,, +7.337855875,0xBE,, +7.355482625,0x55,, +7.355578125,0x55,, +7.355673625,0x18,, +7.355769125,0x00,, +7.3558645,0xF3,, +7.35596,0xEA,, +7.3560555,0xDC,, +7.356151,0x00,, +7.356246375,0x2C,, +7.356341875,0x68,, +7.356437375,0x00,, +7.35653275,0x80,, +7.35662825,0x08,, +7.35672375,0x00,, +7.35681925,0xD5,, +7.35691475,0x42,, +7.357010125,0xAB,, +7.357105625,0x80,, +7.357201125,0x0D,, +7.3572965,0x54,, +7.357392,0x80,, +7.3574875,0x08,, +7.357583,0x00,, +7.357678375,0x2A,, +7.357773875,0x62,, +7.357869375,0xA6,, +7.357964875,0x4C,, +7.37557425,0x55,, +7.37566975,0x55,, +7.375765125,0x18,, +7.375860625,0x00,, +7.375956125,0xF4,, +7.3760515,0xB0,, +7.376147,0xDC,, +7.3762425,0x00,, +7.376338,0x2C,, +7.376433375,0x68,, +7.376528875,0x00,, +7.376624375,0x80,, +7.376719875,0x08,, +7.37681525,0x00,, +7.37691075,0xD5,, +7.37700625,0x42,, +7.37710175,0xAB,, +7.377197125,0x80,, +7.377292625,0x0D,, +7.377388125,0x54,, +7.377483625,0x80,, +7.377579,0x08,, +7.3776745,0x00,, +7.37777,0x2A,, +7.377865375,0x62,, +7.377960875,0xA6,, +7.378056375,0xBB,, +7.39646425,0x55,, +7.396559625,0x55,, +7.396655125,0x18,, +7.396750625,0x00,, +7.396846125,0xF5,, +7.3969415,0x7E,, +7.397037,0xDA,, +7.3971325,0x00,, +7.397228,0x2C,, +7.397323375,0x68,, +7.397418875,0x00,, +7.397514375,0x80,, +7.39760975,0x08,, +7.39770525,0x00,, +7.39780075,0xD5,, +7.39789625,0x42,, +7.39799175,0xAB,, +7.398087125,0x80,, +7.398182625,0x0D,, +7.398278125,0x54,, +7.3983735,0x80,, +7.398469,0x08,, +7.3985645,0x00,, +7.39866,0x2A,, +7.398755375,0x62,, +7.398850875,0xA6,, +7.398946375,0x1B,, +7.415766,0x55,, +7.4158615,0x55,, +7.415956875,0x18,, +7.416052375,0x00,, +7.416147875,0xF6,, +7.41624325,0x3B,, +7.41633875,0xDD,, +7.41643425,0x00,, +7.41652975,0x2C,, +7.41662525,0x58,, +7.416720625,0x00,, +7.416816125,0x80,, +7.416911625,0x08,, +7.417007,0x00,, +7.4171025,0xD5,, +7.417198,0x42,, +7.4172935,0xAB,, +7.417388875,0x80,, +7.417484375,0x0D,, +7.417579875,0x54,, +7.417675375,0x80,, +7.41777075,0x08,, +7.41786625,0x00,, +7.41796175,0x2A,, +7.41805725,0x62,, +7.418152625,0xA6,, +7.418248125,0x44,, +7.436274125,0x55,, +7.436369625,0x55,, +7.436465,0x2B,, +7.4365605,0x03,, +7.436656,0xF7,, +7.436751375,0x02,, +7.436846875,0xDC,, +7.436942375,0x00,, +7.437037875,0x2C,, +7.43713325,0x68,, +7.43722875,0x00,, +7.43732425,0x80,, +7.43741975,0x08,, +7.43751525,0x00,, +7.437610625,0xD5,, +7.437706125,0x42,, +7.437801625,0xAB,, +7.437897,0x80,, +7.4379925,0x0D,, +7.438088,0x54,, +7.4381835,0x80,, +7.438278875,0x08,, +7.438374375,0x00,, +7.438469875,0x2A,, +7.438565375,0x62,, +7.43866075,0xA6,, +7.43875625,0x00,, +7.43885175,0x00,, +7.438947125,0x00,, +7.439042625,0x00,, +7.439138125,0x00,, +7.439233625,0x00,, +7.439329125,0x00,, +7.4394245,0x00,, +7.43952,0x00,, +7.4396155,0x00,, +7.439710875,0x00,, +7.439806375,0x00,, +7.439901875,0x00,, +7.439997375,0x00,, +7.44009275,0x00,, +7.44018825,0x00,, +7.44028375,0x00,, +7.44037925,0x00,, +7.440474625,0x00,, +7.440570125,0x35,, +7.455966375,0x55,, +7.456061875,0x55,, +7.456157375,0x18,, +7.456252875,0x00,, +7.45634825,0xF7,, +7.45644375,0xC8,, +7.45653925,0xD9,, +7.456634625,0x00,, +7.456730125,0x2C,, +7.456825625,0x68,, +7.456921125,0x00,, +7.4570165,0x80,, +7.457112,0x08,, +7.4572075,0x00,, +7.457303,0xD5,, +7.4573985,0x42,, +7.457493875,0xAB,, +7.457589375,0x80,, +7.457684875,0x0D,, +7.45778025,0x54,, +7.45787575,0x80,, +7.45797125,0x08,, +7.45806675,0x00,, +7.458162125,0x2A,, +7.458257625,0x62,, +7.458353125,0xA6,, +7.458448625,0xD8,, +7.476066625,0x55,, +7.476162125,0x55,, +7.476257625,0x18,, +7.476353,0x00,, +7.4764485,0xF8,, +7.476544,0x8F,, +7.476639375,0xDB,, +7.476734875,0x00,, +7.476830375,0x2C,, +7.476925875,0x68,, +7.477021375,0x00,, +7.47711675,0x80,, +7.47721225,0x08,, +7.47730775,0x00,, +7.477403125,0xD5,, +7.477498625,0x42,, +7.477594125,0xAB,, +7.477689625,0x80,, +7.477785,0x0D,, +7.4778805,0x54,, +7.477976,0x80,, +7.4780715,0x08,, +7.478166875,0x00,, +7.478262375,0x2A,, +7.478357875,0x62,, +7.47845325,0xA6,, +7.47854875,0x62,, +7.496166875,0x55,, +7.49626225,0x55,, +7.49635775,0x18,, +7.49645325,0x00,, +7.49654875,0xF9,, +7.49664425,0x55,, +7.496739625,0xDB,, +7.496835125,0x00,, +7.496930625,0x2C,, +7.497026125,0x68,, +7.4971215,0x00,, +7.497217,0x80,, +7.4973125,0x08,, +7.497407875,0x00,, +7.497503375,0xD5,, +7.497598875,0x42,, +7.497694375,0xAB,, +7.49778975,0x80,, +7.49788525,0x0D,, +7.49798075,0x54,, +7.49807625,0x80,, +7.498171625,0x08,, +7.498267125,0x00,, +7.498362625,0x2A,, +7.498458125,0x62,, +7.4985535,0xA6,, +7.498649,0x57,, +7.516258375,0x55,, +7.516353875,0x55,, +7.516449375,0x18,, +7.51654475,0x00,, +7.51664025,0xFA,, +7.51673575,0x1B,, +7.51683125,0xDC,, +7.516926625,0x00,, +7.517022125,0x2C,, +7.517117625,0x68,, +7.517213125,0x00,, +7.5173085,0x80,, +7.517404,0x08,, +7.5174995,0x00,, +7.517595,0xD5,, +7.517690375,0x42,, +7.517785875,0xAB,, +7.517881375,0x80,, +7.51797675,0x0D,, +7.51807225,0x54,, +7.51816775,0x80,, +7.51826325,0x08,, +7.51835875,0x00,, +7.518454125,0x2A,, +7.518549625,0x62,, +7.518645125,0xA6,, +7.5187405,0x56,, +7.536349875,0x55,, +7.536445375,0x55,, +7.536540875,0x18,, +7.536636375,0x00,, +7.536731875,0xFA,, +7.53682725,0xE2,, +7.53692275,0xDB,, +7.53701825,0x00,, +7.537113625,0x2C,, +7.537209125,0x78,, +7.537304625,0x00,, +7.537400125,0x80,, +7.5374955,0x08,, +7.537591,0x00,, +7.5376865,0xD5,, +7.537782,0x42,, +7.537877375,0xAB,, +7.537972875,0x80,, +7.538068375,0x0D,, +7.53816375,0x54,, +7.53825925,0x80,, +7.53835475,0x08,, +7.53845025,0x00,, +7.538545625,0x2A,, +7.538641125,0x62,, +7.538736625,0xA6,, +7.538832125,0x96,, +7.556450125,0x55,, +7.556545625,0x55,, +7.556641125,0x18,, +7.556736625,0x00,, +7.556832,0xFB,, +7.5569275,0xA8,, +7.557023,0xD9,, +7.557118375,0x00,, +7.557213875,0x2C,, +7.557309375,0x68,, +7.557404875,0x00,, +7.55750025,0x80,, +7.55759575,0x08,, +7.55769125,0x00,, +7.55778675,0xD5,, +7.557882125,0x42,, +7.557977625,0xAB,, +7.558073125,0x80,, +7.558168625,0x0D,, +7.558264,0x54,, +7.5583595,0x80,, +7.558455,0x08,, +7.5585505,0x00,, +7.558645875,0x2A,, +7.558741375,0x62,, +7.558836875,0xA6,, +7.55893225,0x56,, +7.576550375,0x55,, +7.576645875,0x55,, +7.57674125,0x18,, +7.57683675,0x00,, +7.57693225,0xFC,, +7.57702775,0x6E,, +7.577123125,0xD9,, +7.577218625,0x00,, +7.577314125,0x2C,, +7.577409625,0x68,, +7.577505125,0x00,, +7.5776005,0x80,, +7.577696,0x08,, +7.5777915,0x00,, +7.577886875,0xD5,, +7.577982375,0x42,, +7.578077875,0xAB,, +7.578173375,0x80,, +7.57826875,0x0D,, +7.57836425,0x54,, +7.57845975,0x80,, +7.57855525,0x08,, +7.578650625,0x00,, +7.578746125,0x2A,, +7.578841625,0x62,, +7.578937,0xA6,, +7.5790325,0xC7,, +7.596702625,0x55,, +7.596798125,0x55,, +7.596893625,0x18,, +7.596989125,0x00,, +7.5970845,0xFD,, +7.59718,0x36,, +7.5972755,0xDD,, +7.597370875,0x00,, +7.597466375,0x2C,, +7.597561875,0x68,, +7.597657375,0x00,, +7.597752875,0x80,, +7.59784825,0x08,, +7.59794375,0x00,, +7.59803925,0xD5,, +7.598134625,0x42,, +7.598230125,0xAB,, +7.598325625,0x80,, +7.598421125,0x0D,, +7.5985165,0x54,, +7.598612,0x80,, +7.5987075,0x08,, +7.598803,0x00,, +7.598898375,0x2A,, +7.598993875,0x62,, +7.599089375,0xA6,, +7.59918475,0xE4,, +7.616750875,0x55,, +7.61684625,0x55,, +7.61694175,0x18,, +7.61703725,0x00,, +7.61713275,0xFD,, +7.617228125,0xFB,, +7.617323625,0xDC,, +7.617419125,0x00,, +7.6175145,0x2C,, +7.61761,0x68,, +7.6177055,0x00,, +7.617801,0x80,, +7.617896375,0x08,, +7.617991875,0x00,, +7.618087375,0xD5,, +7.618182875,0x42,, +7.61827825,0xAB,, +7.61837375,0x80,, +7.61846925,0x0D,, +7.61856475,0x54,, +7.618660125,0x80,, +7.618755625,0x08,, +7.618851125,0x00,, +7.618946625,0x2A,, +7.619042,0x62,, +7.6191375,0xA6,, +7.619233,0x99,, +7.63725025,0x55,, +7.63734575,0x55,, +7.63744125,0x2B,, +7.637536625,0x03,, +7.637632125,0xFE,, +7.637727625,0xC0,, +7.637823,0xD9,, +7.6379185,0x00,, +7.638014,0x2C,, +7.6381095,0x58,, +7.638205,0x00,, +7.638300375,0x80,, +7.638395875,0x08,, +7.638491375,0x00,, +7.63858675,0xD5,, +7.63868225,0x42,, +7.63877775,0xAB,, +7.63887325,0x80,, +7.638968625,0x0D,, +7.639064125,0x54,, +7.639159625,0x80,, +7.639255125,0x08,, +7.6393505,0x00,, +7.639446,0x2A,, +7.6395415,0x62,, +7.639636875,0xA6,, +7.639732375,0x00,, +7.639827875,0x00,, +7.639923375,0x00,, +7.640018875,0x00,, +7.64011425,0x00,, +7.64020975,0x00,, +7.64030525,0x00,, +7.64040075,0x00,, +7.640496125,0x00,, +7.640591625,0x00,, +7.640687125,0x00,, +7.6407825,0x00,, +7.640878,0x00,, +7.6409735,0x00,, +7.641069,0x00,, +7.641164375,0x00,, +7.641259875,0x00,, +7.641355375,0x00,, +7.641450875,0x00,, +7.64154625,0xC4,, +7.656942625,0x55,, +7.657038,0x55,, +7.6571335,0x18,, +7.657229,0x00,, +7.6573245,0xFF,, +7.657419875,0x87,, +7.657515375,0xDB,, +7.657610875,0x00,, +7.65770625,0x2C,, +7.65780175,0x58,, +7.65789725,0x00,, +7.65799275,0x80,, +7.65808825,0x08,, +7.658183625,0x00,, +7.658279125,0xD5,, +7.658374625,0x42,, +7.65847,0xAB,, +7.6585655,0x80,, +7.658661,0x0D,, +7.6587565,0x54,, +7.658851875,0x80,, +7.658947375,0x08,, +7.659042875,0x00,, +7.659138375,0x2A,, +7.65923375,0x62,, +7.65932925,0xA6,, +7.65942475,0xCE,, +7.67704275,0x55,, +7.67713825,0x55,, +7.67723375,0x18,, +7.677329125,0x00,, +7.677424625,0x00,, +7.677520125,0x4E,, +7.677615625,0xDB,, +7.677711125,0x00,, +7.6778065,0x2C,, +7.677902,0x48,, +7.6779975,0x00,, +7.678093,0x80,, +7.678188375,0x08,, +7.678283875,0x00,, +7.678379375,0xD5,, +7.67847475,0x42,, +7.67857025,0xAB,, +7.67866575,0x80,, +7.67876125,0x0D,, +7.678856625,0x54,, +7.678952125,0x80,, +7.679047625,0x08,, +7.679143125,0x00,, +7.6792385,0x2A,, +7.679334,0x62,, +7.6794295,0xA6,, +7.679525,0x3F,, +7.69717775,0x55,, +7.697273125,0x55,, +7.697368625,0x18,, +7.697464125,0x00,, +7.697559625,0x01,, +7.697655125,0x15,, +7.6977505,0xDB,, +7.697846,0x01,, +7.6979415,0x2C,, +7.698037,0x48,, +7.698132375,0x00,, +7.698227875,0x80,, +7.698323375,0x08,, +7.69841875,0x00,, +7.69851425,0xD5,, +7.69860975,0x42,, +7.69870525,0xAB,, +7.698800625,0x80,, +7.698896125,0x0D,, +7.698991625,0x54,, +7.699087125,0x80,, +7.6991825,0x08,, +7.699278,0x00,, +7.6993735,0x2A,, +7.699469,0x62,, +7.699564375,0xA6,, +7.699659875,0x31,, +7.718197875,0x55,, +7.718293375,0x55,, +7.718388875,0x18,, +7.718484375,0x00,, +7.71857975,0x01,, +7.71867525,0xE4,, +7.71877075,0xD9,, +7.718866125,0x00,, +7.718961625,0x2C,, +7.719057125,0x68,, +7.719152625,0x00,, +7.719248,0x80,, +7.7193435,0x08,, +7.719439,0x00,, +7.7195345,0xD5,, +7.719629875,0x42,, +7.719725375,0xAB,, +7.719820875,0x80,, +7.71991625,0x0D,, +7.72001175,0x54,, +7.72010725,0x80,, +7.72020275,0x08,, +7.72029825,0x00,, +7.720393625,0x2A,, +7.720489125,0x62,, +7.720584625,0xA6,, +7.720680125,0xAE,, +7.738055125,0x55,, +7.738150625,0x55,, +7.738246,0x18,, +7.7383415,0x00,, +7.738437,0x02,, +7.7385325,0xA7,, +7.738627875,0xDB,, +7.738723375,0x00,, +7.738818875,0x2C,, +7.738914375,0x88,, +7.73900975,0x00,, +7.73910525,0x80,, +7.73920075,0x08,, +7.73929625,0x00,, +7.739391625,0xD5,, +7.739487125,0x42,, +7.739582625,0xAB,, +7.739678125,0x80,, +7.7397735,0x0D,, +7.739869,0x54,, +7.7399645,0x80,, +7.740059875,0x08,, +7.740155375,0x00,, +7.740250875,0x2A,, +7.740346375,0x62,, +7.74044175,0xA6,, +7.74053725,0x87,, +7.75825075,0x55,, +7.75834625,0x55,, +7.75844175,0x18,, +7.75853725,0x00,, +7.758632625,0x03,, +7.758728125,0x6E,, +7.758823625,0xDB,, +7.758919125,0x00,, +7.7590145,0x2C,, +7.75911,0x68,, +7.7592055,0x00,, +7.759301,0x80,, +7.759396375,0x08,, +7.759491875,0x00,, +7.759587375,0xD5,, +7.759682875,0x42,, +7.75977825,0xAB,, +7.75987375,0x80,, +7.75996925,0x0D,, +7.760064625,0x54,, +7.760160125,0x80,, +7.760255625,0x08,, +7.760351125,0x00,, +7.760446625,0x2A,, +7.760542,0x62,, +7.7606375,0xA6,, +7.760733,0xC6,, +7.777604625,0x55,, +7.777700125,0x55,, +7.777795625,0x18,, +7.777891,0x00,, +7.7779865,0x04,, +7.778082,0x2D,, +7.7781775,0xDA,, +7.778272875,0x00,, +7.778368375,0x2C,, +7.778463875,0x68,, +7.778559375,0x00,, +7.77865475,0x80,, +7.77875025,0x08,, +7.77884575,0x00,, +7.77894125,0xD5,, +7.779036625,0x42,, +7.779132125,0xAB,, +7.779227625,0x80,, +7.779323,0x0D,, +7.7794185,0x54,, +7.779514,0x80,, +7.7796095,0x08,, +7.779705,0x00,, +7.779800375,0x2A,, +7.779895875,0x62,, +7.779991375,0xA6,, +7.78008675,0x8F,, +7.798407875,0x55,, +7.798503375,0x55,, +7.79859875,0x18,, +7.79869425,0x00,, +7.79878975,0x04,, +7.798885125,0xFA,, +7.798980625,0xDD,, +7.799076125,0x00,, +7.799171625,0x2C,, +7.799267,0x68,, +7.7993625,0x00,, +7.799458,0x80,, +7.7995535,0x08,, +7.799649,0x00,, +7.799744375,0xD5,, +7.799839875,0x42,, +7.799935375,0xAB,, +7.80003075,0x80,, +7.80012625,0x0D,, +7.80022175,0x54,, +7.80031725,0x80,, +7.800412625,0x08,, +7.800508125,0x00,, +7.800603625,0x2A,, +7.800699125,0x62,, +7.8007945,0xA6,, +7.80089,0xC3,, +7.81803075,0x55,, +7.81812625,0x55,, +7.818221625,0x18,, +7.818317125,0x00,, +7.818412625,0x05,, +7.818508125,0xBB,, +7.8186035,0xDD,, +7.818699,0x00,, +7.8187945,0x2C,, +7.818889875,0x68,, +7.818985375,0x00,, +7.819080875,0x80,, +7.819176375,0x08,, +7.819271875,0x00,, +7.81936725,0xD5,, +7.81946275,0x42,, +7.81955825,0xAB,, +7.819653625,0x80,, +7.819749125,0x0D,, +7.819844625,0x54,, +7.819940125,0x80,, +7.8200355,0x08,, +7.820131,0x00,, +7.8202265,0x2A,, +7.820322,0x62,, +7.820417375,0xA6,, +7.820512875,0xC2,, +7.83897275,0x55,, +7.83906825,0x55,, +7.83916375,0x2B,, +7.83925925,0x03,, +7.839354625,0x06,, +7.839450125,0x86,, +7.839545625,0xDA,, +7.839641125,0x00,, +7.8397365,0x2C,, +7.839832,0x48,, +7.8399275,0x00,, +7.840022875,0x80,, +7.840118375,0x08,, +7.840213875,0x00,, +7.840309375,0xD5,, +7.840404875,0x42,, +7.84050025,0xAB,, +7.84059575,0x80,, +7.84069125,0x0D,, +7.840786625,0x54,, +7.840882125,0x80,, +7.840977625,0x08,, +7.841073125,0x00,, +7.8411685,0x2A,, +7.841264,0x62,, +7.8413595,0xA6,, +7.841455,0x00,, +7.841550375,0x00,, +7.841645875,0x00,, +7.841741375,0x00,, +7.841836875,0x00,, +7.84193225,0x00,, +7.84202775,0x00,, +7.84212325,0x00,, +7.84221875,0x00,, +7.842314125,0x00,, +7.842409625,0x00,, +7.842505125,0x00,, +7.842600625,0x00,, +7.842696,0x00,, +7.8427915,0x00,, +7.842887,0x00,, +7.842982375,0x00,, +7.843077875,0x00,, +7.843173375,0x00,, +7.843268875,0x8D,, +7.858942875,0x55,, +7.85903825,0x55,, +7.85913375,0x18,, +7.85922925,0x00,, +7.85932475,0x07,, +7.859420125,0x4E,, +7.859515625,0xDD,, +7.859611125,0x00,, +7.859706625,0x2C,, +7.859802,0x68,, +7.8598975,0x00,, +7.859993,0x80,, +7.8600885,0x08,, +7.860183875,0x00,, +7.860279375,0xD5,, +7.860374875,0x42,, +7.860470375,0xAB,, +7.86056575,0x80,, +7.86066125,0x0D,, +7.86075675,0x54,, +7.860852125,0x80,, +7.860947625,0x08,, +7.861043125,0x00,, +7.861138625,0x2A,, +7.861234125,0x62,, +7.8613295,0xA6,, +7.861425,0xA9,, +7.878748,0x55,, +7.8788435,0x55,, +7.878938875,0x18,, +7.879034375,0x00,, +7.879129875,0x08,, +7.87922525,0x11,, +7.87932075,0xDB,, +7.87941625,0x00,, +7.87951175,0x2C,, +7.87960725,0x58,, +7.879702625,0x00,, +7.879798125,0x80,, +7.879893625,0x08,, +7.879989,0x00,, +7.8800845,0xD5,, +7.88018,0x42,, +7.8802755,0xAB,, +7.880370875,0x80,, +7.880466375,0x0D,, +7.880561875,0x54,, +7.880657375,0x80,, +7.88075275,0x08,, +7.88084825,0x00,, +7.88094375,0x2A,, +7.88103925,0x62,, +7.881134625,0xA6,, +7.881230125,0xD2,, +7.898249375,0x55,, +7.898344875,0x55,, +7.89844025,0x18,, +7.89853575,0x00,, +7.89863125,0x08,, +7.89872675,0xD0,, +7.898822125,0xC9,, +7.898917625,0x00,, +7.899013125,0x2C,, +7.8991085,0x58,, +7.899204,0x00,, +7.8992995,0x80,, +7.899395,0x08,, +7.899490375,0x00,, +7.899585875,0xD5,, +7.899681375,0x42,, +7.899776875,0xAB,, +7.89987225,0x80,, +7.89996775,0x0D,, +7.90006325,0x54,, +7.90015875,0x80,, +7.900254125,0x08,, +7.900349625,0x00,, +7.900445125,0x2A,, +7.900540625,0x62,, +7.900636,0xA6,, +7.9007315,0x3E,, +7.918349625,0x55,, +7.918445,0x55,, +7.9185405,0x18,, +7.918636,0x00,, +7.9187315,0x09,, +7.918826875,0x97,, +7.918922375,0xCA,, +7.919017875,0x00,, +7.91911325,0x2C,, +7.91920875,0x58,, +7.91930425,0x00,, +7.91939975,0x80,, +7.91949525,0x08,, +7.919590625,0x00,, +7.919686125,0xD5,, +7.919781625,0x42,, +7.919877,0xAB,, +7.9199725,0x80,, +7.920068,0x0D,, +7.9201635,0x54,, +7.920258875,0x80,, +7.920354375,0x08,, +7.920449875,0x00,, +7.920545375,0x2A,, +7.92064075,0x62,, +7.92073625,0xA6,, +7.92083175,0xDD,, +7.938310875,0x55,, +7.938406375,0x55,, +7.938501875,0x18,, +7.938597375,0x00,, +7.93869275,0x0A,, +7.93878825,0x5D,, +7.93888375,0xC6,, +7.93897925,0x00,, +7.939074625,0x2C,, +7.939170125,0x48,, +7.939265625,0x00,, +7.939361125,0x80,, +7.9394565,0x08,, +7.939552,0x00,, +7.9396475,0xD5,, +7.939743,0x42,, +7.939838375,0xAB,, +7.939933875,0x80,, +7.940029375,0x0D,, +7.94012475,0x54,, +7.94022025,0x80,, +7.94031575,0x08,, +7.94041125,0x00,, +7.940506625,0x2A,, +7.940602125,0x62,, +7.940697625,0xA6,, +7.940793125,0x1F,, +7.958411125,0x55,, +7.958506625,0x55,, +7.958602125,0x18,, +7.958697625,0x00,, +7.958793,0x0B,, +7.9588885,0x23,, +7.958984,0xBC,, +7.959079375,0x00,, +7.959174875,0x2C,, +7.959270375,0x68,, +7.959365875,0x00,, +7.95946125,0x80,, +7.95955675,0x08,, +7.95965225,0x00,, +7.95974775,0xD5,, +7.959843125,0x42,, +7.959938625,0xAB,, +7.960034125,0x80,, +7.9601295,0x0D,, +7.960225,0x54,, +7.9603205,0x80,, +7.960416,0x08,, +7.9605115,0x00,, +7.960606875,0x2A,, +7.960702375,0x62,, +7.960797875,0xA6,, +7.96089325,0x4E,, +7.978511375,0x55,, +7.978606875,0x55,, +7.97870225,0x18,, +7.97879775,0x00,, +7.97889325,0x0B,, +7.97898875,0xE9,, +7.979084125,0xBA,, +7.979179625,0x00,, +7.979275125,0x2C,, +7.979370625,0x48,, +7.979466,0x00,, +7.9795615,0x80,, +7.979657,0x08,, +7.979752375,0x00,, +7.979847875,0xD5,, +7.979943375,0x42,, +7.980038875,0xAB,, +7.980134375,0x80,, +7.98022975,0x0D,, +7.98032525,0x54,, +7.98042075,0x80,, +7.98051625,0x08,, +7.980611625,0x00,, +7.980707125,0x2A,, +7.980802625,0x62,, +7.980898,0xA6,, +7.9809935,0x62,, +7.998611625,0x55,, +7.998707,0x55,, +7.9988025,0x18,, +7.998898,0x00,, +7.9989935,0x0C,, +7.999089,0xB0,, +7.999184375,0xB9,, +7.999279875,0x00,, +7.999375375,0x2C,, +7.99947075,0x78,, +7.99956625,0x00,, +7.99966175,0x80,, +7.99975725,0x08,, +7.999852625,0x00,, +7.999948125,0xD5,, +8.000043625,0x42,, +8.000139125,0xAB,, +8.0002345,0x80,, +8.00033,0x0D,, +8.0004255,0x54,, +8.000520875,0x80,, +8.000616375,0x08,, +8.000711875,0x00,, +8.000807375,0x2A,, +8.00090275,0x62,, +8.00099825,0xA6,, +8.00109375,0x8D,, +8.018711875,0x55,, +8.01880725,0x55,, +8.01890275,0x18,, +8.01899825,0x00,, +8.01909375,0x0D,, +8.019189125,0x77,, +8.019284625,0xC3,, +8.019380125,0x00,, +8.0194755,0x2C,, +8.019571,0x68,, +8.0196665,0x00,, +8.019762,0x80,, +8.019857375,0x08,, +8.019952875,0x00,, +8.020048375,0xD5,, +8.020143875,0x42,, +8.02023925,0xAB,, +8.02033475,0x80,, +8.02043025,0x0D,, +8.020525625,0x54,, +8.020621125,0x80,, +8.020716625,0x08,, +8.020812125,0x00,, +8.020907625,0x2A,, +8.021003,0x62,, +8.0210985,0xA6,, +8.021194,0x88,, +8.03921125,0x55,, +8.03930675,0x55,, +8.03940225,0x2B,, +8.039497625,0x03,, +8.039593125,0x0E,, +8.039688625,0x3E,, +8.039784,0xC5,, +8.0398795,0x00,, +8.039975,0x2C,, +8.0400705,0x48,, +8.040166,0x00,, +8.040261375,0x80,, +8.040356875,0x08,, +8.040452375,0x00,, +8.04054775,0xD5,, +8.04064325,0x42,, +8.04073875,0xAB,, +8.04083425,0x80,, +8.040929625,0x0D,, +8.041025125,0x54,, +8.041120625,0x80,, +8.041216125,0x08,, +8.0413115,0x00,, +8.041407,0x2A,, +8.0415025,0x62,, +8.041597875,0xA6,, +8.041693375,0x00,, +8.041788875,0x00,, +8.041884375,0x00,, +8.04197975,0x00,, +8.04207525,0x00,, +8.04217075,0x00,, +8.04226625,0x00,, +8.04236175,0x00,, +8.042457125,0x00,, +8.042552625,0x00,, +8.042648125,0x00,, +8.0427435,0x00,, +8.042839,0x00,, +8.0429345,0x00,, +8.04303,0x00,, +8.043125375,0x00,, +8.043220875,0x00,, +8.043316375,0x00,, +8.043411875,0x00,, +8.04350725,0xD7,, +8.0590945,0x55,, +8.05919,0x55,, +8.0592855,0x18,, +8.059380875,0x00,, +8.059476375,0x0F,, +8.059571875,0x06,, +8.05966725,0xBA,, +8.05976275,0x00,, +8.05985825,0x2C,, +8.05995375,0x68,, +8.060049125,0x00,, +8.060144625,0x80,, +8.060240125,0x08,, +8.060335625,0x00,, +8.060431,0xD5,, +8.0605265,0x42,, +8.060622,0xAB,, +8.0607175,0x80,, +8.060812875,0x0D,, +8.060908375,0x54,, +8.061003875,0x80,, +8.061099375,0x08,, +8.06119475,0x00,, +8.06129025,0x2A,, +8.06138575,0x62,, +8.061481125,0xA6,, +8.061576625,0xFC,, +8.079264125,0x55,, +8.079359625,0x55,, +8.079455125,0x18,, +8.0795505,0x00,, +8.079646,0x0F,, +8.0797415,0xCD,, +8.079837,0xC1,, +8.079932375,0x00,, +8.080027875,0x2C,, +8.080123375,0x68,, +8.080218875,0x00,, +8.08031425,0x80,, +8.08040975,0x08,, +8.08050525,0x00,, +8.08060075,0xD5,, +8.080696125,0x42,, +8.080791625,0xAB,, +8.080887125,0x80,, +8.080982625,0x0D,, +8.081078,0x54,, +8.0811735,0x80,, +8.081269,0x08,, +8.0813645,0x00,, +8.081459875,0x2A,, +8.081555375,0x62,, +8.081650875,0xA6,, +8.08174625,0x4C,, +8.099104,0x55,, +8.0991995,0x55,, +8.099295,0x18,, +8.099390375,0x00,, +8.099485875,0x10,, +8.099581375,0x90,, +8.099676875,0xB5,, +8.09977225,0x00,, +8.09986775,0x2C,, +8.09996325,0x58,, +8.100058625,0x00,, +8.100154125,0x80,, +8.100249625,0x08,, +8.100345125,0x00,, +8.1004405,0xD5,, +8.100536,0x42,, +8.1006315,0xAB,, +8.100727,0x80,, +8.1008225,0x0D,, +8.100917875,0x54,, +8.101013375,0x80,, +8.10110875,0x08,, +8.10120425,0x00,, +8.10129975,0x2A,, +8.10139525,0x62,, +8.10149075,0xA6,, +8.101586125,0xBF,, +8.119620875,0x55,, +8.11971625,0x55,, +8.11981175,0x18,, +8.11990725,0x00,, +8.120002625,0x11,, +8.120098125,0x5A,, +8.120193625,0xC1,, +8.120289125,0x00,, +8.1203845,0x2C,, +8.12048,0x68,, +8.1205755,0x00,, +8.120671,0x80,, +8.120766375,0x08,, +8.120861875,0x00,, +8.120957375,0xD5,, +8.121052875,0x42,, +8.12114825,0xAB,, +8.12124375,0x80,, +8.12133925,0x0D,, +8.12143475,0x54,, +8.121530125,0x80,, +8.121625625,0x08,, +8.121721125,0x00,, +8.121816625,0x2A,, +8.121912,0x62,, +8.1220075,0xA6,, +8.122103,0x1D,, +8.139530125,0x55,, +8.139625625,0x55,, +8.139721,0x18,, +8.1398165,0x00,, +8.139912,0x12,, +8.140007375,0x1E,, +8.140102875,0xC1,, +8.140198375,0x00,, +8.140293875,0x2C,, +8.140389375,0x48,, +8.14048475,0x00,, +8.14058025,0x80,, +8.14067575,0x08,, +8.140771125,0x00,, +8.140866625,0xD5,, +8.140962125,0x42,, +8.141057625,0xAB,, +8.141153,0x80,, +8.1412485,0x0D,, +8.141344,0x54,, +8.1414395,0x80,, +8.141534875,0x08,, +8.141630375,0x00,, +8.141725875,0x2A,, +8.14182125,0x62,, +8.14191675,0xA6,, +8.14201225,0xA2,, +8.160472125,0x55,, +8.160567625,0x55,, +8.160663125,0x18,, +8.160758625,0x00,, +8.160854,0x12,, +8.1609495,0xEB,, +8.161045,0xC0,, +8.161140375,0x00,, +8.161235875,0x2C,, +8.161331375,0x68,, +8.161426875,0x00,, +8.161522375,0x80,, +8.16161775,0x08,, +8.16171325,0x00,, +8.16180875,0xD5,, +8.161904125,0x42,, +8.161999625,0xAB,, +8.162095125,0x80,, +8.162190625,0x0D,, +8.162286,0x54,, +8.1623815,0x80,, +8.162477,0x08,, +8.1625725,0x00,, +8.162667875,0x2A,, +8.162763375,0x62,, +8.162858875,0xA6,, +8.16295425,0x86,, +8.1804335,0x55,, +8.180529,0x55,, +8.1806245,0x18,, +8.180719875,0x00,, +8.180815375,0x13,, +8.180910875,0xAF,, +8.181006375,0xCC,, +8.181101875,0x00,, +8.18119725,0x2C,, +8.18129275,0x68,, +8.181388125,0x00,, +8.181483625,0x80,, +8.181579125,0x08,, +8.181674625,0x00,, +8.181770125,0xD5,, +8.1818655,0x42,, +8.181961,0xAB,, +8.1820565,0x80,, +8.182152,0x0D,, +8.182247375,0x54,, +8.182342875,0x80,, +8.182438375,0x08,, +8.18253375,0x00,, +8.18262925,0x2A,, +8.18272475,0x62,, +8.18282025,0xA6,, +8.182915625,0x8D,, +8.199717875,0x55,, +8.199813375,0x55,, +8.199908875,0x18,, +8.200004375,0x00,, +8.200099875,0x14,, +8.20019525,0x6D,, +8.20029075,0xDD,, +8.20038625,0x00,, +8.200481625,0x2C,, +8.200577125,0x48,, +8.200672625,0x00,, +8.200768125,0x80,, +8.2008635,0x08,, +8.200959,0x00,, +8.2010545,0xD5,, +8.20115,0x42,, +8.201245375,0xAB,, +8.201340875,0x80,, +8.201436375,0x0D,, +8.20153175,0x54,, +8.20162725,0x80,, +8.20172275,0x08,, +8.20181825,0x00,, +8.201913625,0x2A,, +8.202009125,0x62,, +8.202104625,0xA6,, +8.202200125,0x20,, +8.2205125,0x55,, +8.220607875,0x55,, +8.220703375,0x18,, +8.220798875,0x00,, +8.220894375,0x15,, +8.22098975,0x3B,, +8.22108525,0xCF,, +8.22118075,0x00,, +8.22127625,0x2C,, +8.221371625,0x48,, +8.221467125,0x00,, +8.221562625,0x80,, +8.221658125,0x08,, +8.2217535,0x00,, +8.221849,0xD5,, +8.2219445,0x42,, +8.222039875,0xAB,, +8.222135375,0x80,, +8.222230875,0x0D,, +8.222326375,0x54,, +8.22242175,0x80,, +8.22251725,0x08,, +8.22261275,0x00,, +8.22270825,0x2A,, +8.22280375,0x62,, +8.222899125,0xA6,, +8.222994625,0x81,, +8.240187375,0x55,, +8.240282875,0x55,, +8.240378375,0x2B,, +8.24047375,0x03,, +8.24056925,0x15,, +8.24066475,0xFA,, +8.24076025,0xB2,, +8.24085575,0x00,, +8.240951125,0x2C,, +8.241046625,0x68,, +8.241142125,0x00,, +8.241237625,0x80,, +8.241333,0x08,, +8.2414285,0x00,, +8.241524,0xD5,, +8.241619375,0x42,, +8.241714875,0xAB,, +8.241810375,0x80,, +8.241905875,0x0D,, +8.24200125,0x54,, +8.24209675,0x80,, +8.24219225,0x08,, +8.24228775,0x00,, +8.242383125,0x2A,, +8.242478625,0x62,, +8.242574125,0xA6,, +8.2426695,0x00,, +8.242765,0x00,, +8.2428605,0x00,, +8.242956,0x00,, +8.2430515,0x00,, +8.243146875,0x00,, +8.243242375,0x00,, +8.243337875,0x00,, +8.24343325,0x00,, +8.24352875,0x00,, +8.24362425,0x00,, +8.24371975,0x00,, +8.243815125,0x00,, +8.243910625,0x00,, +8.244006125,0x00,, +8.244101625,0x00,, +8.244197,0x00,, +8.2442925,0x00,, +8.244388,0x00,, +8.244483375,0xFF,, +8.260869125,0x55,, +8.260964625,0x55,, +8.26106,0x18,, +8.2611555,0x00,, +8.261251,0x16,, +8.2613465,0xC8,, +8.261441875,0xC9,, +8.261537375,0x00,, +8.261632875,0x2C,, +8.261728375,0x88,, +8.26182375,0x00,, +8.26191925,0x80,, +8.26201475,0x08,, +8.26211025,0x00,, +8.262205625,0xD5,, +8.262301125,0x42,, +8.262396625,0xAB,, +8.262492,0x80,, +8.2625875,0x0D,, +8.262683,0x54,, +8.2627785,0x80,, +8.262873875,0x08,, +8.262969375,0x00,, +8.263064875,0x2A,, +8.263160375,0x62,, +8.263255875,0xA6,, +8.26335125,0x5A,, +8.280136125,0x55,, +8.280231625,0x55,, +8.280327125,0x18,, +8.280422625,0x00,, +8.280518,0x17,, +8.2806135,0x85,, +8.280709,0xC8,, +8.2808045,0x00,, +8.280899875,0x2C,, +8.280995375,0x78,, +8.281090875,0x00,, +8.28118625,0x80,, +8.28128175,0x08,, +8.28137725,0x00,, +8.28147275,0xD5,, +8.281568125,0x42,, +8.281663625,0xAB,, +8.281759125,0x80,, +8.281854625,0x0D,, +8.28195,0x54,, +8.2820455,0x80,, +8.282141,0x08,, +8.2822365,0x00,, +8.282331875,0x2A,, +8.282427375,0x62,, +8.282522875,0xA6,, +8.282618375,0xE7,, +8.300080125,0x55,, +8.300175625,0x55,, +8.300271125,0x18,, +8.300366625,0x00,, +8.300462,0x18,, +8.3005575,0x4A,, +8.300653,0xC9,, +8.3007485,0x00,, +8.300843875,0x2C,, +8.300939375,0x78,, +8.301034875,0x00,, +8.30113025,0x80,, +8.30122575,0x08,, +8.30132125,0x00,, +8.30141675,0xD5,, +8.301512125,0x42,, +8.301607625,0xAB,, +8.301703125,0x80,, +8.301798625,0x0D,, +8.301894,0x54,, +8.3019895,0x80,, +8.302085,0x08,, +8.3021805,0x00,, +8.302275875,0x2A,, +8.302371375,0x62,, +8.302466875,0xA6,, +8.302562375,0x32,, +8.320180375,0x55,, +8.320275875,0x55,, +8.320371375,0x18,, +8.32046675,0x00,, +8.32056225,0x19,, +8.32065775,0x11,, +8.320753125,0xCF,, +8.320848625,0x00,, +8.320944125,0x2C,, +8.321039625,0x58,, +8.321135125,0x00,, +8.3212305,0x80,, +8.321326,0x08,, +8.3214215,0x00,, +8.321517,0xD5,, +8.321612375,0x42,, +8.321707875,0xAB,, +8.321803375,0x80,, +8.32189875,0x0D,, +8.32199425,0x54,, +8.32208975,0x80,, +8.32218525,0x08,, +8.322280625,0x00,, +8.322376125,0x2A,, +8.322471625,0x62,, +8.322567125,0xA6,, +8.3226625,0xC5,, +8.340280625,0x55,, +8.340376125,0x55,, +8.3404715,0x18,, +8.340567,0x00,, +8.3406625,0x19,, +8.340758,0xD8,, +8.340853375,0xCF,, +8.340948875,0x00,, +8.341044375,0x2C,, +8.341139875,0x48,, +8.34123525,0x00,, +8.34133075,0x80,, +8.34142625,0x08,, +8.341521625,0x00,, +8.341617125,0xD5,, +8.341712625,0x42,, +8.341808125,0xAB,, +8.3419035,0x80,, +8.341999,0x0D,, +8.3420945,0x54,, +8.34219,0x80,, +8.342285375,0x08,, +8.342380875,0x00,, +8.342476375,0x2A,, +8.342571875,0x62,, +8.34266725,0xA6,, +8.34276275,0x6D,, +8.360372125,0x55,, +8.360467625,0x55,, +8.360563125,0x18,, +8.360658625,0x00,, +8.360754,0x1A,, +8.3608495,0x9D,, +8.360945,0xCF,, +8.361040375,0x00,, +8.361135875,0x2C,, +8.361231375,0x68,, +8.361326875,0x00,, +8.36142225,0x80,, +8.36151775,0x08,, +8.36161325,0x00,, +8.36170875,0xD5,, +8.361804125,0x42,, +8.361899625,0xAB,, +8.361995125,0x80,, +8.3620905,0x0D,, +8.362186,0x54,, +8.3622815,0x80,, +8.362377,0x08,, +8.362472375,0x00,, +8.362567875,0x2A,, +8.362663375,0x62,, +8.362758875,0xA6,, +8.36285425,0x16,, +8.380750125,0x55,, +8.3808455,0x55,, +8.380941,0x18,, +8.3810365,0x00,, +8.381132,0x1B,, +8.3812275,0x66,, +8.381322875,0xC7,, +8.381418375,0x00,, +8.381513875,0x2C,, +8.38160925,0x58,, +8.38170475,0x00,, +8.38180025,0x80,, +8.38189575,0x08,, +8.381991125,0x00,, +8.382086625,0xD5,, +8.382182125,0x42,, +8.382277625,0xAB,, +8.382373,0x80,, +8.3824685,0x0D,, +8.382564,0x54,, +8.382659375,0x80,, +8.382754875,0x08,, +8.382850375,0x00,, +8.382945875,0x2A,, +8.383041375,0x62,, +8.38313675,0xA6,, +8.38323225,0x6F,, +8.400563875,0x55,, +8.400659375,0x55,, +8.400754875,0x18,, +8.400850375,0x00,, +8.40094575,0x1C,, +8.40104125,0x29,, +8.40113675,0xCE,, +8.401232125,0x00,, +8.401327625,0x2C,, +8.401423125,0x68,, +8.401518625,0x00,, +8.401614,0x80,, +8.4017095,0x08,, +8.401805,0x00,, +8.4019005,0xD5,, +8.401995875,0x42,, +8.402091375,0xAB,, +8.402186875,0x80,, +8.40228225,0x0D,, +8.40237775,0x54,, +8.40247325,0x80,, +8.40256875,0x08,, +8.40266425,0x00,, +8.402759625,0x2A,, +8.402855125,0x62,, +8.402950625,0xA6,, +8.403046125,0x4D,, +8.421297625,0x55,, +8.421393125,0x55,, +8.421488625,0x18,, +8.421584125,0x00,, +8.421679625,0x1C,, +8.421775,0xF6,, +8.4218705,0xCB,, +8.421966,0x00,, +8.422061375,0x2C,, +8.422156875,0x68,, +8.422252375,0x00,, +8.422347875,0x80,, +8.42244325,0x08,, +8.42253875,0x00,, +8.42263425,0xD5,, +8.42272975,0x42,, +8.422825125,0xAB,, +8.422920625,0x80,, +8.423016125,0x0D,, +8.4231115,0x54,, +8.423207,0x80,, +8.4233025,0x08,, +8.423398,0x00,, +8.4234935,0x2A,, +8.423588875,0x62,, +8.423684375,0xA6,, +8.423779875,0x6B,, +8.44129375,0x55,, +8.44138925,0x55,, +8.441484625,0x2B,, +8.441580125,0x03,, +8.441675625,0x1D,, +8.441771125,0xB6,, +8.441866625,0xDD,, +8.441962,0x00,, +8.4420575,0x2C,, +8.442153,0x68,, +8.4422485,0x00,, +8.442343875,0x80,, +8.442439375,0x08,, +8.442534875,0x00,, +8.44263025,0xD5,, +8.44272575,0x42,, +8.44282125,0xAB,, +8.44291675,0x80,, +8.443012125,0x0D,, +8.443107625,0x54,, +8.443203125,0x80,, +8.443298625,0x08,, +8.443394,0x00,, +8.4434895,0x2A,, +8.443585,0x62,, +8.443680375,0xA6,, +8.443775875,0x00,, +8.443871375,0x00,, +8.443966875,0x00,, +8.444062375,0x00,, +8.44415775,0x00,, +8.44425325,0x00,, +8.44434875,0x00,, +8.444444125,0x00,, +8.444539625,0x00,, +8.444635125,0x00,, +8.444730625,0x00,, +8.444826,0x00,, +8.4449215,0x00,, +8.445017,0x00,, +8.4451125,0x00,, +8.445207875,0x00,, +8.445303375,0x00,, +8.445398875,0x00,, +8.44549425,0x00,, +8.44558975,0xA6,, +8.460864625,0x55,, +8.46096,0x55,, +8.4610555,0x18,, +8.461151,0x00,, +8.4612465,0x1E,, +8.461341875,0x7C,, +8.461437375,0xDC,, +8.461532875,0x00,, +8.46162825,0x2C,, +8.46172375,0x58,, +8.46181925,0x00,, +8.46191475,0x80,, +8.462010125,0x08,, +8.462105625,0x00,, +8.462201125,0xD5,, +8.462296625,0x42,, +8.462392,0xAB,, +8.4624875,0x80,, +8.462583,0x0D,, +8.462678375,0x54,, +8.462773875,0x80,, +8.462869375,0x08,, +8.462964875,0x00,, +8.463060375,0x2A,, +8.46315575,0x62,, +8.46325125,0xA6,, +8.46334675,0x76,, +8.48096475,0x55,, +8.48106025,0x55,, +8.48115575,0x18,, +8.481251125,0x00,, +8.481346625,0x1F,, +8.481442125,0x42,, +8.481537625,0xDD,, +8.481633,0x00,, +8.4817285,0x2C,, +8.481824,0x68,, +8.4819195,0x00,, +8.482015,0x80,, +8.482110375,0x08,, +8.482205875,0x00,, +8.482301375,0xD5,, +8.48239675,0x42,, +8.48249225,0xAB,, +8.48258775,0x80,, +8.48268325,0x0D,, +8.482778625,0x54,, +8.482874125,0x80,, +8.482969625,0x08,, +8.483065125,0x00,, +8.4831605,0x2A,, +8.483256,0x62,, +8.4833515,0xA6,, +8.483446875,0x27,, +8.501065,0x55,, +8.5011605,0x55,, +8.501255875,0x18,, +8.501351375,0x00,, +8.501446875,0x20,, +8.501542375,0x09,, +8.501637875,0xDC,, +8.50173325,0x00,, +8.50182875,0x2C,, +8.50192425,0x78,, +8.502019625,0x00,, +8.502115125,0x80,, +8.502210625,0x08,, +8.502306125,0x00,, +8.5024015,0xD5,, +8.502497,0x42,, +8.5025925,0xAB,, +8.502688,0x80,, +8.502783375,0x0D,, +8.502878875,0x54,, +8.502974375,0x80,, +8.50306975,0x08,, +8.50316525,0x00,, +8.50326075,0x2A,, +8.50335625,0x62,, +8.50345175,0xA6,, +8.503547125,0x6F,, +8.5211565,0x55,, +8.521252,0x55,, +8.5213475,0x18,, +8.521443,0x00,, +8.521538375,0x20,, +8.521633875,0xCF,, +8.521729375,0xDB,, +8.521824875,0x00,, +8.52192025,0x2C,, +8.52201575,0x68,, +8.52211125,0x00,, +8.52220675,0x80,, +8.522302125,0x08,, +8.522397625,0x00,, +8.522493125,0xD5,, +8.5225885,0x42,, +8.522684,0xAB,, +8.5227795,0x80,, +8.522875,0x0D,, +8.522970375,0x54,, +8.523065875,0x80,, +8.523161375,0x08,, +8.523256875,0x00,, +8.52335225,0x2A,, +8.52344775,0x62,, +8.52354325,0xA6,, +8.523638625,0x63,, +8.54125675,0x55,, +8.54135225,0x55,, +8.54144775,0x18,, +8.541543125,0x00,, +8.541638625,0x21,, +8.541734125,0x95,, +8.541829625,0xD9,, +8.541925,0x00,, +8.5420205,0x2C,, +8.542116,0x48,, +8.5422115,0x00,, +8.542306875,0x80,, +8.542402375,0x08,, +8.542497875,0x00,, +8.54259325,0xD5,, +8.54268875,0x42,, +8.54278425,0xAB,, +8.54287975,0x80,, +8.54297525,0x0D,, +8.543070625,0x54,, +8.543166125,0x80,, +8.543261625,0x08,, +8.543357,0x00,, +8.5434525,0x2A,, +8.543548,0x62,, +8.5436435,0xA6,, +8.543738875,0xE0,, +8.561357,0x55,, +8.5614525,0x55,, +8.561547875,0x18,, +8.561643375,0x00,, +8.561738875,0x22,, +8.561834375,0x5B,, +8.56192975,0xDC,, +8.56202525,0x00,, +8.56212075,0x2C,, +8.562216125,0x68,, +8.562311625,0x00,, +8.562407125,0x80,, +8.562502625,0x08,, +8.562598125,0x00,, +8.5626935,0xD5,, +8.562789,0x42,, +8.5628845,0xAB,, +8.562979875,0x80,, +8.563075375,0x0D,, +8.563170875,0x54,, +8.563266375,0x80,, +8.56336175,0x08,, +8.56345725,0x00,, +8.56355275,0x2A,, +8.56364825,0x62,, +8.563743625,0xA6,, +8.563839125,0x57,, +8.5814485,0x55,, +8.581544,0x55,, +8.5816395,0x18,, +8.581734875,0x00,, +8.581830375,0x23,, +8.581925875,0x21,, +8.582021375,0xDD,, +8.58211675,0x00,, +8.58221225,0x2C,, +8.58230775,0x68,, +8.58240325,0x00,, +8.582498625,0x80,, +8.582594125,0x08,, +8.582689625,0x00,, +8.582785,0xD5,, +8.5828805,0x42,, +8.582976,0xAB,, +8.5830715,0x80,, +8.583167,0x0D,, +8.583262375,0x54,, +8.583357875,0x80,, +8.583453375,0x08,, +8.58354875,0x00,, +8.58364425,0x2A,, +8.58373975,0x62,, +8.58383525,0xA6,, +8.583930625,0xAF,, +8.60154875,0x55,, +8.60164425,0x55,, +8.601739625,0x18,, +8.601835125,0x00,, +8.601930625,0x23,, +8.602026125,0xE8,, +8.602121625,0xDD,, +8.602217,0x00,, +8.6023125,0x2C,, +8.602407875,0x68,, +8.602503375,0x00,, +8.602598875,0x80,, +8.602694375,0x08,, +8.602789875,0x00,, +8.60288525,0xD5,, +8.60298075,0x42,, +8.60307625,0xAB,, +8.60317175,0x80,, +8.603267125,0x0D,, +8.603362625,0x54,, +8.603458125,0x80,, +8.6035535,0x08,, +8.603649,0x00,, +8.6037445,0x2A,, +8.60384,0x62,, +8.603935375,0xA6,, +8.604030875,0xE7,, +8.621649,0x55,, +8.6217445,0x55,, +8.621839875,0x18,, +8.621935375,0x00,, +8.622030875,0x24,, +8.62212625,0xAF,, +8.62222175,0xD9,, +8.62231725,0x00,, +8.62241275,0x2C,, +8.622508125,0x48,, +8.622603625,0x00,, +8.622699125,0x80,, +8.622794625,0x08,, +8.62289,0x00,, +8.6229855,0xD5,, +8.623081,0x42,, +8.623176375,0xAB,, +8.623271875,0x80,, +8.623367375,0x0D,, +8.623462875,0x54,, +8.623558375,0x80,, +8.62365375,0x08,, +8.62374925,0x00,, +8.62384475,0x2A,, +8.62394025,0x62,, +8.624035625,0xA6,, +8.624131125,0xEC,, +8.642269875,0x55,, +8.642365375,0x55,, +8.642460875,0x2B,, +8.642556375,0x03,, +8.64265175,0x25,, +8.64274725,0x75,, +8.64284275,0xDB,, +8.64293825,0x00,, +8.643033625,0x2C,, +8.643129125,0x78,, +8.643224625,0x00,, +8.64332,0x80,, +8.6434155,0x08,, +8.643511,0x00,, +8.6436065,0xD5,, +8.643701875,0x42,, +8.643797375,0xAB,, +8.643892875,0x80,, +8.643988375,0x0D,, +8.64408375,0x54,, +8.64417925,0x80,, +8.64427475,0x08,, +8.644370125,0x00,, +8.644465625,0x2A,, +8.644561125,0x62,, +8.644656625,0xA6,, +8.644752125,0x00,, +8.6448475,0x00,, +8.644943,0x00,, +8.6450385,0x00,, +8.645134,0x00,, +8.645229375,0x00,, +8.645324875,0x00,, +8.645420375,0x00,, +8.64551575,0x00,, +8.64561125,0x00,, +8.64570675,0x00,, +8.64580225,0x00,, +8.645897625,0x00,, +8.645993125,0x00,, +8.646088625,0x00,, +8.646184125,0x00,, +8.6462795,0x00,, +8.646375,0x00,, +8.6464705,0x00,, +8.646566,0x78,, +8.66184075,0x55,, +8.66193625,0x55,, +8.662031625,0x18,, +8.662127125,0x00,, +8.662222625,0x26,, +8.662318,0x3C,, +8.6624135,0xDC,, +8.662509,0x00,, +8.6626045,0x2C,, +8.662699875,0x58,, +8.662795375,0x00,, +8.662890875,0x80,, +8.662986375,0x08,, +8.663081875,0x00,, +8.66317725,0xD5,, +8.66327275,0x42,, +8.66336825,0xAB,, +8.663463625,0x80,, +8.663559125,0x0D,, +8.663654625,0x54,, +8.663750125,0x80,, +8.6638455,0x08,, +8.663941,0x00,, +8.6640365,0x2A,, +8.664132,0x62,, +8.664227375,0xA6,, +8.664322875,0x32,, +8.681941,0x55,, +8.682036375,0x55,, +8.682131875,0x18,, +8.682227375,0x00,, +8.68232275,0x27,, +8.68241825,0x03,, +8.68251375,0xDD,, +8.68260925,0x00,, +8.68270475,0x2C,, +8.682800125,0x58,, +8.682895625,0x00,, +8.682991125,0x80,, +8.6830865,0x08,, +8.683182,0x00,, +8.6832775,0xD5,, +8.683373,0x42,, +8.683468375,0xAB,, +8.683563875,0x80,, +8.683659375,0x0D,, +8.683754875,0x54,, +8.68385025,0x80,, +8.68394575,0x08,, +8.68404125,0x00,, +8.684136625,0x2A,, +8.684232125,0x62,, +8.684327625,0xA6,, +8.684423125,0x80,, +8.702041125,0x55,, +8.702136625,0x55,, +8.702232125,0x18,, +8.702327625,0x00,, +8.702423,0x27,, +8.7025185,0xC8,, +8.702614,0xDB,, +8.7027095,0x00,, +8.702804875,0x2C,, +8.702900375,0x68,, +8.702995875,0x00,, +8.70309125,0x80,, +8.70318675,0x08,, +8.70328225,0x00,, +8.70337775,0xD5,, +8.703473125,0x42,, +8.703568625,0xAB,, +8.703664125,0x80,, +8.703759625,0x0D,, +8.703855,0x54,, +8.7039505,0x80,, +8.704046,0x08,, +8.704141375,0x00,, +8.704236875,0x2A,, +8.704332375,0x62,, +8.704427875,0xA6,, +8.704523375,0x88,, +8.722262875,0x55,, +8.722358375,0x55,, +8.722453875,0x18,, +8.72254925,0x00,, +8.72264475,0x28,, +8.72274025,0x8E,, +8.72283575,0xDD,, +8.722931125,0x00,, +8.723026625,0x2C,, +8.723122125,0x68,, +8.723217625,0x00,, +8.723313,0x80,, +8.7234085,0x08,, +8.723504,0x00,, +8.723599375,0xD5,, +8.723694875,0x42,, +8.723790375,0xAB,, +8.723885875,0x80,, +8.72398125,0x0D,, +8.72407675,0x54,, +8.72417225,0x80,, +8.72426775,0x08,, +8.724363125,0x00,, +8.724458625,0x2A,, +8.724554125,0x62,, +8.7246495,0xA6,, +8.724745,0x46,, +8.7429185,0x55,, +8.743014,0x55,, +8.7431095,0x18,, +8.743205,0x00,, +8.743300375,0x29,, +8.743395875,0x5A,, +8.743491375,0xCC,, +8.743586875,0x00,, +8.74368225,0x2C,, +8.74377775,0x58,, +8.74387325,0x00,, +8.74396875,0x80,, +8.744064125,0x08,, +8.744159625,0x00,, +8.744255125,0xD5,, +8.744350625,0x42,, +8.744446,0xAB,, +8.7445415,0x80,, +8.744637,0x0D,, +8.744732375,0x54,, +8.744827875,0x80,, +8.744923375,0x08,, +8.745018875,0x00,, +8.74511425,0x2A,, +8.74520975,0x62,, +8.74530525,0xA6,, +8.74540075,0x12,, +8.76314025,0x55,, +8.76323575,0x55,, +8.76333125,0x18,, +8.763426625,0x00,, +8.763522125,0x2A,, +8.763617625,0x23,, +8.763713125,0xD0,, +8.7638085,0x00,, +8.763904,0x2C,, +8.7639995,0x48,, +8.764095,0x00,, +8.764190375,0x80,, +8.764285875,0x08,, +8.764381375,0x00,, +8.76447675,0xD5,, +8.76457225,0x42,, +8.76466775,0xAB,, +8.76476325,0x80,, +8.76485875,0x0D,, +8.764954125,0x54,, +8.765049625,0x80,, +8.765145125,0x08,, +8.7652405,0x00,, +8.765336,0x2A,, +8.7654315,0x62,, +8.765527,0xA6,, +8.765622375,0xD0,, +8.782554875,0x55,, +8.782650375,0x55,, +8.78274575,0x18,, +8.78284125,0x00,, +8.78293675,0x2A,, +8.78303225,0xE2,, +8.783127625,0xDD,, +8.783223125,0x00,, +8.783318625,0x2C,, +8.783414125,0x58,, +8.7835095,0x00,, +8.783605,0x80,, +8.7837005,0x08,, +8.783795875,0x00,, +8.783891375,0xD5,, +8.783986875,0x42,, +8.784082375,0xAB,, +8.784177875,0x80,, +8.78427325,0x0D,, +8.78436875,0x54,, +8.78446425,0x80,, +8.784559625,0x08,, +8.784655125,0x00,, +8.784750625,0x2A,, +8.784846125,0x62,, +8.7849415,0xA6,, +8.785037,0xB1,, +8.802655125,0x55,, +8.8027505,0x55,, +8.802846,0x18,, +8.8029415,0x00,, +8.803037,0x2B,, +8.803132375,0xA9,, +8.803227875,0xDD,, +8.803323375,0x00,, +8.80341875,0x2C,, +8.80351425,0x48,, +8.80360975,0x00,, +8.80370525,0x80,, +8.80380075,0x08,, +8.803896125,0x00,, +8.803991625,0xD5,, +8.804087125,0x42,, +8.804182625,0xAB,, +8.804278,0x80,, +8.8043735,0x0D,, +8.804469,0x54,, +8.804564375,0x80,, +8.804659875,0x08,, +8.804755375,0x00,, +8.804850875,0x2A,, +8.80494625,0x62,, +8.80504175,0xA6,, +8.80513725,0xED,, +8.82263375,0x55,, +8.82272925,0x55,, +8.82282475,0x18,, +8.82292025,0x00,, +8.823015625,0x2C,, +8.823111125,0x6E,, +8.823206625,0xDB,, +8.823302,0x00,, +8.8233975,0x2C,, +8.823493,0x58,, +8.8235885,0x00,, +8.823684,0x80,, +8.823779375,0x08,, +8.823874875,0x00,, +8.823970375,0xD5,, +8.824065875,0x42,, +8.82416125,0xAB,, +8.82425675,0x80,, +8.82435225,0x0D,, +8.824447625,0x54,, +8.824543125,0x80,, +8.824638625,0x08,, +8.824734125,0x00,, +8.8248295,0x2A,, +8.824925,0x62,, +8.8250205,0xA6,, +8.825116,0xB0,, +8.843124625,0x55,, +8.84322,0x55,, +8.8433155,0x2B,, +8.843411,0x03,, +8.843506375,0x2D,, +8.843601875,0x33,, +8.843697375,0xDB,, +8.843792875,0x00,, +8.843888375,0x2C,, +8.84398375,0x58,, +8.84407925,0x00,, +8.84417475,0x80,, +8.844270125,0x08,, +8.844365625,0x00,, +8.844461125,0xD5,, +8.844556625,0x42,, +8.844652,0xAB,, +8.8447475,0x80,, +8.844843,0x0D,, +8.8449385,0x54,, +8.845033875,0x80,, +8.845129375,0x08,, +8.845224875,0x00,, +8.84532025,0x2A,, +8.84541575,0x62,, +8.84551125,0xA6,, +8.84560675,0x00,, +8.845702125,0x00,, +8.845797625,0x00,, +8.845893125,0x00,, +8.845988625,0x00,, +8.846084125,0x00,, +8.8461795,0x00,, +8.846275,0x00,, +8.8463705,0x00,, +8.846465875,0x00,, +8.846561375,0x00,, +8.846656875,0x00,, +8.846752375,0x00,, +8.84684775,0x00,, +8.84694325,0x00,, +8.84703875,0x00,, +8.84713425,0x00,, +8.847229625,0x00,, +8.847325125,0x00,, +8.847420625,0x92,, +8.8628255,0x55,, +8.862921,0x55,, +8.8630165,0x18,, +8.863112,0x00,, +8.8632075,0x2D,, +8.863302875,0xFA,, +8.863398375,0xDB,, +8.863493875,0x00,, +8.86358925,0x2C,, +8.86368475,0x48,, +8.86378025,0x00,, +8.86387575,0x80,, +8.863971125,0x08,, +8.864066625,0x00,, +8.864162125,0xD5,, +8.864257625,0x42,, +8.864353,0xAB,, +8.8644485,0x80,, +8.864544,0x0D,, +8.864639375,0x54,, +8.864734875,0x80,, +8.864830375,0x08,, +8.864925875,0x00,, +8.865021375,0x2A,, +8.86511675,0x62,, +8.86521225,0xA6,, +8.86530775,0x56,, +8.88292575,0x55,, +8.88302125,0x55,, +8.88311675,0x18,, +8.883212125,0x00,, +8.883307625,0x2E,, +8.883403125,0xC0,, +8.883498625,0xDA,, +8.883594,0x00,, +8.8836895,0x2C,, +8.883785,0x58,, +8.8838805,0x00,, +8.883975875,0x80,, +8.884071375,0x08,, +8.884166875,0x00,, +8.884262375,0xD5,, +8.88435775,0x42,, +8.88445325,0xAB,, +8.88454875,0x80,, +8.88464425,0x0D,, +8.884739625,0x54,, +8.884835125,0x80,, +8.884930625,0x08,, +8.885026125,0x00,, +8.8851215,0x2A,, +8.885217,0x62,, +8.8853125,0xA6,, +8.885407875,0x7D,, +8.903026,0x55,, +8.9031215,0x55,, +8.903216875,0x18,, +8.903312375,0x00,, +8.903407875,0x2F,, +8.903503375,0x86,, +8.90359875,0xDD,, +8.90369425,0x00,, +8.90378975,0x2C,, +8.90388525,0x78,, +8.903980625,0x00,, +8.904076125,0x80,, +8.904171625,0x08,, +8.904267125,0x00,, +8.9043625,0xD5,, +8.904458,0x42,, +8.9045535,0xAB,, +8.904649,0x80,, +8.904744375,0x0D,, +8.904839875,0x54,, +8.904935375,0x80,, +8.90503075,0x08,, +8.90512625,0x00,, +8.90522175,0x2A,, +8.90531725,0x62,, +8.905412625,0xA6,, +8.905508125,0x2D,, +8.9231175,0x55,, +8.923213,0x55,, +8.9233085,0x18,, +8.923404,0x00,, +8.923499375,0x30,, +8.923594875,0x4C,, +8.923690375,0xDC,, +8.92378575,0x00,, +8.92388125,0x2C,, +8.92397675,0x78,, +8.92407225,0x00,, +8.92416775,0x80,, +8.924263125,0x08,, +8.924358625,0x00,, +8.924454125,0xD5,, +8.9245495,0x42,, +8.924645,0xAB,, +8.9247405,0x80,, +8.924836,0x0D,, +8.924931375,0x54,, +8.925026875,0x80,, +8.925122375,0x08,, +8.925217875,0x00,, +8.92531325,0x2A,, +8.92540875,0x62,, +8.92550425,0xA6,, +8.925599625,0x1E,, +8.9432265,0x55,, +8.943321875,0x55,, +8.943417375,0x18,, +8.943512875,0x00,, +8.94360825,0x31,, +8.94370375,0x11,, +8.94379925,0xDA,, +8.94389475,0x00,, +8.943990125,0x2C,, +8.944085625,0x58,, +8.944181125,0x00,, +8.944276625,0x80,, +8.944372125,0x08,, +8.9444675,0x00,, +8.944563,0xD5,, +8.944658375,0x42,, +8.944753875,0xAB,, +8.944849375,0x80,, +8.944944875,0x0D,, +8.945040375,0x54,, +8.94513575,0x80,, +8.94523125,0x08,, +8.94532675,0x00,, +8.94542225,0x2A,, +8.945517625,0x62,, +8.945613125,0xA6,, +8.945708625,0x7F,, +8.963318,0x55,, +8.9634135,0x55,, +8.963508875,0x18,, +8.963604375,0x00,, +8.963699875,0x31,, +8.963795375,0xD8,, +8.96389075,0xDC,, +8.96398625,0x00,, +8.96408175,0x2C,, +8.964177125,0x68,, +8.964272625,0x00,, +8.964368125,0x80,, +8.964463625,0x08,, +8.964559,0x00,, +8.9646545,0xD5,, +8.96475,0x42,, +8.9648455,0xAB,, +8.964940875,0x80,, +8.965036375,0x0D,, +8.965131875,0x54,, +8.965227375,0x80,, +8.96532275,0x08,, +8.96541825,0x00,, +8.96551375,0x2A,, +8.96560925,0x62,, +8.965704625,0xA6,, +8.965800125,0xF8,, +8.9834095,0x55,, +8.983505,0x55,, +8.9836005,0x18,, +8.983695875,0x00,, +8.983791375,0x32,, +8.983886875,0x9F,, +8.983982375,0xDA,, +8.98407775,0x00,, +8.98417325,0x2C,, +8.98426875,0x58,, +8.98436425,0x00,, +8.984459625,0x80,, +8.984555125,0x08,, +8.984650625,0x00,, +8.984746,0xD5,, +8.9848415,0x42,, +8.984937,0xAB,, +8.9850325,0x80,, +8.985128,0x0D,, +8.985223375,0x54,, +8.985318875,0x80,, +8.985414375,0x08,, +8.98550975,0x00,, +8.98560525,0x2A,, +8.98570075,0x62,, +8.98579625,0xA6,, +8.985891625,0x04,, +9.003518375,0x55,, +9.003613875,0x55,, +9.003709375,0x18,, +9.00380475,0x00,, +9.00390025,0x33,, +9.00399575,0x66,, +9.00409125,0xDC,, +9.00418675,0x00,, +9.004282125,0x2C,, +9.004377625,0x58,, +9.004473125,0x00,, +9.0045685,0x80,, +9.004664,0x08,, +9.0047595,0x00,, +9.004855,0xD5,, +9.004950375,0x42,, +9.005045875,0xAB,, +9.005141375,0x80,, +9.005236875,0x0D,, +9.00533225,0x54,, +9.00542775,0x80,, +9.00552325,0x08,, +9.00561875,0x00,, +9.005714125,0x2A,, +9.005809625,0x62,, +9.005905125,0xA6,, +9.006000625,0x5A,, +9.02361,0x55,, +9.023705375,0x55,, +9.023800875,0x18,, +9.023896375,0x00,, +9.023991875,0x34,, +9.02408725,0x2D,, +9.02418275,0xD9,, +9.02427825,0x00,, +9.02437375,0x2C,, +9.024469125,0x48,, +9.024564625,0x00,, +9.024660125,0x80,, +9.024755625,0x08,, +9.024851,0x00,, +9.0249465,0xD5,, +9.025042,0x42,, +9.025137375,0xAB,, +9.025232875,0x80,, +9.025328375,0x0D,, +9.025423875,0x54,, +9.02551925,0x80,, +9.02561475,0x08,, +9.02571025,0x00,, +9.02580575,0x2A,, +9.02590125,0x62,, +9.025996625,0xA6,, +9.026092125,0x71,, +9.044109375,0x55,, +9.044204875,0x55,, +9.044300375,0x2B,, +9.04439575,0x03,, +9.04449125,0x34,, +9.04458675,0xF4,, +9.04468225,0xD9,, +9.044777625,0x00,, +9.044873125,0x2C,, +9.044968625,0x48,, +9.045064125,0x00,, +9.0451595,0x80,, +9.045255,0x08,, +9.0453505,0x00,, +9.045446,0xD5,, +9.045541375,0x42,, +9.045636875,0xAB,, +9.045732375,0x80,, +9.045827875,0x0D,, +9.04592325,0x54,, +9.04601875,0x80,, +9.04611425,0x08,, +9.04620975,0x00,, +9.046305125,0x2A,, +9.046400625,0x62,, +9.046496125,0xA6,, +9.0465915,0x00,, +9.046687,0x00,, +9.0467825,0x00,, +9.046878,0x00,, +9.046973375,0x00,, +9.047068875,0x00,, +9.047164375,0x00,, +9.047259875,0x00,, +9.04735525,0x00,, +9.04745075,0x00,, +9.04754625,0x00,, +9.04764175,0x00,, +9.047737125,0x00,, +9.047832625,0x00,, +9.047928125,0x00,, +9.048023625,0x00,, +9.048119,0x00,, +9.0482145,0x00,, +9.04831,0x00,, +9.048405375,0x7F,, +9.063810375,0x55,, +9.063905875,0x55,, +9.064001375,0x18,, +9.06409675,0x00,, +9.06419225,0x35,, +9.06428775,0xBB,, +9.06438325,0xDB,, +9.064478625,0x00,, +9.064574125,0x2C,, +9.064669625,0x58,, +9.064765,0x00,, +9.0648605,0x80,, +9.064956,0x08,, +9.0650515,0x00,, +9.065147,0xD5,, +9.065242375,0x42,, +9.065337875,0xAB,, +9.065433375,0x80,, +9.065528875,0x0D,, +9.06562425,0x54,, +9.06571975,0x80,, +9.06581525,0x08,, +9.065910625,0x00,, +9.066006125,0x2A,, +9.066101625,0x62,, +9.066197125,0xA6,, +9.0662925,0x40,, +9.083910625,0x55,, +9.084006125,0x55,, +9.084101625,0x18,, +9.084197,0x00,, +9.0842925,0x36,, +9.084388,0x81,, +9.084483375,0xDA,, +9.084578875,0x00,, +9.084674375,0x2C,, +9.084769875,0x68,, +9.08486525,0x00,, +9.08496075,0x80,, +9.08505625,0x08,, +9.08515175,0x00,, +9.085247125,0xD5,, +9.085342625,0x42,, +9.085438125,0xAB,, +9.0855335,0x80,, +9.085629,0x0D,, +9.0857245,0x54,, +9.08582,0x80,, +9.085915375,0x08,, +9.086010875,0x00,, +9.086106375,0x2A,, +9.086201875,0x62,, +9.08629725,0xA6,, +9.08639275,0xAC,, +9.104002125,0x55,, +9.104097625,0x55,, +9.104193125,0x18,, +9.1042885,0x00,, +9.104384,0x37,, +9.1044795,0x47,, +9.104575,0xDD,, +9.1046705,0x00,, +9.104765875,0x2C,, +9.104861375,0x48,, +9.104956875,0x00,, +9.10505225,0x80,, +9.10514775,0x08,, +9.10524325,0x00,, +9.10533875,0xD5,, +9.105434125,0x42,, +9.105529625,0xAB,, +9.105625125,0x80,, +9.105720625,0x0D,, +9.105816,0x54,, +9.1059115,0x80,, +9.106007,0x08,, +9.106102375,0x00,, +9.106197875,0x2A,, +9.106293375,0x62,, +9.106388875,0xA6,, +9.106484375,0xD5,, +9.124102375,0x55,, +9.124197875,0x55,, +9.124293375,0x18,, +9.12438875,0x00,, +9.12448425,0x38,, +9.12457975,0x0E,, +9.124675125,0xDC,, +9.124770625,0x00,, +9.124866125,0x2C,, +9.124961625,0x58,, +9.125057,0x00,, +9.1251525,0x80,, +9.125248,0x08,, +9.1253435,0x00,, +9.125438875,0xD5,, +9.125534375,0x42,, +9.125629875,0xAB,, +9.12572525,0x80,, +9.12582075,0x0D,, +9.12591625,0x54,, +9.12601175,0x80,, +9.12610725,0x08,, +9.126202625,0x00,, +9.126298125,0x2A,, +9.126393625,0x62,, +9.126489125,0xA6,, +9.1265845,0x5F,, +9.144193875,0x55,, +9.144289375,0x55,, +9.144384875,0x18,, +9.144480375,0x00,, +9.14457575,0x38,, +9.14467125,0xD4,, +9.14476675,0xD9,, +9.14486225,0x00,, +9.144957625,0x2C,, +9.145053125,0x58,, +9.145148625,0x00,, +9.145244,0x80,, +9.1453395,0x08,, +9.145435,0x00,, +9.1455305,0xD5,, +9.145625875,0x42,, +9.145721375,0xAB,, +9.145816875,0x80,, +9.145912375,0x0D,, +9.14600775,0x54,, +9.14610325,0x80,, +9.14619875,0x08,, +9.14629425,0x00,, +9.146389625,0x2A,, +9.146485125,0x62,, +9.146580625,0xA6,, +9.146676125,0xA4,, +9.16430275,0x55,, +9.16439825,0x55,, +9.16449375,0x18,, +9.16458925,0x00,, +9.164684625,0x39,, +9.164780125,0x9A,, +9.164875625,0xDC,, +9.164971125,0x00,, +9.165066625,0x2C,, +9.165162,0x58,, +9.1652575,0x00,, +9.165353,0x80,, +9.165448375,0x08,, +9.165543875,0x00,, +9.165639375,0xD5,, +9.165734875,0x42,, +9.16583025,0xAB,, +9.16592575,0x80,, +9.16602125,0x0D,, +9.16611675,0x54,, +9.166212125,0x80,, +9.166307625,0x08,, +9.166403125,0x00,, +9.1664985,0x2A,, +9.166594,0x62,, +9.1666895,0xA6,, +9.166785,0x59,, +9.184394375,0x55,, +9.184489875,0x55,, +9.18458525,0x18,, +9.18468075,0x00,, +9.18477625,0x3A,, +9.184871625,0x60,, +9.184967125,0xD9,, +9.185062625,0x00,, +9.185158125,0x2C,, +9.185253625,0x58,, +9.185349,0x00,, +9.1854445,0x80,, +9.18554,0x08,, +9.185635375,0x00,, +9.185730875,0xD5,, +9.185826375,0x42,, +9.185921875,0xAB,, +9.18601725,0x80,, +9.18611275,0x0D,, +9.18620825,0x54,, +9.18630375,0x80,, +9.186399125,0x08,, +9.186494625,0x00,, +9.186590125,0x2A,, +9.186685625,0x62,, +9.186781,0xA6,, +9.1868765,0x9C,, +9.204485875,0x55,, +9.204581375,0x55,, +9.204676875,0x18,, +9.20477225,0x00,, +9.20486775,0x3B,, +9.20496325,0x26,, +9.20505875,0xD8,, +9.205154125,0x00,, +9.205249625,0x2C,, +9.205345125,0x48,, +9.205440625,0x00,, +9.205536,0x80,, +9.2056315,0x08,, +9.205727,0x00,, +9.2058225,0xD5,, +9.205917875,0x42,, +9.206013375,0xAB,, +9.206108875,0x80,, +9.20620425,0x0D,, +9.20629975,0x54,, +9.20639525,0x80,, +9.20649075,0x08,, +9.206586125,0x00,, +9.206681625,0x2A,, +9.206777125,0x62,, +9.206872625,0xA6,, +9.206968125,0x03,, +9.224586125,0x55,, +9.224681625,0x55,, +9.224777125,0x18,, +9.2248725,0x00,, +9.224968,0x3B,, +9.2250635,0xED,, +9.225158875,0xDA,, +9.225254375,0x00,, +9.225349875,0x2C,, +9.225445375,0x38,, +9.22554075,0x00,, +9.22563625,0x80,, +9.22573175,0x08,, +9.22582725,0x00,, +9.225922625,0xD5,, +9.226018125,0x42,, +9.226113625,0xAB,, +9.226209,0x80,, +9.2263045,0x0D,, +9.2264,0x54,, +9.2264955,0x80,, +9.226591,0x08,, +9.226686375,0x00,, +9.226781875,0x2A,, +9.226877375,0x62,, +9.22697275,0xA6,, +9.22706825,0x32,, +9.245085625,0x55,, +9.245181,0x55,, +9.2452765,0x2B,, +9.245372,0x03,, +9.245467375,0x3C,, +9.245562875,0xB2,, +9.245658375,0xDB,, +9.245753875,0x00,, +9.24584925,0x2C,, +9.24594475,0x58,, +9.24604025,0x00,, +9.24613575,0x80,, +9.246231125,0x08,, +9.246326625,0x00,, +9.246422125,0xD5,, +9.246517625,0x42,, +9.246613,0xAB,, +9.2467085,0x80,, +9.246804,0x0D,, +9.2468995,0x54,, +9.246994875,0x80,, +9.247090375,0x08,, +9.247185875,0x00,, +9.24728125,0x2A,, +9.24737675,0x62,, +9.24747225,0xA6,, +9.24756775,0x00,, +9.247663125,0x00,, +9.247758625,0x00,, +9.247854125,0x00,, +9.247949625,0x00,, +9.248045125,0x00,, +9.2481405,0x00,, +9.248236,0x00,, +9.2483315,0x00,, +9.248426875,0x00,, +9.248522375,0x00,, +9.248617875,0x00,, +9.248713375,0x00,, +9.24880875,0x00,, +9.24890425,0x00,, +9.24899975,0x00,, +9.24909525,0x00,, +9.249190625,0x00,, +9.249286125,0x00,, +9.249381625,0xD5,, +9.2647865,0x55,, +9.264882,0x55,, +9.2649775,0x18,, +9.265073,0x00,, +9.265168375,0x3D,, +9.265263875,0x79,, +9.265359375,0xDC,, +9.265454875,0x00,, +9.26555025,0x2C,, +9.26564575,0x78,, +9.26574125,0x00,, +9.26583675,0x80,, +9.265932125,0x08,, +9.266027625,0x00,, +9.266123125,0xD5,, +9.266218625,0x42,, +9.266314,0xAB,, +9.2664095,0x80,, +9.266505,0x0D,, +9.266600375,0x54,, +9.266695875,0x80,, +9.266791375,0x08,, +9.266886875,0x00,, +9.26698225,0x2A,, +9.26707775,0x62,, +9.26717325,0xA6,, +9.26726875,0xEC,, +9.28488675,0x55,, +9.28498225,0x55,, +9.28507775,0x18,, +9.285173125,0x00,, +9.285268625,0x3E,, +9.285364125,0x3F,, +9.285459625,0xD8,, +9.285555,0x00,, +9.2856505,0x2C,, +9.285746,0x58,, +9.2858415,0x00,, +9.285936875,0x80,, +9.286032375,0x08,, +9.286127875,0x00,, +9.286223375,0xD5,, +9.28631875,0x42,, +9.28641425,0xAB,, +9.28650975,0x80,, +9.286605125,0x0D,, +9.286700625,0x54,, +9.286796125,0x80,, +9.286891625,0x08,, +9.286987125,0x00,, +9.2870825,0x2A,, +9.287178,0x62,, +9.2872735,0xA6,, +9.287368875,0x6C,, +9.304987,0x55,, +9.3050825,0x55,, +9.305177875,0x18,, +9.305273375,0x00,, +9.305368875,0x3F,, +9.305464375,0x06,, +9.30555975,0xDB,, +9.30565525,0x00,, +9.30575075,0x2C,, +9.30584625,0x68,, +9.305941625,0x00,, +9.306037125,0x80,, +9.306132625,0x08,, +9.306228,0x00,, +9.3063235,0xD5,, +9.306419,0x42,, +9.3065145,0xAB,, +9.30661,0x80,, +9.306705375,0x0D,, +9.306800875,0x54,, +9.306896375,0x80,, +9.30699175,0x08,, +9.30708725,0x00,, +9.30718275,0x2A,, +9.30727825,0x62,, +9.307373625,0xA6,, +9.307469125,0x37,, +9.32508725,0x55,, +9.325182625,0x55,, +9.325278125,0x18,, +9.325373625,0x00,, +9.325469125,0x3F,, +9.325564625,0xCD,, +9.32566,0xDC,, +9.3257555,0x00,, +9.325851,0x2C,, +9.325946375,0x58,, +9.326041875,0x00,, +9.326137375,0x80,, +9.326232875,0x08,, +9.32632825,0x00,, +9.32642375,0xD5,, +9.32651925,0x42,, +9.32661475,0xAB,, +9.326710125,0x80,, +9.326805625,0x0D,, +9.326901125,0x54,, +9.3269965,0x80,, +9.327092,0x08,, +9.3271875,0x00,, +9.327283,0x2A,, +9.327378375,0x62,, +9.327473875,0xA6,, +9.327569375,0x13,, +9.345170125,0x55,, +9.3452655,0x55,, +9.345361,0x18,, +9.3454565,0x00,, +9.345552,0x40,, +9.345647375,0x92,, +9.345742875,0xDB,, +9.345838375,0x00,, +9.345933875,0x2C,, +9.34602925,0x68,, +9.34612475,0x00,, +9.34622025,0x80,, +9.346315625,0x08,, +9.346411125,0x00,, +9.346506625,0xD5,, +9.346602125,0x42,, +9.346697625,0xAB,, +9.346793,0x80,, +9.3468885,0x0D,, +9.346984,0x54,, +9.347079375,0x80,, +9.347174875,0x08,, +9.347270375,0x00,, +9.347365875,0x2A,, +9.34746125,0x62,, +9.34755675,0xA6,, +9.34765225,0xE5,, +9.365279,0x55,, +9.365374375,0x55,, +9.365469875,0x18,, +9.365565375,0x00,, +9.365660875,0x41,, +9.365756375,0x59,, +9.36585175,0xD7,, +9.36594725,0x00,, +9.36604275,0x2C,, +9.366138125,0x38,, +9.366233625,0x00,, +9.366329125,0x80,, +9.366424625,0x08,, +9.36652,0x00,, +9.3666155,0xD5,, +9.366711,0x42,, +9.3668065,0xAB,, +9.366901875,0x80,, +9.366997375,0x0D,, +9.367092875,0x54,, +9.36718825,0x80,, +9.36728375,0x08,, +9.36737925,0x00,, +9.36747475,0x2A,, +9.36757025,0x62,, +9.367665625,0xA6,, +9.367761125,0xCE,, +9.38537925,0x55,, +9.385474625,0x55,, +9.385570125,0x18,, +9.385665625,0x00,, +9.385761125,0x42,, +9.3858565,0x1F,, +9.385952,0xD9,, +9.3860475,0x00,, +9.386142875,0x2C,, +9.386238375,0x38,, +9.386333875,0x00,, +9.386429375,0x80,, +9.38652475,0x08,, +9.38662025,0x00,, +9.38671575,0xD5,, +9.38681125,0x42,, +9.386906625,0xAB,, +9.387002125,0x80,, +9.387097625,0x0D,, +9.387193125,0x54,, +9.3872885,0x80,, +9.387384,0x08,, +9.3874795,0x00,, +9.387575,0x2A,, +9.387670375,0x62,, +9.387765875,0xA6,, +9.387861375,0xB6,, +9.40547075,0x55,, +9.40556625,0x55,, +9.405661625,0x18,, +9.405757125,0x00,, +9.405852625,0x42,, +9.405948125,0xE5,, +9.4060435,0xDC,, +9.406139,0x00,, +9.4062345,0x2C,, +9.406329875,0x68,, +9.406425375,0x00,, +9.406520875,0x80,, +9.406616375,0x08,, +9.40671175,0x00,, +9.40680725,0xD5,, +9.40690275,0x42,, +9.40699825,0xAB,, +9.40709375,0x80,, +9.407189125,0x0D,, +9.407284625,0x54,, +9.407380125,0x80,, +9.4074755,0x08,, +9.407571,0x00,, +9.4076665,0x2A,, +9.407762,0x62,, +9.407857375,0xA6,, +9.407952875,0xEC,, +9.425571,0x55,, +9.425666375,0x55,, +9.425761875,0x18,, +9.425857375,0x00,, +9.425952875,0x43,, +9.42604825,0xAA,, +9.42614375,0xD9,, +9.42623925,0x00,, +9.426334625,0x2C,, +9.426430125,0x68,, +9.426525625,0x00,, +9.426621125,0x80,, +9.426716625,0x08,, +9.426812,0x00,, +9.4269075,0xD5,, +9.427003,0x42,, +9.427098375,0xAB,, +9.427193875,0x80,, +9.427289375,0x0D,, +9.427384875,0x54,, +9.42748025,0x80,, +9.42757575,0x08,, +9.42767125,0x00,, +9.42776675,0x2A,, +9.427862125,0x62,, +9.427957625,0xA6,, +9.428053125,0xD5,, +9.446070375,0x55,, +9.446165875,0x55,, +9.446261375,0x2B,, +9.44635675,0x03,, +9.44645225,0x44,, +9.44654775,0x71,, +9.44664325,0xDC,, +9.446738625,0x00,, +9.446834125,0x2C,, +9.446929625,0x38,, +9.447025125,0x00,, +9.4471205,0x80,, +9.447216,0x08,, +9.4473115,0x00,, +9.447407,0xD5,, +9.447502375,0x42,, +9.447597875,0xAB,, +9.447693375,0x80,, +9.44778875,0x0D,, +9.44788425,0x54,, +9.44797975,0x80,, +9.44807525,0x08,, +9.44817075,0x00,, +9.448266125,0x2A,, +9.448361625,0x62,, +9.448457125,0xA6,, +9.4485525,0x00,, +9.448648,0x00,, +9.4487435,0x00,, +9.448839,0x00,, +9.448934375,0x00,, +9.449029875,0x00,, +9.449125375,0x00,, +9.449220875,0x00,, +9.44931625,0x00,, +9.44941175,0x00,, +9.44950725,0x00,, +9.449602625,0x00,, +9.449698125,0x00,, +9.449793625,0x00,, +9.449889125,0x00,, +9.449984625,0x00,, +9.45008,0x00,, +9.4501755,0x00,, +9.450271,0x00,, +9.450366375,0xDE,, +9.465771375,0x55,, +9.465866875,0x55,, +9.465962375,0x18,, +9.46605775,0x00,, +9.46615325,0x45,, +9.46624875,0x37,, +9.46634425,0xD8,, +9.466439625,0x00,, +9.466535125,0x2C,, +9.466630625,0x48,, +9.466726,0x00,, +9.4668215,0x80,, +9.466917,0x08,, +9.4670125,0x00,, +9.467107875,0xD5,, +9.467203375,0x42,, +9.467298875,0xAB,, +9.467394375,0x80,, +9.467489875,0x0D,, +9.46758525,0x54,, +9.46768075,0x80,, +9.46777625,0x08,, +9.467871625,0x00,, +9.467967125,0x2A,, +9.468062625,0x62,, +9.468158125,0xA6,, +9.4682535,0x77,, +9.485871625,0x55,, +9.485967125,0x55,, +9.4860625,0x18,, +9.486158,0x00,, +9.4862535,0x45,, +9.486349,0xFD,, +9.486444375,0xDB,, +9.486539875,0x00,, +9.486635375,0x2C,, +9.48673075,0x58,, +9.48682625,0x00,, +9.48692175,0x80,, +9.48701725,0x08,, +9.48711275,0x00,, +9.487208125,0xD5,, +9.487303625,0x42,, +9.487399125,0xAB,, +9.4874945,0x80,, +9.48759,0x0D,, +9.4876855,0x54,, +9.487781,0x80,, +9.487876375,0x08,, +9.487971875,0x00,, +9.488067375,0x2A,, +9.488162875,0x62,, +9.48825825,0xA6,, +9.48835375,0xE0,, +9.505963125,0x55,, +9.506058625,0x55,, +9.506154125,0x18,, +9.5062495,0x00,, +9.506345,0x46,, +9.5064405,0xC3,, +9.506536,0xDA,, +9.506631375,0x00,, +9.506726875,0x2C,, +9.506822375,0x68,, +9.506917875,0x00,, +9.50701325,0x80,, +9.50710875,0x08,, +9.50720425,0x00,, +9.50729975,0xD5,, +9.507395125,0x42,, +9.507490625,0xAB,, +9.507586125,0x80,, +9.507681625,0x0D,, +9.507777,0x54,, +9.5078725,0x80,, +9.507968,0x08,, +9.508063375,0x00,, +9.508158875,0x2A,, +9.508254375,0x62,, +9.508349875,0xA6,, +9.50844525,0x15,, +9.526063375,0x55,, +9.526158875,0x55,, +9.52625425,0x18,, +9.52634975,0x00,, +9.52644525,0x47,, +9.52654075,0x89,, +9.526636125,0xDB,, +9.526731625,0x00,, +9.526827125,0x2C,, +9.526922625,0x38,, +9.527018,0x00,, +9.5271135,0x80,, +9.527209,0x08,, +9.5273045,0x00,, +9.527399875,0xD5,, +9.527495375,0x42,, +9.527590875,0xAB,, +9.52768625,0x80,, +9.52778175,0x0D,, +9.52787725,0x54,, +9.52797275,0x80,, +9.528068125,0x08,, +9.528163625,0x00,, +9.528259125,0x2A,, +9.528354625,0x62,, +9.528450125,0xA6,, +9.5285455,0x28,, +9.546163625,0x55,, +9.546259125,0x55,, +9.5463545,0x18,, +9.54645,0x00,, +9.5465455,0x48,, +9.546640875,0x4F,, +9.546736375,0xDC,, +9.546831875,0x00,, +9.546927375,0x2C,, +9.54702275,0x38,, +9.54711825,0x00,, +9.54721375,0x80,, +9.54730925,0x08,, +9.547404625,0x00,, +9.547500125,0xD5,, +9.547595625,0x42,, +9.547691,0xAB,, +9.5477865,0x80,, +9.547882,0x0D,, +9.5479775,0x54,, +9.548073,0x80,, +9.548168375,0x08,, +9.548263875,0x00,, +9.548359375,0x2A,, +9.54845475,0x62,, +9.54855025,0xA6,, +9.54864575,0xE3,, +9.566255125,0x55,, +9.566350625,0x55,, +9.566446125,0x18,, +9.5665415,0x00,, +9.566637,0x49,, +9.5667325,0x15,, +9.566828,0xD7,, +9.566923375,0x00,, +9.567018875,0x2C,, +9.567114375,0x58,, +9.56720975,0x00,, +9.56730525,0x80,, +9.56740075,0x08,, +9.56749625,0x00,, +9.567591625,0xD5,, +9.567687125,0x42,, +9.567782625,0xAB,, +9.567878125,0x80,, +9.5679735,0x0D,, +9.568069,0x54,, +9.5681645,0x80,, +9.56826,0x08,, +9.568355375,0x00,, +9.568450875,0x2A,, +9.568546375,0x62,, +9.568641875,0xA6,, +9.56873725,0xA2,, +9.586364,0x55,, +9.5864595,0x55,, +9.586555,0x18,, +9.5866505,0x00,, +9.586745875,0x49,, +9.586841375,0xDB,, +9.586936875,0xD8,, +9.58703225,0x00,, +9.58712775,0x2C,, +9.58722325,0x48,, +9.58731875,0x00,, +9.587414125,0x80,, +9.587509625,0x08,, +9.587605125,0x00,, +9.587700625,0xD5,, +9.587796,0x42,, +9.5878915,0xAB,, +9.587987,0x80,, +9.588082375,0x0D,, +9.588177875,0x54,, +9.588273375,0x80,, +9.588368875,0x08,, +9.588464375,0x00,, +9.58855975,0x2A,, +9.58865525,0x62,, +9.58875075,0xA6,, +9.58884625,0xFB,, +9.606455625,0x55,, +9.606551,0x55,, +9.6066465,0x18,, +9.606742,0x00,, +9.606837375,0x4A,, +9.606932875,0xA1,, +9.607028375,0xD8,, +9.607123875,0x00,, +9.607219375,0x2C,, +9.60731475,0x68,, +9.60741025,0x00,, +9.60750575,0x80,, +9.607601125,0x08,, +9.607696625,0x00,, +9.607792125,0xD5,, +9.607887625,0x42,, +9.607983,0xAB,, +9.6080785,0x80,, +9.608174,0x0D,, +9.6082695,0x54,, +9.608364875,0x80,, +9.608460375,0x08,, +9.608555875,0x00,, +9.60865125,0x2A,, +9.60874675,0x62,, +9.60884225,0xA6,, +9.60893775,0x4C,, +9.62655575,0x55,, +9.62665125,0x55,, +9.62674675,0x18,, +9.62684225,0x00,, +9.626937625,0x4B,, +9.627033125,0x68,, +9.627128625,0xDA,, +9.627224,0x00,, +9.6273195,0x2C,, +9.627415,0x38,, +9.6275105,0x00,, +9.627605875,0x80,, +9.627701375,0x08,, +9.627796875,0x00,, +9.627892375,0xD5,, +9.62798775,0x42,, +9.62808325,0xAB,, +9.62817875,0x80,, +9.62827425,0x0D,, +9.628369625,0x54,, +9.628465125,0x80,, +9.628560625,0x08,, +9.628656125,0x00,, +9.6287515,0x2A,, +9.628847,0x62,, +9.6289425,0xA6,, +9.629038,0x67,, +9.64705525,0x55,, +9.64715075,0x55,, +9.647246125,0x2B,, +9.647341625,0x03,, +9.647437125,0x4C,, +9.647532625,0x2E,, +9.647628,0xD7,, +9.6477235,0x00,, +9.647819,0x2C,, +9.6479145,0x68,, +9.648009875,0x00,, +9.648105375,0x80,, +9.648200875,0x08,, +9.648296375,0x00,, +9.64839175,0xD5,, +9.64848725,0x42,, +9.64858275,0xAB,, +9.648678125,0x80,, +9.648773625,0x0D,, +9.648869125,0x54,, +9.648964625,0x80,, +9.64906,0x08,, +9.6491555,0x00,, +9.649251,0x2A,, +9.6493465,0x62,, +9.649441875,0xA6,, +9.649537375,0x00,, +9.649632875,0x00,, +9.64972825,0x00,, +9.64982375,0x00,, +9.64991925,0x00,, +9.65001475,0x00,, +9.65011025,0x00,, +9.650205625,0x00,, +9.650301125,0x00,, +9.650396625,0x00,, +9.650492125,0x00,, +9.6505875,0x00,, +9.650683,0x00,, +9.6507785,0x00,, +9.650873875,0x00,, +9.650969375,0x00,, +9.651064875,0x00,, +9.651160375,0x00,, +9.65125575,0x00,, +9.65135125,0x3C,, +9.6667475,0x55,, +9.666843,0x55,, +9.6669385,0x18,, +9.667034,0x00,, +9.667129375,0x4C,, +9.667224875,0xF4,, +9.667320375,0xD8,, +9.667415875,0x00,, +9.66751125,0x2C,, +9.66760675,0x68,, +9.66770225,0x00,, +9.667797625,0x80,, +9.667893125,0x08,, +9.667988625,0x00,, +9.668084125,0xD5,, +9.668179625,0x42,, +9.668275,0xAB,, +9.6683705,0x80,, +9.668466,0x0D,, +9.668561375,0x54,, +9.668656875,0x80,, +9.668752375,0x08,, +9.668847875,0x00,, +9.66894325,0x2A,, +9.66903875,0x62,, +9.66913425,0xA6,, +9.66922975,0x89,, +9.68684775,0x55,, +9.68694325,0x55,, +9.68703875,0x18,, +9.687134125,0x00,, +9.687229625,0x4D,, +9.687325125,0xBA,, +9.6874205,0xDC,, +9.687516,0x00,, +9.6876115,0x2C,, +9.687707,0x58,, +9.6878025,0x00,, +9.687897875,0x80,, +9.687993375,0x08,, +9.688088875,0x00,, +9.688184375,0xD5,, +9.68827975,0x42,, +9.68837525,0xAB,, +9.68847075,0x80,, +9.688566125,0x0D,, +9.688661625,0x54,, +9.688757125,0x80,, +9.688852625,0x08,, +9.688948,0x00,, +9.6890435,0x2A,, +9.689139,0x62,, +9.6892345,0xA6,, +9.689329875,0x7F,, +9.706948,0x55,, +9.7070435,0x55,, +9.707138875,0x18,, +9.707234375,0x00,, +9.707329875,0x4E,, +9.707425375,0x81,, +9.70752075,0xDB,, +9.70761625,0x00,, +9.70771175,0x2C,, +9.70780725,0x58,, +9.707902625,0x00,, +9.707998125,0x80,, +9.708093625,0x08,, +9.708189,0x00,, +9.7082845,0xD5,, +9.70838,0x42,, +9.7084755,0xAB,, +9.708570875,0x80,, +9.708666375,0x0D,, +9.708761875,0x54,, +9.708857375,0x80,, +9.70895275,0x08,, +9.70904825,0x00,, +9.70914375,0x2A,, +9.70923925,0x62,, +9.709334625,0xA6,, +9.709430125,0x98,, +9.7270395,0x55,, +9.727135,0x55,, +9.7272305,0x18,, +9.727326,0x00,, +9.727421375,0x4F,, +9.727516875,0x47,, +9.727612375,0xDC,, +9.72770775,0x00,, +9.72780325,0x2C,, +9.72789875,0x68,, +9.72799425,0x00,, +9.728089625,0x80,, +9.728185125,0x08,, +9.728280625,0x00,, +9.728376125,0xD5,, +9.7284715,0x42,, +9.728567,0xAB,, +9.7286625,0x80,, +9.728757875,0x0D,, +9.728853375,0x54,, +9.728948875,0x80,, +9.729044375,0x08,, +9.729139875,0x00,, +9.72923525,0x2A,, +9.72933075,0x62,, +9.72942625,0xA6,, +9.729521625,0x01,, +9.74713975,0x55,, +9.74723525,0x55,, +9.747330625,0x18,, +9.747426125,0x00,, +9.747521625,0x50,, +9.747617125,0x0E,, +9.7477125,0xDB,, +9.747808,0x00,, +9.7479035,0x2C,, +9.747999,0x38,, +9.748094375,0x00,, +9.748189875,0x80,, +9.748285375,0x08,, +9.74838075,0x00,, +9.74847625,0xD5,, +9.74857175,0x42,, +9.74866725,0xAB,, +9.74876275,0x80,, +9.748858125,0x0D,, +9.748953625,0x54,, +9.749049125,0x80,, +9.749144625,0x08,, +9.74924,0x00,, +9.7493355,0x2A,, +9.749431,0x62,, +9.749526375,0xA6,, +9.749621875,0xD1,, +9.76724,0x55,, +9.767335375,0x55,, +9.767430875,0x18,, +9.767526375,0x00,, +9.767621875,0x50,, +9.76771725,0xD4,, +9.76781275,0xDC,, +9.76790825,0x00,, +9.76800375,0x2C,, +9.768099125,0x58,, +9.768194625,0x00,, +9.768290125,0x80,, +9.768385625,0x08,, +9.768481,0x00,, +9.7685765,0xD5,, +9.768672,0x42,, +9.7687675,0xAB,, +9.768862875,0x80,, +9.768958375,0x0D,, +9.769053875,0x54,, +9.76914925,0x80,, +9.76924475,0x08,, +9.76934025,0x00,, +9.76943575,0x2A,, +9.769531125,0x62,, +9.769626625,0xA6,, +9.769722125,0x3C,, +9.7873315,0x55,, +9.787427,0x55,, +9.7875225,0x18,, +9.787617875,0x00,, +9.787713375,0x51,, +9.787808875,0x9B,, +9.78790425,0xDB,, +9.78799975,0x00,, +9.78809525,0x2C,, +9.78819075,0x48,, +9.78828625,0x00,, +9.788381625,0x80,, +9.788477125,0x08,, +9.788572625,0x00,, +9.788668,0xD5,, +9.7887635,0x42,, +9.788859,0xAB,, +9.7889545,0x80,, +9.789049875,0x0D,, +9.789145375,0x54,, +9.789240875,0x80,, +9.789336375,0x08,, +9.78943175,0x00,, +9.78952725,0x2A,, +9.78962275,0x62,, +9.789718125,0xA6,, +9.789813625,0xBD,, +9.807423,0x55,, +9.8075185,0x55,, +9.807614,0x18,, +9.8077095,0x00,, +9.807804875,0x52,, +9.807900375,0x62,, +9.807995875,0xD8,, +9.80809125,0x00,, +9.80818675,0x2C,, +9.80828225,0x58,, +9.80837775,0x00,, +9.808473125,0x80,, +9.808568625,0x08,, +9.808664125,0x00,, +9.808759625,0xD5,, +9.808855125,0x42,, +9.8089505,0xAB,, +9.809046,0x80,, +9.8091415,0x0D,, +9.809236875,0x54,, +9.809332375,0x80,, +9.809427875,0x08,, +9.809523375,0x00,, +9.80961875,0x2A,, +9.80971425,0x62,, +9.80980975,0xA6,, +9.80990525,0x3B,, +9.82752325,0x55,, +9.82761875,0x55,, +9.82771425,0x18,, +9.827809625,0x00,, +9.827905125,0x53,, +9.828000625,0x29,, +9.828096125,0xDA,, +9.8281915,0x00,, +9.828287,0x2C,, +9.8283825,0x58,, +9.828478,0x00,, +9.828573375,0x80,, +9.828668875,0x08,, +9.828764375,0x00,, +9.82885975,0xD5,, +9.82895525,0x42,, +9.82905075,0xAB,, +9.82914625,0x80,, +9.829241625,0x0D,, +9.829337125,0x54,, +9.829432625,0x80,, +9.829528125,0x08,, +9.829623625,0x00,, +9.829719,0x2A,, +9.8298145,0x62,, +9.82991,0xA6,, +9.830005375,0xDF,, +9.848031375,0x55,, +9.848126875,0x55,, +9.848222375,0x2B,, +9.84831775,0x03,, +9.84841325,0x53,, +9.84850875,0xEE,, +9.848604125,0xDA,, +9.848699625,0x00,, +9.848795125,0x2C,, +9.848890625,0x78,, +9.848986125,0x00,, +9.8490815,0x80,, +9.849177,0x08,, +9.8492725,0x00,, +9.849368,0xD5,, +9.849463375,0x42,, +9.849558875,0xAB,, +9.849654375,0x80,, +9.84974975,0x0D,, +9.84984525,0x54,, +9.84994075,0x80,, +9.85003625,0x08,, +9.850131625,0x00,, +9.850227125,0x2A,, +9.850322625,0x62,, +9.850418125,0xA6,, +9.8505135,0x00,, +9.850609,0x00,, +9.8507045,0x00,, +9.8508,0x00,, +9.850895375,0x00,, +9.850990875,0x00,, +9.851086375,0x00,, +9.851181875,0x00,, +9.85127725,0x00,, +9.85137275,0x00,, +9.85146825,0x00,, +9.851563625,0x00,, +9.851659125,0x00,, +9.851754625,0x00,, +9.851850125,0x00,, +9.8519455,0x00,, +9.852041,0x00,, +9.8521365,0x00,, +9.852232,0x00,, +9.852327375,0x9B,, +9.86772375,0x55,, +9.867819125,0x55,, +9.867914625,0x18,, +9.868010125,0x00,, +9.868105625,0x54,, +9.868201,0xB4,, +9.8682965,0xDC,, +9.868392,0x00,, +9.868487375,0x2C,, +9.868582875,0x58,, +9.868678375,0x00,, +9.868773875,0x80,, +9.868869375,0x08,, +9.86896475,0x00,, +9.86906025,0xD5,, +9.86915575,0x42,, +9.86925125,0xAB,, +9.869346625,0x80,, +9.869442125,0x0D,, +9.869537625,0x54,, +9.869633,0x80,, +9.8697285,0x08,, +9.869824,0x00,, +9.8699195,0x2A,, +9.870014875,0x62,, +9.870110375,0xA6,, +9.870205875,0x2C,, +9.887823875,0x55,, +9.887919375,0x55,, +9.888014875,0x18,, +9.888110375,0x00,, +9.88820575,0x55,, +9.88830125,0x7A,, +9.88839675,0xDB,, +9.88849225,0x00,, +9.888587625,0x2C,, +9.888683125,0x68,, +9.888778625,0x00,, +9.888874125,0x80,, +9.8889695,0x08,, +9.889065,0x00,, +9.8891605,0xD5,, +9.889255875,0x42,, +9.889351375,0xAB,, +9.889446875,0x80,, +9.889542375,0x0D,, +9.88963775,0x54,, +9.88973325,0x80,, +9.88982875,0x08,, +9.88992425,0x00,, +9.890019625,0x2A,, +9.890115125,0x62,, +9.890210625,0xA6,, +9.890306125,0x87,, +9.907924125,0x55,, +9.908019625,0x55,, +9.908115125,0x18,, +9.9082105,0x00,, +9.908306,0x56,, +9.9084015,0x40,, +9.908497,0xD8,, +9.908592375,0x00,, +9.908687875,0x2C,, +9.908783375,0x68,, +9.908878875,0x00,, +9.90897425,0x80,, +9.90906975,0x08,, +9.90916525,0x00,, +9.909260625,0xD5,, +9.909356125,0x42,, +9.909451625,0xAB,, +9.909547125,0x80,, +9.909642625,0x0D,, +9.909738,0x54,, +9.9098335,0x80,, +9.909929,0x08,, +9.910024375,0x00,, +9.910119875,0x2A,, +9.910215375,0x62,, +9.910310875,0xA6,, +9.91040625,0x14,, +9.92801575,0x55,, +9.928111125,0x55,, +9.928206625,0x18,, +9.928302125,0x00,, +9.9283975,0x57,, +9.928493,0x07,, +9.9285885,0xD8,, +9.928684,0x00,, +9.928779375,0x2C,, +9.928874875,0x58,, +9.928970375,0x00,, +9.929065875,0x80,, +9.92916125,0x08,, +9.92925675,0x00,, +9.92935225,0xD5,, +9.929447625,0x42,, +9.929543125,0xAB,, +9.929638625,0x80,, +9.929734125,0x0D,, +9.929829625,0x54,, +9.929925,0x80,, +9.9300205,0x08,, +9.930116,0x00,, +9.9302115,0x2A,, +9.930306875,0x62,, +9.930402375,0xA6,, +9.930497875,0xA4,, +9.948115875,0x55,, +9.948211375,0x55,, +9.948306875,0x18,, +9.94840225,0x00,, +9.94849775,0x57,, +9.94859325,0xCE,, +9.94868875,0xDB,, +9.948784125,0x00,, +9.948879625,0x2C,, +9.948975125,0x58,, +9.949070625,0x00,, +9.949166,0x80,, +9.9492615,0x08,, +9.949357,0x00,, +9.9494525,0xD5,, +9.949547875,0x42,, +9.949643375,0xAB,, +9.949738875,0x80,, +9.949834375,0x0D,, +9.94992975,0x54,, +9.95002525,0x80,, +9.95012075,0x08,, +9.950216125,0x00,, +9.950311625,0x2A,, +9.950407125,0x62,, +9.950502625,0xA6,, +9.950598,0x98,, +9.968216125,0x55,, +9.968311625,0x55,, +9.968407,0x18,, +9.9685025,0x00,, +9.968598,0x58,, +9.9686935,0x94,, +9.968789,0xDB,, +9.968884375,0x00,, +9.968979875,0x2C,, +9.969075375,0x68,, +9.96917075,0x00,, +9.96926625,0x80,, +9.96936175,0x08,, +9.96945725,0x00,, +9.969552625,0xD5,, +9.969648125,0x42,, +9.969743625,0xAB,, +9.969839125,0x80,, +9.9699345,0x0D,, +9.97003,0x54,, +9.9701255,0x80,, +9.970220875,0x08,, +9.970316375,0x00,, +9.970411875,0x2A,, +9.970507375,0x62,, +9.970602875,0xA6,, +9.97069825,0xD6,, +9.988316375,0x55,, +9.988411875,0x55,, +9.98850725,0x18,, +9.98860275,0x00,, +9.98869825,0x59,, +9.988793625,0x5B,, +9.988889125,0xD9,, +9.988984625,0x00,, +9.989080125,0x2C,, +9.9891755,0x68,, +9.989271,0x00,, +9.9893665,0x80,, +9.989462,0x08,, +9.989557375,0x00,, +9.989652875,0xD5,, +9.989748375,0x42,, +9.98984375,0xAB,, +9.98993925,0x80,, +9.99003475,0x0D,, +9.99013025,0x54,, +9.99022575,0x80,, +9.990321125,0x08,, +9.990416625,0x00,, +9.990512125,0x2A,, +9.990607625,0x62,, +9.990703,0xA6,, +9.9907985,0x02,, +10.008407875,0x55,, +10.008503375,0x55,, +10.008598875,0x18,, +10.00869425,0x00,, +10.00878975,0x5A,, +10.00888525,0x21,, +10.00898075,0xDC,, +10.009076125,0x00,, +10.009171625,0x2C,, +10.009267125,0x68,, +10.0093625,0x00,, +10.009458,0x80,, +10.0095535,0x08,, +10.009649,0x00,, +10.009744375,0xD5,, +10.009839875,0x42,, +10.009935375,0xAB,, +10.010030875,0x80,, +10.01012625,0x0D,, +10.01022175,0x54,, +10.01031725,0x80,, +10.01041275,0x08,, +10.010508125,0x00,, +10.010603625,0x2A,, +10.010699125,0x62,, +10.010794625,0xA6,, +10.01089,0xEE,, +10.02851675,0x55,, +10.02861225,0x55,, +10.02870775,0x18,, +10.02880325,0x00,, +10.028898625,0x5A,, +10.028994125,0xE8,, +10.029089625,0xDC,, +10.029185,0x00,, +10.0292805,0x2C,, +10.029376,0x58,, +10.0294715,0x00,, +10.029566875,0x80,, +10.029662375,0x08,, +10.029757875,0x00,, +10.029853375,0xD5,, +10.02994875,0x42,, +10.03004425,0xAB,, +10.03013975,0x80,, +10.03023525,0x0D,, +10.030330625,0x54,, +10.030426125,0x80,, +10.030521625,0x08,, +10.030617,0x00,, +10.0307125,0x2A,, +10.030808,0x62,, +10.0309035,0xA6,, +10.030999,0x81,, +10.04901625,0x55,, +10.04911175,0x55,, +10.049207125,0x2B,, +10.049302625,0x03,, +10.049398125,0x5B,, +10.049493625,0xAE,, +10.049589,0xD9,, +10.0496845,0x00,, +10.04978,0x2C,, +10.049875375,0x58,, +10.049970875,0x00,, +10.050066375,0x80,, +10.050161875,0x08,, +10.050257375,0x00,, +10.05035275,0xD5,, +10.05044825,0x42,, +10.05054375,0xAB,, +10.050639125,0x80,, +10.050734625,0x0D,, +10.050830125,0x54,, +10.050925625,0x80,, +10.051021,0x08,, +10.0511165,0x00,, +10.051212,0x2A,, +10.0513075,0x62,, +10.051402875,0xA6,, +10.051498375,0x00,, +10.051593875,0x00,, +10.05168925,0x00,, +10.05178475,0x00,, +10.05188025,0x00,, +10.05197575,0x00,, +10.052071125,0x00,, +10.052166625,0x00,, +10.052262125,0x00,, +10.052357625,0x00,, +10.052453125,0x00,, +10.0525485,0x00,, +10.052644,0x00,, +10.0527395,0x00,, +10.052834875,0x00,, +10.052930375,0x00,, +10.053025875,0x00,, +10.053121375,0x00,, +10.05321675,0x00,, +10.05331225,0x41,, +10.0687085,0x55,, +10.068804,0x55,, +10.0688995,0x18,, +10.068995,0x00,, +10.069090375,0x5C,, +10.069185875,0x74,, +10.069281375,0xDD,, +10.069376875,0x00,, +10.06947225,0x2C,, +10.06956775,0x58,, +10.06966325,0x00,, +10.069758625,0x80,, +10.069854125,0x08,, +10.069949625,0x00,, +10.070045125,0xD5,, +10.0701405,0x42,, +10.070236,0xAB,, +10.0703315,0x80,, +10.070427,0x0D,, +10.070522375,0x54,, +10.070617875,0x80,, +10.070713375,0x08,, +10.070808875,0x00,, +10.07090425,0x2A,, +10.07099975,0x62,, +10.07109525,0xA6,, +10.07119075,0x20,, +10.08880875,0x55,, +10.08890425,0x55,, +10.08899975,0x18,, +10.089095125,0x00,, +10.089190625,0x5D,, +10.089286125,0x3A,, +10.0893815,0xDD,, +10.089477,0x01,, +10.0895725,0x2C,, +10.089668,0x58,, +10.089763375,0x00,, +10.089858875,0x80,, +10.089954375,0x08,, +10.090049875,0x00,, +10.090145375,0xD5,, +10.09024075,0x42,, +10.09033625,0xAB,, +10.09043175,0x80,, +10.090527125,0x0D,, +10.090622625,0x54,, +10.090718125,0x80,, +10.090813625,0x08,, +10.090909,0x00,, +10.0910045,0x2A,, +10.0911,0x62,, +10.0911955,0xA6,, +10.091290875,0x97,, +10.108909,0x55,, +10.1090045,0x55,, +10.109099875,0x18,, +10.109195375,0x00,, +10.109290875,0x5E,, +10.109386375,0x00,, +10.10948175,0xDB,, +10.10957725,0x00,, +10.10967275,0x2C,, +10.10976825,0x68,, +10.109863625,0x00,, +10.109959125,0x80,, +10.110054625,0x08,, +10.11015,0x00,, +10.1102455,0xD5,, +10.110341,0x42,, +10.1104365,0xAB,, +10.110531875,0x80,, +10.110627375,0x0D,, +10.110722875,0x54,, +10.110818375,0x80,, +10.11091375,0x08,, +10.11100925,0x00,, +10.11110475,0x2A,, +10.111200125,0x62,, +10.111295625,0xA6,, +10.111391125,0x69,, +10.1290005,0x55,, +10.129096,0x55,, +10.1291915,0x18,, +10.129286875,0x00,, +10.129382375,0x5E,, +10.129477875,0xC6,, +10.129573375,0xDD,, +10.12966875,0x00,, +10.12976425,0x2C,, +10.12985975,0x48,, +10.12995525,0x00,, +10.130050625,0x80,, +10.130146125,0x08,, +10.130241625,0x00,, +10.130337125,0xD5,, +10.1304325,0x42,, +10.130528,0xAB,, +10.1306235,0x80,, +10.130718875,0x0D,, +10.130814375,0x54,, +10.130909875,0x80,, +10.131005375,0x08,, +10.13110075,0x00,, +10.13119625,0x2A,, +10.13129175,0x62,, +10.13138725,0xA6,, +10.131482625,0x6E,, +10.14910075,0x55,, +10.14919625,0x55,, +10.149291625,0x18,, +10.149387125,0x00,, +10.149482625,0x5F,, +10.149578125,0x8C,, +10.1496735,0xDA,, +10.149769,0x00,, +10.1498645,0x2C,, +10.14996,0x58,, +10.150055375,0x00,, +10.150150875,0x80,, +10.150246375,0x08,, +10.15034175,0x00,, +10.15043725,0xD5,, +10.15053275,0x42,, +10.15062825,0xAB,, +10.150723625,0x80,, +10.150819125,0x0D,, +10.150914625,0x54,, +10.151010125,0x80,, +10.151105625,0x08,, +10.151201,0x00,, +10.1512965,0x2A,, +10.151392,0x62,, +10.151487375,0xA6,, +10.151582875,0x32,, +10.169201,0x55,, +10.169296375,0x55,, +10.169391875,0x18,, +10.169487375,0x00,, +10.169582875,0x60,, +10.16967825,0x52,, +10.16977375,0xDC,, +10.16986925,0x00,, +10.16996475,0x2C,, +10.170060125,0x48,, +10.170155625,0x00,, +10.170251125,0x80,, +10.1703465,0x08,, +10.170442,0x00,, +10.1705375,0xD5,, +10.170633,0x42,, +10.1707285,0xAB,, +10.170823875,0x80,, +10.170919375,0x0D,, +10.171014875,0x54,, +10.17111025,0x80,, +10.17120575,0x08,, +10.17130125,0x00,, +10.17139675,0x2A,, +10.171492125,0x62,, +10.171587625,0xA6,, +10.171683125,0x2E,, +10.189301125,0x55,, +10.189396625,0x55,, +10.189492125,0x18,, +10.189587625,0x00,, +10.189683125,0x61,, +10.1897785,0x17,, +10.189874,0xDD,, +10.189969375,0x00,, +10.190064875,0x2C,, +10.190160375,0x58,, +10.190255875,0x00,, +10.190351375,0x80,, +10.19044675,0x08,, +10.19054225,0x00,, +10.19063775,0xD5,, +10.19073325,0x42,, +10.190828625,0xAB,, +10.190924125,0x80,, +10.191019625,0x0D,, +10.191115,0x54,, +10.1912105,0x80,, +10.191306,0x08,, +10.1914015,0x00,, +10.191496875,0x2A,, +10.191592375,0x62,, +10.191687875,0xA6,, +10.191783375,0xFA,, +10.20939275,0x55,, +10.209488125,0x55,, +10.209583625,0x18,, +10.209679125,0x00,, +10.209774625,0x61,, +10.20987,0xDD,, +10.2099655,0xDB,, +10.210061,0x00,, +10.2101565,0x2C,, +10.210251875,0x68,, +10.210347375,0x00,, +10.210442875,0x80,, +10.210538375,0x08,, +10.21063375,0x00,, +10.21072925,0xD5,, +10.21082475,0x42,, +10.21092025,0xAB,, +10.211015625,0x80,, +10.211111125,0x0D,, +10.211206625,0x54,, +10.211302125,0x80,, +10.2113975,0x08,, +10.211493,0x00,, +10.2115885,0x2A,, +10.211683875,0x62,, +10.211779375,0xA6,, +10.211874875,0x36,, +10.229492875,0x55,, +10.229588375,0x55,, +10.229683875,0x18,, +10.229779375,0x00,, +10.229874875,0x62,, +10.22997025,0xA3,, +10.23006575,0xDC,, +10.23016125,0x00,, +10.230256625,0x2C,, +10.230352125,0x58,, +10.230447625,0x00,, +10.230543125,0x80,, +10.2306385,0x08,, +10.230734,0x00,, +10.2308295,0xD5,, +10.230925,0x42,, +10.231020375,0xAB,, +10.231115875,0x80,, +10.231211375,0x0D,, +10.23130675,0x54,, +10.23140225,0x80,, +10.23149775,0x08,, +10.23159325,0x00,, +10.23168875,0x2A,, +10.231784125,0x62,, +10.231879625,0xA6,, +10.231975125,0xBC,, +10.249992375,0x55,, +10.250087875,0x55,, +10.250183375,0x2B,, +10.25027875,0x03,, +10.25037425,0x63,, +10.25046975,0x6A,, +10.250565125,0xDE,, +10.250660625,0x00,, +10.250756125,0x2C,, +10.250851625,0x48,, +10.250947,0x00,, +10.2510425,0x80,, +10.251138,0x08,, +10.2512335,0x00,, +10.251329,0xD5,, +10.251424375,0x42,, +10.251519875,0xAB,, +10.251615375,0x80,, +10.25171075,0x0D,, +10.25180625,0x54,, +10.25190175,0x80,, +10.25199725,0x08,, +10.252092625,0x00,, +10.252188125,0x2A,, +10.252283625,0x62,, +10.252379125,0xA6,, +10.2524745,0x00,, +10.25257,0x00,, +10.2526655,0x00,, +10.252760875,0x00,, +10.252856375,0x00,, +10.252951875,0x00,, +10.253047375,0x00,, +10.253142875,0x00,, +10.25323825,0x00,, +10.25333375,0x00,, +10.25342925,0x00,, +10.253524625,0x00,, +10.253620125,0x00,, +10.253715625,0x00,, +10.253811125,0x00,, +10.2539065,0x00,, +10.254002,0x00,, +10.2540975,0x00,, +10.254193,0x00,, +10.254288375,0x09,, +10.269693375,0x55,, +10.269788875,0x55,, +10.26988425,0x18,, +10.26997975,0x00,, +10.27007525,0x64,, +10.27017075,0x30,, +10.27026625,0xDE,, +10.270361625,0x00,, +10.270457125,0x2C,, +10.270552625,0x48,, +10.270648,0x00,, +10.2707435,0x80,, +10.270839,0x08,, +10.2709345,0x00,, +10.271029875,0xD5,, +10.271125375,0x42,, +10.271220875,0xAB,, +10.271316375,0x80,, +10.27141175,0x0D,, +10.27150725,0x54,, +10.27160275,0x80,, +10.271698125,0x08,, +10.271793625,0x00,, +10.271889125,0x2A,, +10.271984625,0x62,, +10.27208,0xA6,, +10.2721755,0xE9,, +10.289784875,0x55,, +10.289880375,0x55,, +10.289975875,0x18,, +10.290071375,0x00,, +10.29016675,0x64,, +10.29026225,0xF5,, +10.29035775,0xDE,, +10.290453125,0x00,, +10.290548625,0x2C,, +10.290644125,0x48,, +10.290739625,0x00,, +10.290835125,0x80,, +10.2909305,0x08,, +10.291026,0x00,, +10.2911215,0xD5,, +10.291216875,0x42,, +10.291312375,0xAB,, +10.291407875,0x80,, +10.291503375,0x0D,, +10.29159875,0x54,, +10.29169425,0x80,, +10.29178975,0x08,, +10.29188525,0x00,, +10.291980625,0x2A,, +10.292076125,0x62,, +10.292171625,0xA6,, +10.292267,0x8A,, +10.309885125,0x55,, +10.309980625,0x55,, +10.310076,0x18,, +10.3101715,0x00,, +10.310267,0x65,, +10.3103625,0xBB,, +10.310458,0xDC,, +10.310553375,0x00,, +10.310648875,0x2C,, +10.310744375,0x68,, +10.310839875,0x00,, +10.31093525,0x80,, +10.31103075,0x08,, +10.31112625,0x00,, +10.311221625,0xD5,, +10.311317125,0x42,, +10.311412625,0xAB,, +10.311508125,0x80,, +10.3116035,0x0D,, +10.311699,0x54,, +10.3117945,0x80,, +10.31189,0x08,, +10.311985375,0x00,, +10.312080875,0x2A,, +10.312176375,0x62,, +10.312271875,0xA6,, +10.31236725,0x74,, +10.329976625,0x55,, +10.330072125,0x55,, +10.330167625,0x18,, +10.330263125,0x00,, +10.3303585,0x66,, +10.330454,0x82,, +10.3305495,0xDA,, +10.330645,0x00,, +10.330740375,0x2C,, +10.330835875,0x68,, +10.330931375,0x00,, +10.331026875,0x80,, +10.33112225,0x08,, +10.33121775,0x00,, +10.33131325,0xD5,, +10.331408625,0x42,, +10.331504125,0xAB,, +10.331599625,0x80,, +10.331695125,0x0D,, +10.3317905,0x54,, +10.331886,0x80,, +10.3319815,0x08,, +10.332077,0x00,, +10.3321725,0x2A,, +10.332267875,0x62,, +10.332363375,0xA6,, +10.332458875,0x30,, +10.350076875,0x55,, +10.350172375,0x55,, +10.350267875,0x18,, +10.35036325,0x00,, +10.35045875,0x67,, +10.35055425,0x49,, +10.35064975,0xDD,, +10.350745125,0x00,, +10.350840625,0x2C,, +10.350936125,0x58,, +10.351031625,0x00,, +10.351127,0x80,, +10.3512225,0x08,, +10.351318,0x00,, +10.351413375,0xD5,, +10.351508875,0x42,, +10.351604375,0xAB,, +10.351699875,0x80,, +10.351795375,0x0D,, +10.35189075,0x54,, +10.35198625,0x80,, +10.35208175,0x08,, +10.352177125,0x00,, +10.352272625,0x2A,, +10.352368125,0x62,, +10.352463625,0xA6,, +10.352559,0x46,, +10.370177125,0x55,, +10.370272625,0x55,, +10.370368,0x18,, +10.3704635,0x00,, +10.370559,0x68,, +10.3706545,0x0F,, +10.370749875,0xDD,, +10.370845375,0x00,, +10.370940875,0x2C,, +10.37103625,0x58,, +10.37113175,0x00,, +10.37122725,0x80,, +10.37132275,0x08,, +10.37141825,0x00,, +10.371513625,0xD5,, +10.371609125,0x42,, +10.371704625,0xAB,, +10.371800125,0x80,, +10.3718955,0x0D,, +10.371991,0x54,, +10.3720865,0x80,, +10.372181875,0x08,, +10.372277375,0x00,, +10.372372875,0x2A,, +10.372468375,0x62,, +10.37256375,0xA6,, +10.37265925,0x60,, +10.390277375,0x55,, +10.390372875,0x55,, +10.39046825,0x18,, +10.39056375,0x00,, +10.39065925,0x68,, +10.390754625,0xD6,, +10.390850125,0xDE,, +10.390945625,0x00,, +10.391041125,0x2C,, +10.3911365,0x58,, +10.391232,0x00,, +10.3913275,0x80,, +10.391423,0x08,, +10.391518375,0x00,, +10.391613875,0xD5,, +10.391709375,0x42,, +10.39180475,0xAB,, +10.39190025,0x80,, +10.39199575,0x0D,, +10.39209125,0x54,, +10.392186625,0x80,, +10.392282125,0x08,, +10.392377625,0x00,, +10.392473125,0x2A,, +10.392568625,0x62,, +10.392664,0xA6,, +10.3927595,0x38,, +10.410377625,0x55,, +10.410473,0x55,, +10.4105685,0x18,, +10.410664,0x00,, +10.410759375,0x69,, +10.410854875,0x9D,, +10.410950375,0xDD,, +10.411045875,0x00,, +10.41114125,0x2C,, +10.41123675,0x58,, +10.41133225,0x00,, +10.41142775,0x80,, +10.411523125,0x08,, +10.411618625,0x00,, +10.411714125,0xD5,, +10.4118095,0x42,, +10.411905,0xAB,, +10.4120005,0x80,, +10.412096,0x0D,, +10.4121915,0x54,, +10.412286875,0x80,, +10.412382375,0x08,, +10.412477875,0x00,, +10.41257325,0x2A,, +10.41266875,0x62,, +10.41276425,0xA6,, +10.41285975,0xF0,, +10.43047775,0x55,, +10.43057325,0x55,, +10.43066875,0x18,, +10.430764125,0x00,, +10.430859625,0x6A,, +10.430955125,0x64,, +10.431050625,0xDA,, +10.431146,0x00,, +10.4312415,0x2C,, +10.431337,0x48,, +10.431432375,0x00,, +10.431527875,0x80,, +10.431623375,0x08,, +10.431718875,0x00,, +10.431814375,0xD5,, +10.43190975,0x42,, +10.43200525,0xAB,, +10.43210075,0x80,, +10.43219625,0x0D,, +10.432291625,0x54,, +10.432387125,0x80,, +10.432482625,0x08,, +10.432578,0x00,, +10.4326735,0x2A,, +10.432769,0x62,, +10.4328645,0xA6,, +10.432959875,0xC6,, +10.45097725,0x55,, +10.45107275,0x55,, +10.451168125,0x2B,, +10.451263625,0x03,, +10.451359125,0x6B,, +10.451454625,0x2A,, +10.45155,0xDB,, +10.4516455,0x00,, +10.451741,0x2C,, +10.451836375,0x48,, +10.451931875,0x00,, +10.452027375,0x80,, +10.452122875,0x08,, +10.45221825,0x00,, +10.45231375,0xD5,, +10.45240925,0x42,, +10.45250475,0xAB,, +10.452600125,0x80,, +10.452695625,0x0D,, +10.452791125,0x54,, +10.452886625,0x80,, +10.452982,0x08,, +10.4530775,0x00,, +10.453173,0x2A,, +10.4532685,0x62,, +10.453363875,0xA6,, +10.453459375,0x00,, +10.453554875,0x00,, +10.45365025,0x00,, +10.45374575,0x00,, +10.45384125,0x00,, +10.45393675,0x00,, +10.454032125,0x00,, +10.454127625,0x00,, +10.454223125,0x00,, +10.454318625,0x00,, +10.454414,0x00,, +10.4545095,0x00,, +10.454605,0x00,, +10.454700375,0x00,, +10.454795875,0x00,, +10.454891375,0x00,, +10.454986875,0x00,, +10.455082375,0x00,, +10.45517775,0x00,, +10.45527325,0xDB,, +10.4706695,0x55,, +10.470765,0x55,, +10.4708605,0x18,, +10.470955875,0x00,, +10.471051375,0x6B,, +10.471146875,0xF1,, +10.471242375,0xD9,, +10.471337875,0x00,, +10.47143325,0x2C,, +10.47152875,0x68,, +10.47162425,0x00,, +10.471719625,0x80,, +10.471815125,0x08,, +10.471910625,0x00,, +10.472006125,0xD5,, +10.4721015,0x42,, +10.472197,0xAB,, +10.4722925,0x80,, +10.472388,0x0D,, +10.472483375,0x54,, +10.472578875,0x80,, +10.472674375,0x08,, +10.47276975,0x00,, +10.47286525,0x2A,, +10.47296075,0x62,, +10.47305625,0xA6,, +10.47315175,0xB7,, +10.490778375,0x55,, +10.490873875,0x55,, +10.490969375,0x18,, +10.491064875,0x00,, +10.49116025,0x6C,, +10.49125575,0xB8,, +10.49135125,0xDC,, +10.49144675,0x00,, +10.491542125,0x2C,, +10.491637625,0x58,, +10.491733125,0x00,, +10.491828625,0x80,, +10.491924,0x08,, +10.4920195,0x00,, +10.492115,0xD5,, +10.4922105,0x42,, +10.492305875,0xAB,, +10.492401375,0x80,, +10.492496875,0x0D,, +10.49259225,0x54,, +10.49268775,0x80,, +10.49278325,0x08,, +10.49287875,0x00,, +10.492974125,0x2A,, +10.493069625,0x62,, +10.493165125,0xA6,, +10.493260625,0xD4,, +10.51087,0x55,, +10.5109655,0x55,, +10.511060875,0x18,, +10.511156375,0x00,, +10.511251875,0x6D,, +10.51134725,0x7E,, +10.51144275,0xDC,, +10.51153825,0x00,, +10.51163375,0x2C,, +10.51172925,0x48,, +10.511824625,0x00,, +10.511920125,0x80,, +10.512015625,0x08,, +10.512111,0x00,, +10.5122065,0xD5,, +10.512302,0x42,, +10.5123975,0xAB,, +10.512492875,0x80,, +10.512588375,0x0D,, +10.512683875,0x54,, +10.512779375,0x80,, +10.51287475,0x08,, +10.51297025,0x00,, +10.51306575,0x2A,, +10.513161125,0x62,, +10.513256625,0xA6,, +10.513352125,0x4E,, +10.5309615,0x55,, +10.531057,0x55,, +10.5311525,0x18,, +10.531247875,0x00,, +10.531343375,0x6E,, +10.531438875,0x44,, +10.531534375,0xDD,, +10.53162975,0x00,, +10.53172525,0x2C,, +10.53182075,0x68,, +10.531916125,0x00,, +10.532011625,0x80,, +10.532107125,0x08,, +10.532202625,0x00,, +10.532298125,0xD5,, +10.5323935,0x42,, +10.532489,0xAB,, +10.5325845,0x80,, +10.532679875,0x0D,, +10.532775375,0x54,, +10.532870875,0x80,, +10.532966375,0x08,, +10.53306175,0x00,, +10.53315725,0x2A,, +10.53325275,0x62,, +10.53334825,0xA6,, +10.533443625,0x42,, +10.55106175,0x55,, +10.55115725,0x55,, +10.551252625,0x18,, +10.551348125,0x00,, +10.551443625,0x6F,, +10.551539,0x0B,, +10.5516345,0xDD,, +10.55173,0x00,, +10.5518255,0x2C,, +10.551921,0x48,, +10.552016375,0x00,, +10.552111875,0x80,, +10.552207375,0x08,, +10.55230275,0x00,, +10.55239825,0xD5,, +10.55249375,0x42,, +10.55258925,0xAB,, +10.552684625,0x80,, +10.552780125,0x0D,, +10.552875625,0x54,, +10.552971125,0x80,, +10.5530665,0x08,, +10.553162,0x00,, +10.5532575,0x2A,, +10.553353,0x62,, +10.553448375,0xA6,, +10.553543875,0x20,, +10.571162,0x55,, +10.571257375,0x55,, +10.571352875,0x18,, +10.571448375,0x00,, +10.571543875,0x6F,, +10.57163925,0xD0,, +10.57173475,0xDB,, +10.57183025,0x00,, +10.57192575,0x2C,, +10.572021125,0x58,, +10.572116625,0x00,, +10.572212125,0x80,, +10.5723075,0x08,, +10.572403,0x00,, +10.5724985,0xD5,, +10.572594,0x42,, +10.572689375,0xAB,, +10.572784875,0x80,, +10.572880375,0x0D,, +10.572975875,0x54,, +10.57307125,0x80,, +10.57316675,0x08,, +10.57326225,0x00,, +10.57335775,0x2A,, +10.573453125,0x62,, +10.573548625,0xA6,, +10.573644125,0x8B,, +10.5912535,0x55,, +10.591349,0x55,, +10.5914445,0x18,, +10.591539875,0x00,, +10.591635375,0x70,, +10.591730875,0x95,, +10.59182625,0xDD,, +10.59192175,0x00,, +10.59201725,0x2C,, +10.59211275,0x58,, +10.592208125,0x00,, +10.592303625,0x80,, +10.592399125,0x08,, +10.592494625,0x00,, +10.59259,0xD5,, +10.5926855,0x42,, +10.592781,0xAB,, +10.592876375,0x80,, +10.592971875,0x0D,, +10.593067375,0x54,, +10.593162875,0x80,, +10.593258375,0x08,, +10.59335375,0x00,, +10.59344925,0x2A,, +10.59354475,0x62,, +10.593640125,0xA6,, +10.593735625,0x35,, +10.611362375,0x55,, +10.611457875,0x55,, +10.611553375,0x18,, +10.61164875,0x00,, +10.61174425,0x71,, +10.61183975,0x5B,, +10.61193525,0xDD,, +10.612030625,0x00,, +10.612126125,0x2C,, +10.612221625,0x48,, +10.612317125,0x00,, +10.6124125,0x80,, +10.612508,0x08,, +10.6126035,0x00,, +10.612698875,0xD5,, +10.612794375,0x42,, +10.612889875,0xAB,, +10.612985375,0x80,, +10.61308075,0x0D,, +10.61317625,0x54,, +10.61327175,0x80,, +10.61336725,0x08,, +10.61346275,0x00,, +10.613558125,0x2A,, +10.613653625,0x62,, +10.613749,0xA6,, +10.6138445,0x9D,, +10.631453875,0x55,, +10.631549375,0x55,, +10.631644875,0x18,, +10.631740375,0x00,, +10.63183575,0x72,, +10.63193125,0x21,, +10.63202675,0xDD,, +10.63212225,0x00,, +10.632217625,0x2C,, +10.632313125,0x58,, +10.632408625,0x00,, +10.632504125,0x80,, +10.6325995,0x08,, +10.632695,0x00,, +10.6327905,0xD5,, +10.632886,0x42,, +10.632981375,0xAB,, +10.633076875,0x80,, +10.633172375,0x0D,, +10.63326775,0x54,, +10.63336325,0x80,, +10.63345875,0x08,, +10.63355425,0x00,, +10.633649625,0x2A,, +10.633745125,0x62,, +10.633840625,0xA6,, +10.633936125,0x0D,, +10.65230925,0x55,, +10.65240475,0x55,, +10.652500125,0x2B,, +10.652595625,0x03,, +10.652691125,0x72,, +10.6527865,0xEB,, +10.652882,0xD9,, +10.6529775,0x00,, +10.653073,0x2C,, +10.653168375,0x48,, +10.653263875,0x00,, +10.653359375,0x80,, +10.653454875,0x08,, +10.65355025,0x00,, +10.65364575,0xD5,, +10.65374125,0x42,, +10.653836625,0xAB,, +10.653932125,0x80,, +10.654027625,0x0D,, +10.654123125,0x54,, +10.654218625,0x80,, +10.654314,0x08,, +10.6544095,0x00,, +10.654505,0x2A,, +10.654600375,0x62,, +10.654695875,0xA6,, +10.654791375,0x00,, +10.654886875,0x00,, +10.65498225,0x00,, +10.65507775,0x00,, +10.65517325,0x00,, +10.65526875,0x00,, +10.655364125,0x00,, +10.655459625,0x00,, +10.655555125,0x00,, +10.655650625,0x00,, +10.655746,0x00,, +10.6558415,0x00,, +10.655937,0x00,, +10.656032375,0x00,, +10.656127875,0x00,, +10.656223375,0x00,, +10.656318875,0x00,, +10.656414375,0x00,, +10.65650975,0x00,, +10.65660525,0x02,, +10.671645625,0x55,, +10.671741125,0x55,, +10.671836625,0x18,, +10.671932125,0x00,, +10.672027625,0x73,, +10.672123,0xAF,, +10.6722185,0xCC,, +10.672314,0x00,, +10.672409375,0x2C,, +10.672504875,0x48,, +10.672600375,0x00,, +10.672695875,0x80,, +10.67279125,0x08,, +10.67288675,0x00,, +10.67298225,0xD5,, +10.67307775,0x42,, +10.673173125,0xAB,, +10.673268625,0x80,, +10.673364125,0x0D,, +10.6734595,0x54,, +10.673555,0x80,, +10.6736505,0x08,, +10.673746,0x00,, +10.6738415,0x2A,, +10.673936875,0x62,, +10.674032375,0xA6,, +10.674127875,0xD0,, +10.692639875,0x55,, +10.69273525,0x55,, +10.69283075,0x18,, +10.69292625,0x00,, +10.69302175,0x74,, +10.693117125,0x7E,, +10.693212625,0xDD,, +10.693308125,0x00,, +10.6934035,0x2C,, +10.693499,0x68,, +10.6935945,0x00,, +10.69369,0x80,, +10.6937855,0x08,, +10.693880875,0x00,, +10.693976375,0xD5,, +10.694071875,0x42,, +10.694167375,0xAB,, +10.69426275,0x80,, +10.69435825,0x0D,, +10.69445375,0x54,, +10.694549125,0x80,, +10.694644625,0x08,, +10.694740125,0x00,, +10.694835625,0x2A,, +10.694931,0x62,, +10.6950265,0xA6,, +10.695122,0x52,, +10.711985,0x55,, +10.7120805,0x55,, +10.712176,0x18,, +10.712271375,0x00,, +10.712366875,0x75,, +10.712462375,0x3C,, +10.71255775,0xCD,, +10.71265325,0x00,, +10.71274875,0x2C,, +10.71284425,0x68,, +10.712939625,0x00,, +10.713035125,0x80,, +10.713130625,0x08,, +10.713226125,0x00,, +10.7133215,0xD5,, +10.713417,0x42,, +10.7135125,0xAB,, +10.713607875,0x80,, +10.713703375,0x0D,, +10.713798875,0x54,, +10.713894375,0x80,, +10.713989875,0x08,, +10.71408525,0x00,, +10.71418075,0x2A,, +10.71427625,0x62,, +10.714371625,0xA6,, +10.714467125,0xD6,, +10.7320765,0x55,, +10.732172,0x55,, +10.7322675,0x18,, +10.732363,0x00,, +10.732458375,0x76,, +10.732553875,0x03,, +10.732649375,0xCD,, +10.73274475,0x00,, +10.73284025,0x2C,, +10.73293575,0x68,, +10.73303125,0x00,, +10.733126625,0x80,, +10.733222125,0x08,, +10.733317625,0x00,, +10.733413125,0xD5,, +10.7335085,0x42,, +10.733604,0xAB,, +10.7336995,0x80,, +10.733795,0x0D,, +10.733890375,0x54,, +10.733985875,0x80,, +10.734081375,0x08,, +10.73417675,0x00,, +10.73427225,0x2A,, +10.73436775,0x62,, +10.73446325,0xA6,, +10.73455875,0xEC,, +10.75217675,0x55,, +10.75227225,0x55,, +10.75236775,0x18,, +10.752463125,0x00,, +10.752558625,0x76,, +10.752654125,0xCA,, +10.7527495,0xCC,, +10.752845,0x00,, +10.7529405,0x2C,, +10.753036,0x28,, +10.753131375,0x00,, +10.753226875,0x80,, +10.753322375,0x08,, +10.753417875,0x00,, +10.75351325,0xD5,, +10.75360875,0x42,, +10.75370425,0xAB,, +10.75379975,0x80,, +10.753895125,0x0D,, +10.753990625,0x54,, +10.754086125,0x80,, +10.754181625,0x08,, +10.754277,0x00,, +10.7543725,0x2A,, +10.754468,0x62,, +10.754563375,0xA6,, +10.754658875,0x01,, +10.77214675,0x55,, +10.77224225,0x55,, +10.77233775,0x18,, +10.772433125,0x00,, +10.772528625,0x77,, +10.772624125,0x90,, +10.772719625,0xC3,, +10.772815125,0x00,, +10.7729105,0x2C,, +10.773006,0x58,, +10.7731015,0x00,, +10.773196875,0x80,, +10.773292375,0x08,, +10.773387875,0x00,, +10.773483375,0xD5,, +10.77357875,0x42,, +10.77367425,0xAB,, +10.77376975,0x80,, +10.77386525,0x0D,, +10.773960625,0x54,, +10.774056125,0x80,, +10.774151625,0x08,, +10.774247125,0x00,, +10.7743425,0x2A,, +10.774438,0x62,, +10.7745335,0xA6,, +10.774628875,0x10,, +10.792238375,0x55,, +10.79233375,0x55,, +10.79242925,0x18,, +10.79252475,0x00,, +10.79262025,0x78,, +10.792715625,0x55,, +10.792811125,0xCC,, +10.792906625,0x00,, +10.793002,0x2C,, +10.7930975,0x58,, +10.793193,0x00,, +10.7932885,0x80,, +10.793384,0x08,, +10.793479375,0x00,, +10.793574875,0xD5,, +10.793670375,0x42,, +10.79376575,0xAB,, +10.79386125,0x80,, +10.79395675,0x0D,, +10.79405225,0x54,, +10.794147625,0x80,, +10.794243125,0x08,, +10.794338625,0x00,, +10.794434125,0x2A,, +10.7945295,0x62,, +10.794625,0xA6,, +10.7947205,0xF7,, +10.81246875,0x55,, +10.81256425,0x55,, +10.812659625,0x18,, +10.812755125,0x00,, +10.812850625,0x79,, +10.812946,0x1B,, +10.8130415,0xC4,, +10.813137,0x00,, +10.8132325,0x2C,, +10.813328,0x68,, +10.813423375,0x00,, +10.813518875,0x80,, +10.813614375,0x08,, +10.81370975,0x00,, +10.81380525,0xD5,, +10.81390075,0x42,, +10.81399625,0xAB,, +10.814091625,0x80,, +10.814187125,0x0D,, +10.814282625,0x54,, +10.814378125,0x80,, +10.8144735,0x08,, +10.814569,0x00,, +10.8146645,0x2A,, +10.814759875,0x62,, +10.814855375,0xA6,, +10.814950875,0xD6,, +10.833376125,0x55,, +10.833471625,0x55,, +10.833567,0x18,, +10.8336625,0x00,, +10.833758,0x79,, +10.833853375,0xEA,, +10.833948875,0xDC,, +10.834044375,0x00,, +10.834139875,0x2C,, +10.83423525,0x68,, +10.83433075,0x00,, +10.83442625,0x80,, +10.83452175,0x08,, +10.834617125,0x00,, +10.834712625,0xD5,, +10.834808125,0x42,, +10.8349035,0xAB,, +10.834999,0x80,, +10.8350945,0x0D,, +10.83519,0x54,, +10.835285375,0x80,, +10.835380875,0x08,, +10.835476375,0x00,, +10.835571875,0x2A,, +10.83566725,0x62,, +10.83576275,0xA6,, +10.83585825,0xA9,, +10.854075125,0x55,, +10.854170625,0x55,, +10.854266125,0x2B,, +10.8543615,0x03,, +10.854457,0x7A,, +10.8545525,0xB2,, +10.854648,0xD9,, +10.854743375,0x00,, +10.854838875,0x2C,, +10.854934375,0x58,, +10.855029875,0x00,, +10.85512525,0x80,, +10.85522075,0x08,, +10.85531625,0x00,, +10.855411625,0xD5,, +10.855507125,0x42,, +10.855602625,0xAB,, +10.855698125,0x80,, +10.8557935,0x0D,, +10.855889,0x54,, +10.8559845,0x80,, +10.85608,0x08,, +10.8561755,0x00,, +10.856270875,0x2A,, +10.856366375,0x62,, +10.856461875,0xA6,, +10.85655725,0x00,, +10.85665275,0x00,, +10.85674825,0x00,, +10.85684375,0x00,, +10.856939125,0x00,, +10.857034625,0x00,, +10.857130125,0x00,, +10.857225625,0x00,, +10.857321,0x00,, +10.8574165,0x00,, +10.857512,0x00,, +10.857607375,0x00,, +10.857702875,0x00,, +10.857798375,0x00,, +10.857893875,0x00,, +10.857989375,0x00,, +10.85808475,0x00,, +10.85818025,0x00,, +10.85827575,0x00,, +10.858371125,0x13,, +10.8726305,0x55,, +10.872726,0x55,, +10.8728215,0x18,, +10.872916875,0x00,, +10.873012375,0x7B,, +10.873107875,0x6E,, +10.873203375,0xCE,, +10.87329875,0x00,, +10.87339425,0x2C,, +10.87348975,0x58,, +10.87358525,0x00,, +10.873680625,0x80,, +10.873776125,0x08,, +10.873871625,0x00,, +10.873967125,0xD5,, +10.8740625,0x42,, +10.874158,0xAB,, +10.8742535,0x80,, +10.874349,0x0D,, +10.874444375,0x54,, +10.874539875,0x80,, +10.874635375,0x08,, +10.87473075,0x00,, +10.87482625,0x2A,, +10.87492175,0x62,, +10.87501725,0xA6,, +10.875112625,0x8C,, +10.89312125,0x55,, +10.89321675,0x55,, +10.89331225,0x18,, +10.89340775,0x00,, +10.893503125,0x7C,, +10.893598625,0x36,, +10.893694125,0xDB,, +10.8937895,0x00,, +10.893885,0x2C,, +10.8939805,0x58,, +10.894076,0x00,, +10.8941715,0x80,, +10.894266875,0x08,, +10.894362375,0x00,, +10.894457875,0xD5,, +10.89455325,0x42,, +10.89464875,0xAB,, +10.89474425,0x80,, +10.89483975,0x0D,, +10.894935125,0x54,, +10.895030625,0x80,, +10.895126125,0x08,, +10.895221625,0x00,, +10.895317,0x2A,, +10.8954125,0x62,, +10.895508,0xA6,, +10.8956035,0xA6,, +10.91282225,0x55,, +10.91291775,0x55,, +10.91301325,0x18,, +10.913108625,0x00,, +10.913204125,0x7C,, +10.913299625,0xFA,, +10.913395125,0xDC,, +10.913490625,0x00,, +10.913586,0x2C,, +10.9136815,0x68,, +10.913777,0x00,, +10.913872375,0x80,, +10.913967875,0x08,, +10.914063375,0x00,, +10.914158875,0xD5,, +10.91425425,0x42,, +10.91434975,0xAB,, +10.91444525,0x80,, +10.91454075,0x0D,, +10.914636125,0x54,, +10.914731625,0x80,, +10.914827125,0x08,, +10.9149225,0x00,, +10.915018,0x2A,, +10.9151135,0x62,, +10.915209,0xA6,, +10.9153045,0xD0,, +10.932931125,0x55,, +10.933026625,0x55,, +10.933122125,0x18,, +10.933217625,0x00,, +10.933313,0x7D,, +10.9334085,0xC0,, +10.933504,0xDB,, +10.9335995,0x00,, +10.933695,0x2C,, +10.933790375,0x68,, +10.933885875,0x00,, +10.933981375,0x80,, +10.93407675,0x08,, +10.93417225,0x00,, +10.93426775,0xD5,, +10.93436325,0x42,, +10.934458625,0xAB,, +10.934554125,0x80,, +10.934649625,0x0D,, +10.934745125,0x54,, +10.9348405,0x80,, +10.934936,0x08,, +10.9350315,0x00,, +10.935126875,0x2A,, +10.935222375,0x62,, +10.935317875,0xA6,, +10.935413375,0x57,, +10.953126875,0x55,, +10.953222375,0x55,, +10.953317875,0x18,, +10.95341325,0x00,, +10.95350875,0x7E,, +10.95360425,0x88,, +10.953699625,0xDB,, +10.953795125,0x00,, +10.953890625,0x2C,, +10.953986125,0x48,, +10.9540815,0x00,, +10.954177,0x80,, +10.9542725,0x08,, +10.954368,0x00,, +10.954463375,0xD5,, +10.954558875,0x42,, +10.954654375,0xAB,, +10.95474975,0x80,, +10.95484525,0x0D,, +10.95494075,0x54,, +10.95503625,0x80,, +10.95513175,0x08,, +10.955227125,0x00,, +10.955322625,0x2A,, +10.955418125,0x62,, +10.955513625,0xA6,, +10.955609,0xC3,, +10.97354825,0x55,, +10.973643625,0x55,, +10.973739125,0x18,, +10.973834625,0x00,, +10.973930125,0x7F,, +10.9740255,0x51,, +10.974121,0xDD,, +10.9742165,0x00,, +10.974312,0x2C,, +10.974407375,0x58,, +10.974502875,0x00,, +10.974598375,0x80,, +10.97469375,0x08,, +10.97478925,0x00,, +10.97488475,0xD5,, +10.97498025,0x42,, +10.97507575,0xAB,, +10.975171125,0x80,, +10.975266625,0x0D,, +10.975362125,0x54,, +10.9754575,0x80,, +10.975553,0x08,, +10.9756485,0x00,, +10.975744,0x2A,, +10.975839375,0x62,, +10.975934875,0xA6,, +10.976030375,0xB5,, +10.99339675,0x55,, +10.99349225,0x55,, +10.993587625,0x18,, +10.993683125,0x00,, +10.993778625,0x80,, +10.993874125,0x13,, +10.9939695,0xDD,, +10.994065,0x01,, +10.9941605,0x2C,, +10.994256,0x58,, +10.994351375,0x00,, +10.994446875,0x80,, +10.994542375,0x08,, +10.99463775,0x00,, +10.99473325,0xD5,, +10.99482875,0x42,, +10.99492425,0xAB,, +10.99501975,0x80,, +10.995115125,0x0D,, +10.995210625,0x54,, +10.995306125,0x80,, +10.9954015,0x08,, +10.995497,0x00,, +10.9955925,0x2A,, +10.995688,0x62,, +10.995783375,0xA6,, +10.995878875,0x22,, +11.013444875,0x55,, +11.013540375,0x55,, +11.01363575,0x18,, +11.01373125,0x00,, +11.01382675,0x80,, +11.01392225,0xD8,, +11.01401775,0xD3,, +11.014113125,0x00,, +11.014208625,0x2C,, +11.014304125,0x58,, +11.014399625,0x00,, +11.014495,0x80,, +11.0145905,0x08,, +11.014686,0x00,, +11.014781375,0xD5,, +11.014876875,0x42,, +11.014972375,0xAB,, +11.015067875,0x80,, +11.01516325,0x0D,, +11.01525875,0x54,, +11.01535425,0x80,, +11.01544975,0x08,, +11.015545125,0x00,, +11.015640625,0x2A,, +11.015736125,0x62,, +11.015831625,0xA6,, +11.015927,0xBC,, +11.033423625,0x55,, +11.033519,0x55,, +11.0336145,0x18,, +11.03371,0x00,, +11.0338055,0x81,, +11.033901,0x9E,, +11.033996375,0xDC,, +11.034091875,0x00,, +11.034187375,0x2C,, +11.034282875,0x58,, +11.03437825,0x00,, +11.03447375,0x80,, +11.03456925,0x08,, +11.034664625,0x00,, +11.034760125,0xD5,, +11.034855625,0x42,, +11.034951125,0xAB,, +11.0350465,0x80,, +11.035142,0x0D,, +11.0352375,0x54,, +11.035333,0x80,, +11.035428375,0x08,, +11.035523875,0x00,, +11.035619375,0x2A,, +11.035714875,0x62,, +11.03581025,0xA6,, +11.03590575,0x4C,, +11.053914375,0x55,, +11.054009875,0x55,, +11.054105375,0x2B,, +11.05420075,0x03,, +11.05429625,0x82,, +11.05439175,0x65,, +11.054487125,0xCD,, +11.054582625,0x00,, +11.054678125,0x2C,, +11.054773625,0x58,, +11.054869,0x00,, +11.0549645,0x80,, +11.05506,0x08,, +11.0551555,0x00,, +11.055250875,0xD5,, +11.055346375,0x42,, +11.055441875,0xAB,, +11.05553725,0x80,, +11.05563275,0x0D,, +11.05572825,0x54,, +11.05582375,0x80,, +11.055919125,0x08,, +11.056014625,0x00,, +11.056110125,0x2A,, +11.056205625,0x62,, +11.056301125,0xA6,, +11.0563965,0x00,, +11.056492,0x00,, +11.0565875,0x00,, +11.056682875,0x00,, +11.056778375,0x00,, +11.056873875,0x00,, +11.056969375,0x00,, +11.05706475,0x00,, +11.05716025,0x00,, +11.05725575,0x00,, +11.05735125,0x00,, +11.057446625,0x00,, +11.057542125,0x00,, +11.057637625,0x00,, +11.057733,0x00,, +11.0578285,0x00,, +11.057924,0x00,, +11.0580195,0x00,, +11.058115,0x00,, +11.058210375,0xAA,, +11.073615375,0x55,, +11.073710875,0x55,, +11.07380625,0x18,, +11.07390175,0x00,, +11.07399725,0x83,, +11.07409275,0x2C,, +11.074188125,0xD0,, +11.074283625,0x00,, +11.074379125,0x2C,, +11.074474625,0x68,, +11.07457,0x00,, +11.0746655,0x80,, +11.074761,0x08,, +11.074856375,0x00,, +11.074951875,0xD5,, +11.075047375,0x42,, +11.075142875,0xAB,, +11.07523825,0x80,, +11.07533375,0x0D,, +11.07542925,0x54,, +11.07552475,0x80,, +11.075620125,0x08,, +11.075715625,0x00,, +11.075811125,0x2A,, +11.075906625,0x62,, +11.076002,0xA6,, +11.0760975,0x12,, +11.093715625,0x55,, +11.093811,0x55,, +11.0939065,0x18,, +11.094002,0x00,, +11.0940975,0x83,, +11.094192875,0xF3,, +11.094288375,0xDB,, +11.094383875,0x00,, +11.094479375,0x2C,, +11.09457475,0x48,, +11.09467025,0x00,, +11.09476575,0x80,, +11.09486125,0x08,, +11.094956625,0x00,, +11.095052125,0xD5,, +11.095147625,0x42,, +11.095243125,0xAB,, +11.0953385,0x80,, +11.095434,0x0D,, +11.0955295,0x54,, +11.095624875,0x80,, +11.095720375,0x08,, +11.095815875,0x00,, +11.095911375,0x2A,, +11.09600675,0x62,, +11.09610225,0xA6,, +11.09619775,0x7C,, +11.114241125,0x55,, +11.1143365,0x55,, +11.114432,0x18,, +11.1145275,0x00,, +11.114622875,0x84,, +11.114718375,0xBD,, +11.114813875,0xC9,, +11.114909375,0x00,, +11.11500475,0x2C,, +11.11510025,0x58,, +11.11519575,0x00,, +11.11529125,0x80,, +11.11538675,0x08,, +11.115482125,0x00,, +11.115577625,0xD5,, +11.115673125,0x42,, +11.1157685,0xAB,, +11.115864,0x80,, +11.1159595,0x0D,, +11.116055,0x54,, +11.116150375,0x80,, +11.116245875,0x08,, +11.116341375,0x00,, +11.116436875,0x2A,, +11.11653225,0x62,, +11.11662775,0xA6,, +11.11672325,0x80,, +11.133907375,0x55,, +11.13400275,0x55,, +11.13409825,0x18,, +11.13419375,0x00,, +11.13428925,0x85,, +11.13438475,0x7E,, +11.134480125,0xCA,, +11.134575625,0x00,, +11.134671125,0x2C,, +11.1347665,0x68,, +11.134862,0x00,, +11.1349575,0x80,, +11.135053,0x08,, +11.135148375,0x00,, +11.135243875,0xD5,, +11.135339375,0x42,, +11.135434875,0xAB,, +11.13553025,0x80,, +11.13562575,0x0D,, +11.13572125,0x54,, +11.135816625,0x80,, +11.135912125,0x08,, +11.136007625,0x00,, +11.136103125,0x2A,, +11.1361985,0x62,, +11.136294,0xA6,, +11.1363895,0x74,, +11.15413775,0x55,, +11.15423325,0x55,, +11.15432875,0x18,, +11.154424125,0x00,, +11.154519625,0x86,, +11.154615125,0x47,, +11.1547105,0xCA,, +11.154806,0x00,, +11.1549015,0x2C,, +11.154997,0x58,, +11.155092375,0x00,, +11.155187875,0x80,, +11.155283375,0x08,, +11.155378875,0x00,, +11.15547425,0xD5,, +11.15556975,0x42,, +11.15566525,0xAB,, +11.155760625,0x80,, +11.155856125,0x0D,, +11.155951625,0x54,, +11.156047125,0x80,, +11.156142625,0x08,, +11.156238,0x00,, +11.1563335,0x2A,, +11.156429,0x62,, +11.156524375,0xA6,, +11.156619875,0xFF,, +11.174099125,0x55,, +11.1741945,0x55,, +11.17429,0x18,, +11.1743855,0x00,, +11.174481,0x87,, +11.1745765,0x0B,, +11.174671875,0xCA,, +11.174767375,0x00,, +11.174862875,0x2C,, +11.17495825,0x38,, +11.17505375,0x00,, +11.17514925,0x80,, +11.17524475,0x08,, +11.175340125,0x00,, +11.175435625,0xD5,, +11.175531125,0x42,, +11.175626625,0xAB,, +11.175722,0x80,, +11.1758175,0x0D,, +11.175913,0x54,, +11.1760085,0x80,, +11.176103875,0x08,, +11.176199375,0x00,, +11.176294875,0x2A,, +11.176390375,0x62,, +11.17648575,0xA6,, +11.17658125,0x5F,, +11.194199375,0x55,, +11.19429475,0x55,, +11.19439025,0x18,, +11.19448575,0x00,, +11.19458125,0x87,, +11.194676625,0xD1,, +11.194772125,0xCC,, +11.194867625,0x00,, +11.194963,0x2C,, +11.1950585,0x58,, +11.195154,0x00,, +11.1952495,0x80,, +11.195344875,0x08,, +11.195440375,0x00,, +11.195535875,0xD5,, +11.195631375,0x42,, +11.19572675,0xAB,, +11.19582225,0x80,, +11.19591775,0x0D,, +11.19601325,0x54,, +11.196108625,0x80,, +11.196204125,0x08,, +11.196299625,0x00,, +11.196395125,0x2A,, +11.1964905,0x62,, +11.196586,0xA6,, +11.1966815,0x9E,, +11.214290875,0x55,, +11.214386375,0x55,, +11.21448175,0x18,, +11.21457725,0x00,, +11.21467275,0x88,, +11.21476825,0x97,, +11.214863625,0xC7,, +11.214959125,0x00,, +11.215054625,0x2C,, +11.215150125,0x48,, +11.2152455,0x00,, +11.215341,0x80,, +11.2154365,0x08,, +11.215531875,0x00,, +11.215627375,0xD5,, +11.215722875,0x42,, +11.215818375,0xAB,, +11.215913875,0x80,, +11.21600925,0x0D,, +11.21610475,0x54,, +11.21620025,0x80,, +11.216295625,0x08,, +11.216391125,0x00,, +11.216486625,0x2A,, +11.216582125,0x62,, +11.2166775,0xA6,, +11.216773,0x4B,, +11.234391125,0x55,, +11.2344865,0x55,, +11.234582,0x18,, +11.2346775,0x00,, +11.234773,0x89,, +11.234868375,0x5E,, +11.234963875,0xCA,, +11.235059375,0x00,, +11.23515475,0x2C,, +11.23525025,0x68,, +11.23534575,0x00,, +11.23544125,0x80,, +11.23553675,0x08,, +11.235632125,0x00,, +11.235727625,0xD5,, +11.235823125,0x42,, +11.235918625,0xAB,, +11.236014,0x80,, +11.2361095,0x0D,, +11.236205,0x54,, +11.236300375,0x80,, +11.236395875,0x08,, +11.236491375,0x00,, +11.236586875,0x2A,, +11.23668225,0x62,, +11.23677775,0xA6,, +11.23687325,0x6D,, +11.2548905,0x55,, +11.254986,0x55,, +11.2550815,0x2B,, +11.255177,0x03,, +11.255272375,0x8A,, +11.255367875,0x24,, +11.255463375,0xCB,, +11.25555875,0x00,, +11.25565425,0x2C,, +11.25574975,0x58,, +11.25584525,0x00,, +11.255940625,0x80,, +11.256036125,0x08,, +11.256131625,0x00,, +11.256227125,0xD5,, +11.2563225,0x42,, +11.256418,0xAB,, +11.2565135,0x80,, +11.256608875,0x0D,, +11.256704375,0x54,, +11.256799875,0x80,, +11.256895375,0x08,, +11.256990875,0x00,, +11.25708625,0x2A,, +11.25718175,0x62,, +11.25727725,0xA6,, +11.257372625,0x00,, +11.257468125,0x00,, +11.257563625,0x00,, +11.257659125,0x00,, +11.2577545,0x00,, +11.25785,0x00,, +11.2579455,0x00,, +11.258041,0x00,, +11.258136375,0x00,, +11.258231875,0x00,, +11.258327375,0x00,, +11.25842275,0x00,, +11.25851825,0x00,, +11.25861375,0x00,, +11.25870925,0x00,, +11.25880475,0x00,, +11.258900125,0x00,, +11.258995625,0x00,, +11.259091125,0x00,, +11.259186625,0xAB,, +11.2745915,0x55,, +11.274687,0x55,, +11.2747825,0x18,, +11.274877875,0x00,, +11.274973375,0x8A,, +11.275068875,0xEA,, +11.275164375,0xC9,, +11.27525975,0x00,, +11.27535525,0x2C,, +11.27545075,0x58,, +11.27554625,0x00,, +11.275641625,0x80,, +11.275737125,0x08,, +11.275832625,0x00,, +11.275928,0xD5,, +11.2760235,0x42,, +11.276119,0xAB,, +11.2762145,0x80,, +11.27631,0x0D,, +11.276405375,0x54,, +11.276500875,0x80,, +11.276596375,0x08,, +11.27669175,0x00,, +11.27678725,0x2A,, +11.27688275,0x62,, +11.27697825,0xA6,, +11.277073625,0x54,, +11.29469175,0x55,, +11.29478725,0x55,, +11.294882625,0x18,, +11.294978125,0x00,, +11.295073625,0x8B,, +11.295169125,0xB1,, +11.2952645,0xCF,, +11.29536,0x00,, +11.2954555,0x2C,, +11.295550875,0x38,, +11.295646375,0x00,, +11.295741875,0x80,, +11.295837375,0x08,, +11.295932875,0x00,, +11.29602825,0xD5,, +11.29612375,0x42,, +11.29621925,0xAB,, +11.296314625,0x80,, +11.296410125,0x0D,, +11.296505625,0x54,, +11.296601125,0x80,, +11.2966965,0x08,, +11.296792,0x00,, +11.2968875,0x2A,, +11.296983,0x62,, +11.297078375,0xA6,, +11.297173875,0x2A,, +11.31478325,0x55,, +11.31487875,0x55,, +11.31497425,0x18,, +11.315069625,0x00,, +11.315165125,0x8C,, +11.315260625,0x78,, +11.315356125,0xC7,, +11.3154515,0x00,, +11.315547,0x2C,, +11.3156425,0x68,, +11.315738,0x00,, +11.315833375,0x80,, +11.315928875,0x08,, +11.316024375,0x00,, +11.316119875,0xD5,, +11.31621525,0x42,, +11.31631075,0xAB,, +11.31640625,0x80,, +11.31650175,0x0D,, +11.316597125,0x54,, +11.316692625,0x80,, +11.316788125,0x08,, +11.3168835,0x00,, +11.316979,0x2A,, +11.3170745,0x62,, +11.31717,0xA6,, +11.317265375,0xD5,, +11.335013625,0x55,, +11.335109125,0x55,, +11.335204625,0x18,, +11.335300125,0x00,, +11.3353955,0x8D,, +11.335491,0x3F,, +11.3355865,0xC4,, +11.335682,0x00,, +11.335777375,0x2C,, +11.335872875,0x58,, +11.335968375,0x00,, +11.336063875,0x80,, +11.33615925,0x08,, +11.33625475,0x00,, +11.33635025,0xD5,, +11.33644575,0x42,, +11.336541125,0xAB,, +11.336636625,0x80,, +11.336732125,0x0D,, +11.3368275,0x54,, +11.336923,0x80,, +11.3370185,0x08,, +11.337114,0x00,, +11.337209375,0x2A,, +11.337304875,0x62,, +11.337400375,0xA6,, +11.337495875,0x11,, +11.355113875,0x55,, +11.355209375,0x55,, +11.355304875,0x18,, +11.35540025,0x00,, +11.35549575,0x8E,, +11.35559125,0x06,, +11.35568675,0xCA,, +11.355782125,0x00,, +11.355877625,0x2C,, +11.355973125,0x58,, +11.356068625,0x00,, +11.356164,0x80,, +11.3562595,0x08,, +11.356355,0x00,, +11.3564505,0xD5,, +11.356545875,0x42,, +11.356641375,0xAB,, +11.356736875,0x80,, +11.35683225,0x0D,, +11.35692775,0x54,, +11.35702325,0x80,, +11.35711875,0x08,, +11.35721425,0x00,, +11.357309625,0x2A,, +11.357405125,0x62,, +11.357500625,0xA6,, +11.357596,0x32,, +11.375084,0x55,, +11.375179375,0x55,, +11.375274875,0x18,, +11.375370375,0x00,, +11.37546575,0x8E,, +11.37556125,0xCD,, +11.37565675,0xC4,, +11.37575225,0x00,, +11.375847625,0x2C,, +11.375943125,0x68,, +11.376038625,0x00,, +11.376134125,0x80,, +11.3762295,0x08,, +11.376325,0x00,, +11.3764205,0xD5,, +11.376516,0x42,, +11.376611375,0xAB,, +11.376706875,0x80,, +11.376802375,0x0D,, +11.376897875,0x54,, +11.37699325,0x80,, +11.37708875,0x08,, +11.37718425,0x00,, +11.377279625,0x2A,, +11.377375125,0x62,, +11.377470625,0xA6,, +11.377566125,0x5D,, +11.395184125,0x55,, +11.395279625,0x55,, +11.395375125,0x18,, +11.395470625,0x00,, +11.395566,0x8F,, +11.3956615,0x94,, +11.395757,0xDD,, +11.395852375,0x00,, +11.395947875,0x2C,, +11.396043375,0x58,, +11.396138875,0x00,, +11.39623425,0x80,, +11.39632975,0x08,, +11.39642525,0x00,, +11.39652075,0xD5,, +11.396616125,0x42,, +11.396711625,0xAB,, +11.396807125,0x80,, +11.396902625,0x0D,, +11.396998,0x54,, +11.3970935,0x80,, +11.397189,0x08,, +11.397284375,0x00,, +11.397379875,0x2A,, +11.397475375,0x62,, +11.397570875,0xA6,, +11.397666375,0xA8,, +11.41527575,0x55,, +11.415371125,0x55,, +11.415466625,0x18,, +11.415562125,0x00,, +11.4156575,0x90,, +11.415753,0x5B,, +11.4158485,0xCB,, +11.415944,0x00,, +11.4160395,0x2C,, +11.416134875,0x48,, +11.416230375,0x00,, +11.416325875,0x80,, +11.41642125,0x08,, +11.41651675,0x00,, +11.41661225,0xD5,, +11.41670775,0x42,, +11.416803125,0xAB,, +11.416898625,0x80,, +11.416994125,0x0D,, +11.417089625,0x54,, +11.417185,0x80,, +11.4172805,0x08,, +11.417376,0x00,, +11.417471375,0x2A,, +11.417566875,0x62,, +11.417662375,0xA6,, +11.417757875,0xAC,, +11.435375875,0x55,, +11.435471375,0x55,, +11.435566875,0x18,, +11.435662375,0x00,, +11.43575775,0x91,, +11.43585325,0x21,, +11.43594875,0xCB,, +11.43604425,0x00,, +11.436139625,0x2C,, +11.436235125,0x58,, +11.436330625,0x00,, +11.436426,0x80,, +11.4365215,0x08,, +11.436617,0x00,, +11.4367125,0xD5,, +11.436807875,0x42,, +11.436903375,0xAB,, +11.436998875,0x80,, +11.437094375,0x0D,, +11.43718975,0x54,, +11.43728525,0x80,, +11.43738075,0x08,, +11.43747625,0x00,, +11.437571625,0x2A,, +11.437667125,0x62,, +11.437762625,0xA6,, +11.437858125,0x98,, +11.455875375,0x55,, +11.455970875,0x55,, +11.45606625,0x2B,, +11.45616175,0x03,, +11.45625725,0x91,, +11.45635275,0xE7,, +11.456448125,0xCF,, +11.456543625,0x00,, +11.456639125,0x2C,, +11.4567345,0x68,, +11.45683,0x00,, +11.4569255,0x80,, +11.457021,0x08,, +11.4571165,0x00,, +11.457211875,0xD5,, +11.457307375,0x42,, +11.457402875,0xAB,, +11.45749825,0x80,, +11.45759375,0x0D,, +11.45768925,0x54,, +11.45778475,0x80,, +11.457880125,0x08,, +11.457975625,0x00,, +11.458071125,0x2A,, +11.458166625,0x62,, +11.458262,0xA6,, +11.4583575,0x00,, +11.458453,0x00,, +11.4585485,0x00,, +11.458643875,0x00,, +11.458739375,0x00,, +11.458834875,0x00,, +11.458930375,0x00,, +11.45902575,0x00,, +11.45912125,0x00,, +11.45921675,0x00,, +11.45931225,0x00,, +11.459407625,0x00,, +11.459503125,0x00,, +11.459598625,0x00,, +11.459694,0x00,, +11.4597895,0x00,, +11.459885,0x00,, +11.4599805,0x00,, +11.460075875,0x00,, +11.460171375,0xB8,, +11.475576375,0x55,, +11.475671875,0x55,, +11.47576725,0x18,, +11.47586275,0x00,, +11.47595825,0x92,, +11.476053625,0xAE,, +11.476149125,0xCC,, +11.476244625,0x00,, +11.476340125,0x2C,, +11.476435625,0x58,, +11.476531,0x00,, +11.4766265,0x80,, +11.476722,0x08,, +11.476817375,0x00,, +11.476912875,0xD5,, +11.477008375,0x42,, +11.477103875,0xAB,, +11.47719925,0x80,, +11.47729475,0x0D,, +11.47739025,0x54,, +11.47748575,0x80,, +11.477581125,0x08,, +11.477676625,0x00,, +11.477772125,0x2A,, +11.4778675,0x62,, +11.477963,0xA6,, +11.4780585,0xE3,, +11.495667875,0x55,, +11.495763375,0x55,, +11.495858875,0x18,, +11.49595425,0x00,, +11.49604975,0x93,, +11.49614525,0x75,, +11.49624075,0xC0,, +11.496336125,0x00,, +11.496431625,0x2C,, +11.496527125,0x48,, +11.496622625,0x00,, +11.496718,0x80,, +11.4968135,0x08,, +11.496909,0x00,, +11.4970045,0xD5,, +11.497099875,0x42,, +11.497195375,0xAB,, +11.497290875,0x80,, +11.49738625,0x0D,, +11.49748175,0x54,, +11.49757725,0x80,, +11.49767275,0x08,, +11.497768125,0x00,, +11.497863625,0x2A,, +11.497959125,0x62,, +11.498054625,0xA6,, +11.49815,0x25,, +11.515768125,0x55,, +11.515863625,0x55,, +11.515959,0x18,, +11.5160545,0x00,, +11.51615,0x94,, +11.5162455,0x3C,, +11.516340875,0xCB,, +11.516436375,0x00,, +11.516531875,0x2C,, +11.516627375,0x58,, +11.51672275,0x00,, +11.51681825,0x80,, +11.51691375,0x08,, +11.517009125,0x00,, +11.517104625,0xD5,, +11.517200125,0x42,, +11.517295625,0xAB,, +11.517391,0x80,, +11.5174865,0x0D,, +11.517582,0x54,, +11.5176775,0x80,, +11.517773,0x08,, +11.517868375,0x00,, +11.517963875,0x2A,, +11.518059375,0x62,, +11.51815475,0xA6,, +11.51825025,0x0E,, +11.535868375,0x55,, +11.53596375,0x55,, +11.53605925,0x18,, +11.53615475,0x00,, +11.53625025,0x95,, +11.536345625,0x01,, +11.536441125,0xCD,, +11.536536625,0x00,, +11.536632125,0x2C,, +11.5367275,0x58,, +11.536823,0x00,, +11.5369185,0x80,, +11.537013875,0x08,, +11.537109375,0x00,, +11.537204875,0xD5,, +11.537300375,0x42,, +11.537395875,0xAB,, +11.53749125,0x80,, +11.53758675,0x0D,, +11.53768225,0x54,, +11.537777625,0x80,, +11.537873125,0x08,, +11.537968625,0x00,, +11.538064125,0x2A,, +11.5381595,0x62,, +11.538255,0xA6,, +11.5383505,0xF7,, +11.5559685,0x55,, +11.556064,0x55,, +11.5561595,0x18,, +11.556255,0x00,, +11.5563505,0x95,, +11.556445875,0xC8,, +11.556541375,0xBE,, +11.55663675,0x00,, +11.55673225,0x2C,, +11.55682775,0x48,, +11.55692325,0x00,, +11.55701875,0x80,, +11.557114125,0x08,, +11.557209625,0x00,, +11.557305125,0xD5,, +11.557400625,0x42,, +11.557496,0xAB,, +11.5575915,0x80,, +11.557687,0x0D,, +11.557782375,0x54,, +11.557877875,0x80,, +11.557973375,0x08,, +11.558068875,0x00,, +11.55816425,0x2A,, +11.55825975,0x62,, +11.55835525,0xA6,, +11.55845075,0x4F,, +11.57606875,0x55,, +11.57616425,0x55,, +11.57625975,0x18,, +11.576355125,0x00,, +11.576450625,0x96,, +11.576546125,0x8F,, +11.576641625,0xC6,, +11.576737,0x00,, +11.5768325,0x2C,, +11.576928,0x48,, +11.5770235,0x00,, +11.577118875,0x80,, +11.577214375,0x08,, +11.577309875,0x00,, +11.57740525,0xD5,, +11.57750075,0x42,, +11.57759625,0xAB,, +11.57769175,0x80,, +11.577787125,0x0D,, +11.577882625,0x54,, +11.577978125,0x80,, +11.578073625,0x08,, +11.578169125,0x00,, +11.5782645,0x2A,, +11.57836,0x62,, +11.5784555,0xA6,, +11.578550875,0x7F,, +11.596169,0x55,, +11.5962645,0x55,, +11.596359875,0x18,, +11.596455375,0x00,, +11.596550875,0x97,, +11.596646375,0x56,, +11.59674175,0xCD,, +11.59683725,0x00,, +11.59693275,0x2C,, +11.59702825,0x58,, +11.597123625,0x00,, +11.597219125,0x80,, +11.597314625,0x08,, +11.597410125,0x00,, +11.5975055,0xD5,, +11.597601,0x42,, +11.5976965,0xAB,, +11.597792,0x80,, +11.597887375,0x0D,, +11.597982875,0x54,, +11.598078375,0x80,, +11.59817375,0x08,, +11.59826925,0x00,, +11.59836475,0x2A,, +11.59846025,0x62,, +11.598555625,0xA6,, +11.598651125,0xF2,, +11.6162605,0x55,, +11.616356,0x55,, +11.6164515,0x18,, +11.616546875,0x00,, +11.616642375,0x98,, +11.616737875,0x1B,, +11.616833375,0xC8,, +11.61692875,0x00,, +11.61702425,0x2C,, +11.61711975,0x48,, +11.61721525,0x00,, +11.617310625,0x80,, +11.617406125,0x08,, +11.617501625,0x00,, +11.617597125,0xD5,, +11.6176925,0x42,, +11.617788,0xAB,, +11.6178835,0x80,, +11.617979,0x0D,, +11.618074375,0x54,, +11.618169875,0x80,, +11.618265375,0x08,, +11.618360875,0x00,, +11.61845625,0x2A,, +11.61855175,0x62,, +11.61864725,0xA6,, +11.618742625,0xD1,, +11.63636075,0x55,, +11.63645625,0x55,, +11.636551625,0x18,, +11.636647125,0x00,, +11.636742625,0x98,, +11.636838125,0xE1,, +11.6369335,0xCB,, +11.637029,0x00,, +11.6371245,0x2C,, +11.63722,0x68,, +11.637315375,0x00,, +11.637410875,0x80,, +11.637506375,0x08,, +11.637601875,0x00,, +11.63769725,0xD5,, +11.63779275,0x42,, +11.63788825,0xAB,, +11.63798375,0x80,, +11.638079125,0x0D,, +11.638174625,0x54,, +11.638270125,0x80,, +11.6383655,0x08,, +11.638461,0x00,, +11.6385565,0x2A,, +11.638652,0x62,, +11.638747375,0xA6,, +11.638842875,0xCD,, +11.656929625,0x55,, +11.657025125,0x55,, +11.6571205,0x2B,, +11.657216,0x03,, +11.6573115,0x99,, +11.657407,0xA7,, +11.6575025,0xCC,, +11.657597875,0x00,, +11.657693375,0x2C,, +11.657788875,0x58,, +11.65788425,0x00,, +11.65797975,0x80,, +11.65807525,0x08,, +11.65817075,0x00,, +11.658266125,0xD5,, +11.658361625,0x42,, +11.658457125,0xAB,, +11.658552625,0x80,, +11.658648,0x0D,, +11.6587435,0x54,, +11.658839,0x80,, +11.658934375,0x08,, +11.659029875,0x00,, +11.659125375,0x2A,, +11.659220875,0x62,, +11.659316375,0xA6,, +11.65941175,0x00,, +11.65950725,0x00,, +11.65960275,0x00,, +11.65969825,0x00,, +11.659793625,0x00,, +11.659889125,0x00,, +11.659984625,0x00,, +11.66008,0x00,, +11.6601755,0x00,, +11.660271,0x00,, +11.6603665,0x00,, +11.660461875,0x00,, +11.660557375,0x00,, +11.660652875,0x00,, +11.660748375,0x00,, +11.66084375,0x00,, +11.66093925,0x00,, +11.66103475,0x00,, +11.661130125,0x00,, +11.661225625,0xF9,, +11.6765525,0x55,, +11.676648,0x55,, +11.676743375,0x18,, +11.676838875,0x00,, +11.676934375,0x9A,, +11.677029875,0x6C,, +11.677125375,0xCD,, +11.67722075,0x00,, +11.67731625,0x2C,, +11.67741175,0x58,, +11.67750725,0x00,, +11.677602625,0x80,, +11.677698125,0x08,, +11.677793625,0x00,, +11.677889,0xD5,, +11.6779845,0x42,, +11.67808,0xAB,, +11.6781755,0x80,, +11.678270875,0x0D,, +11.678366375,0x54,, +11.678461875,0x80,, +11.678557375,0x08,, +11.67865275,0x00,, +11.67874825,0x2A,, +11.67884375,0x62,, +11.67893925,0xA6,, +11.679034625,0x60,, +11.69665275,0x55,, +11.69674825,0x55,, +11.696843625,0x18,, +11.696939125,0x00,, +11.697034625,0x9B,, +11.697130125,0x32,, +11.6972255,0xCC,, +11.697321,0x00,, +11.6974165,0x2C,, +11.697511875,0x38,, +11.697607375,0x00,, +11.697702875,0x80,, +11.697798375,0x08,, +11.69789375,0x00,, +11.69798925,0xD5,, +11.69808475,0x42,, +11.69818025,0xAB,, +11.698275625,0x80,, +11.698371125,0x0D,, +11.698466625,0x54,, +11.698562125,0x80,, +11.6986575,0x08,, +11.698753,0x00,, +11.6988485,0x2A,, +11.698944,0x62,, +11.699039375,0xA6,, +11.699134875,0x07,, +11.716753,0x55,, +11.716848375,0x55,, +11.716943875,0x18,, +11.717039375,0x00,, +11.717134875,0x9B,, +11.71723025,0xF8,, +11.71732575,0xCB,, +11.71742125,0x00,, +11.717516625,0x2C,, +11.717612125,0x68,, +11.717707625,0x00,, +11.717803125,0x80,, +11.717898625,0x08,, +11.717994,0x00,, +11.7180895,0xD5,, +11.718185,0x42,, +11.718280375,0xAB,, +11.718375875,0x80,, +11.718471375,0x0D,, +11.718566875,0x54,, +11.71866225,0x80,, +11.71875775,0x08,, +11.71885325,0x00,, +11.71894875,0x2A,, +11.719044125,0x62,, +11.719139625,0xA6,, +11.719235125,0xA9,, +11.73694,0x55,, +11.737035375,0x55,, +11.737130875,0x18,, +11.737226375,0x00,, +11.737321875,0x9C,, +11.73741725,0xC0,, +11.73751275,0xC9,, +11.73760825,0x00,, +11.737703625,0x2C,, +11.737799125,0x28,, +11.737894625,0x00,, +11.737990125,0x80,, +11.738085625,0x08,, +11.738181,0x00,, +11.7382765,0xD5,, +11.738372,0x42,, +11.7384675,0xAB,, +11.738562875,0x80,, +11.738658375,0x0D,, +11.738753875,0x54,, +11.73884925,0x80,, +11.73894475,0x08,, +11.73904025,0x00,, +11.73913575,0x2A,, +11.739231125,0x62,, +11.739326625,0xA6,, +11.739422125,0x5F,, +11.757647625,0x55,, +11.757743125,0x55,, +11.757838625,0x18,, +11.757934125,0x00,, +11.758029625,0x9D,, +11.758125,0x8C,, +11.7582205,0xCD,, +11.758316,0x00,, +11.7584115,0x2C,, +11.758506875,0x28,, +11.758602375,0x00,, +11.758697875,0x80,, +11.75879325,0x08,, +11.75888875,0x00,, +11.75898425,0xD5,, +11.75907975,0x42,, +11.759175125,0xAB,, +11.759270625,0x80,, +11.759366125,0x0D,, +11.759461625,0x54,, +11.759557,0x80,, +11.7596525,0x08,, +11.759748,0x00,, +11.7598435,0x2A,, +11.759938875,0x62,, +11.760034375,0xA6,, +11.760129875,0x01,, +11.77730525,0x55,, +11.77740075,0x55,, +11.77749625,0x18,, +11.777591625,0x00,, +11.777687125,0x9E,, +11.777782625,0x4D,, +11.777878125,0xCD,, +11.777973625,0x00,, +11.778069,0x2C,, +11.7781645,0x58,, +11.77826,0x00,, +11.778355375,0x80,, +11.778450875,0x08,, +11.778546375,0x00,, +11.778641875,0xD5,, +11.77873725,0x42,, +11.77883275,0xAB,, +11.77892825,0x80,, +11.77902375,0x0D,, +11.779119125,0x54,, +11.779214625,0x80,, +11.779310125,0x08,, +11.779405625,0x00,, +11.779501,0x2A,, +11.7795965,0x62,, +11.779692,0xA6,, +11.7797875,0x23,, +11.797544375,0x55,, +11.797639875,0x55,, +11.79773525,0x18,, +11.79783075,0x00,, +11.79792625,0x9F,, +11.79802175,0x15,, +11.798117125,0xC2,, +11.798212625,0x00,, +11.798308125,0x2C,, +11.798403625,0x38,, +11.798499,0x00,, +11.7985945,0x80,, +11.79869,0x08,, +11.7987855,0x00,, +11.798880875,0xD5,, +11.798976375,0x42,, +11.799071875,0xAB,, +11.799167375,0x80,, +11.79926275,0x0D,, +11.79935825,0x54,, +11.79945375,0x80,, +11.799549125,0x08,, +11.799644625,0x00,, +11.799740125,0x2A,, +11.799835625,0x62,, +11.799931,0xA6,, +11.8000265,0x5D,, +11.817366875,0x55,, +11.817462375,0x55,, +11.81755775,0x18,, +11.81765325,0x00,, +11.81774875,0x9F,, +11.81784425,0xD8,, +11.817939625,0xDC,, +11.818035125,0x00,, +11.818130625,0x2C,, +11.818226125,0x48,, +11.8183215,0x00,, +11.818417,0x80,, +11.8185125,0x08,, +11.818607875,0x00,, +11.818703375,0xD5,, +11.818798875,0x42,, +11.818894375,0xAB,, +11.818989875,0x80,, +11.81908525,0x0D,, +11.81918075,0x54,, +11.81927625,0x80,, +11.81937175,0x08,, +11.819467125,0x00,, +11.819562625,0x2A,, +11.819658125,0x62,, +11.8197535,0xA6,, +11.819849,0xE3,, +11.83839575,0x55,, +11.83849125,0x55,, +11.838586625,0x18,, +11.838682125,0x00,, +11.838777625,0xA0,, +11.838873,0xA8,, +11.8389685,0xCD,, +11.839064,0x00,, +11.8391595,0x2C,, +11.839255,0x48,, +11.839350375,0x00,, +11.839445875,0x80,, +11.839541375,0x08,, +11.83963675,0x00,, +11.83973225,0xD5,, +11.83982775,0x42,, +11.83992325,0xAB,, +11.840018625,0x80,, +11.840114125,0x0D,, +11.840209625,0x54,, +11.840305125,0x80,, +11.8404005,0x08,, +11.840496,0x00,, +11.8405915,0x2A,, +11.840686875,0x62,, +11.840782375,0xA6,, +11.840877875,0x50,, +11.857836375,0x55,, +11.857931875,0x55,, +11.85802725,0x2B,, +11.85812275,0x03,, +11.85821825,0xA1,, +11.85831375,0x63,, +11.858409125,0xDC,, +11.858504625,0x00,, +11.858600125,0x2C,, +11.8586955,0x28,, +11.858791,0x00,, +11.8588865,0x80,, +11.858982,0x08,, +11.859077375,0x00,, +11.859172875,0xD5,, +11.859268375,0x42,, +11.859363875,0xAB,, +11.85945925,0x80,, +11.85955475,0x0D,, +11.85965025,0x54,, +11.85974575,0x80,, +11.859841125,0x08,, +11.859936625,0x00,, +11.860032125,0x2A,, +11.860127625,0x62,, +11.860223,0xA6,, +11.8603185,0x00,, +11.860414,0x00,, +11.8605095,0x00,, +11.860604875,0x00,, +11.860700375,0x00,, +11.860795875,0x00,, +11.86089125,0x00,, +11.86098675,0x00,, +11.86108225,0x00,, +11.86117775,0x00,, +11.86127325,0x00,, +11.861368625,0x00,, +11.861464125,0x00,, +11.861559625,0x00,, +11.861655,0x00,, +11.8617505,0x00,, +11.861846,0x00,, +11.8619415,0x00,, +11.862036875,0x00,, +11.862132375,0xEB,, +11.877528625,0x55,, +11.877624125,0x55,, +11.877719625,0x18,, +11.877815125,0x00,, +11.8779105,0xA2,, +11.878006,0x2A,, +11.8781015,0xDB,, +11.878197,0x00,, +11.878292375,0x2C,, +11.878387875,0x48,, +11.878483375,0x00,, +11.87857875,0x80,, +11.87867425,0x08,, +11.87876975,0x00,, +11.87886525,0xD5,, +11.878960625,0x42,, +11.879056125,0xAB,, +11.879151625,0x80,, +11.879247125,0x0D,, +11.8793425,0x54,, +11.879438,0x80,, +11.8795335,0x08,, +11.879628875,0x00,, +11.879724375,0x2A,, +11.879819875,0x62,, +11.879915375,0xA6,, +11.880010875,0x74,, +11.897628875,0x55,, +11.897724375,0x55,, +11.897819875,0x18,, +11.89791525,0x00,, +11.89801075,0xA2,, +11.89810625,0xF1,, +11.89820175,0xDD,, +11.898297125,0x00,, +11.898392625,0x2C,, +11.898488125,0x58,, +11.8985835,0x00,, +11.898679,0x80,, +11.8987745,0x08,, +11.89887,0x00,, +11.8989655,0xD5,, +11.899060875,0x42,, +11.899156375,0xAB,, +11.899251875,0x80,, +11.89934725,0x0D,, +11.89944275,0x54,, +11.89953825,0x80,, +11.89963375,0x08,, +11.899729125,0x00,, +11.899824625,0x2A,, +11.899920125,0x62,, +11.900015625,0xA6,, +11.900111,0xDF,, +11.917720375,0x55,, +11.917815875,0x55,, +11.917911375,0x18,, +11.918006875,0x00,, +11.91810225,0xA3,, +11.91819775,0xB7,, +11.91829325,0xDC,, +11.91838875,0x00,, +11.918484125,0x2C,, +11.918579625,0x38,, +11.918675125,0x00,, +11.9187705,0x80,, +11.918866,0x08,, +11.9189615,0x00,, +11.919057,0xD5,, +11.919152375,0x42,, +11.919247875,0xAB,, +11.919343375,0x80,, +11.919438875,0x0D,, +11.919534375,0x54,, +11.91962975,0x80,, +11.91972525,0x08,, +11.91982075,0x00,, +11.919916125,0x2A,, +11.920011625,0x62,, +11.920107125,0xA6,, +11.920202625,0xEE,, +11.937829375,0x55,, +11.93792475,0x55,, +11.93802025,0x18,, +11.93811575,0x00,, +11.93821125,0xA4,, +11.938306625,0x7D,, +11.938402125,0xDD,, +11.938497625,0x00,, +11.938593125,0x2C,, +11.9386885,0x68,, +11.938784,0x00,, +11.9388795,0x80,, +11.938974875,0x08,, +11.939070375,0x00,, +11.939165875,0xD5,, +11.939261375,0x42,, +11.93935675,0xAB,, +11.93945225,0x80,, +11.93954775,0x0D,, +11.93964325,0x54,, +11.939738625,0x80,, +11.939834125,0x08,, +11.939929625,0x00,, +11.940025125,0x2A,, +11.9401205,0x62,, +11.940216,0xA6,, +11.9403115,0x11,, +11.957920875,0x55,, +11.958016375,0x55,, +11.958111875,0x18,, +11.95820725,0x00,, +11.95830275,0xA5,, +11.95839825,0x43,, +11.958493625,0xDA,, +11.958589125,0x00,, +11.958684625,0x2C,, +11.958780125,0x68,, +11.9588755,0x00,, +11.958971,0x80,, +11.9590665,0x08,, +11.959162,0x00,, +11.959257375,0xD5,, +11.959352875,0x42,, +11.959448375,0xAB,, +11.95954375,0x80,, +11.95963925,0x0D,, +11.95973475,0x54,, +11.95983025,0x80,, +11.95992575,0x08,, +11.960021125,0x00,, +11.960116625,0x2A,, +11.960212125,0x62,, +11.9603075,0xA6,, +11.960403,0x8F,, +11.978021125,0x55,, +11.9781165,0x55,, +11.978212,0x18,, +11.9783075,0x00,, +11.978403,0xA6,, +11.978498375,0x08,, +11.978593875,0xDB,, +11.978689375,0x00,, +11.978784875,0x2C,, +11.97888025,0x58,, +11.97897575,0x00,, +11.97907125,0x80,, +11.979166625,0x08,, +11.979262125,0x00,, +11.979357625,0xD5,, +11.979453125,0x42,, +11.979548625,0xAB,, +11.979644,0x80,, +11.9797395,0x0D,, +11.979835,0x54,, +11.9799305,0x80,, +11.980025875,0x08,, +11.980121375,0x00,, +11.980216875,0x2A,, +11.98031225,0x62,, +11.98040775,0xA6,, +11.98050325,0x9C,, +11.99812125,0x55,, +11.99821675,0x55,, +11.99831225,0x18,, +11.99840775,0x00,, +11.998503125,0xA6,, +11.998598625,0xCE,, +11.998694125,0xDC,, +11.998789625,0x00,, +11.998885,0x2C,, +11.9989805,0x48,, +11.999076,0x00,, +11.9991715,0x80,, +11.999266875,0x08,, +11.999362375,0x00,, +11.999457875,0xD5,, +11.999553375,0x42,, +11.99964875,0xAB,, +11.99974425,0x80,, +11.99983975,0x0D,, +11.999935125,0x54,, +12.000030625,0x80,, +12.000126125,0x08,, +12.000221625,0x00,, +12.000317,0x2A,, +12.0004125,0x62,, +12.000508,0xA6,, +12.0006035,0x90,, +12.0182215,0x55,, +12.018317,0x55,, +12.0184125,0x18,, +12.018507875,0x00,, +12.018603375,0xA7,, +12.018698875,0x94,, +12.018794375,0xDB,, +12.01888975,0x00,, +12.01898525,0x2C,, +12.01908075,0x58,, +12.01917625,0x00,, +12.019271625,0x80,, +12.019367125,0x08,, +12.019462625,0x00,, +12.019558125,0xD5,, +12.0196535,0x42,, +12.019749,0xAB,, +12.0198445,0x80,, +12.019939875,0x0D,, +12.020035375,0x54,, +12.020130875,0x80,, +12.020226375,0x08,, +12.020321875,0x00,, +12.02041725,0x2A,, +12.02051275,0x62,, +12.02060825,0xA6,, +12.020703625,0xA8,, +12.038313,0x55,, +12.0384085,0x55,, +12.038504,0x18,, +12.0385995,0x00,, +12.038695,0xA8,, +12.038790375,0x5A,, +12.038885875,0xDB,, +12.038981375,0x00,, +12.03907675,0x2C,, +12.03917225,0x68,, +12.03926775,0x00,, +12.03936325,0x80,, +12.039458625,0x08,, +12.039554125,0x00,, +12.039649625,0xD5,, +12.039745125,0x42,, +12.0398405,0xAB,, +12.039936,0x80,, +12.0400315,0x0D,, +12.040126875,0x54,, +12.040222375,0x80,, +12.040317875,0x08,, +12.040413375,0x00,, +12.04050875,0x2A,, +12.04060425,0x62,, +12.04069975,0xA6,, +12.04079525,0xB2,, +12.058821125,0x55,, +12.058916625,0x55,, +12.059012125,0x2B,, +12.059107625,0x03,, +12.059203,0xA9,, +12.0592985,0x21,, +12.059394,0xDA,, +12.0594895,0x00,, +12.059584875,0x2C,, +12.059680375,0x58,, +12.059775875,0x00,, +12.059871375,0x80,, +12.05996675,0x08,, +12.06006225,0x00,, +12.06015775,0xD5,, +12.06025325,0x42,, +12.060348625,0xAB,, +12.060444125,0x80,, +12.060539625,0x0D,, +12.060635125,0x54,, +12.0607305,0x80,, +12.060826,0x08,, +12.0609215,0x00,, +12.061016875,0x2A,, +12.061112375,0x62,, +12.061207875,0xA6,, +12.061303375,0x00,, +12.061398875,0x00,, +12.06149425,0x00,, +12.06158975,0x00,, +12.06168525,0x00,, +12.061780625,0x00,, +12.061876125,0x00,, +12.061971625,0x00,, +12.062067125,0x00,, +12.0621625,0x00,, +12.062258,0x00,, +12.0623535,0x00,, +12.062449,0x00,, +12.062544375,0x00,, +12.062639875,0x00,, +12.062735375,0x00,, +12.06283075,0x00,, +12.06292625,0x00,, +12.06302175,0x00,, +12.06311725,0xF6,, +12.0785135,0x55,, +12.078609,0x55,, +12.078704375,0x18,, +12.078799875,0x00,, +12.078895375,0xA9,, +12.078990875,0xE8,, +12.07908625,0xDB,, +12.07918175,0x00,, +12.07927725,0x2C,, +12.07937275,0x68,, +12.07946825,0x00,, +12.079563625,0x80,, +12.079659125,0x08,, +12.079754625,0x00,, +12.07985,0xD5,, +12.0799455,0x42,, +12.080041,0xAB,, +12.0801365,0x80,, +12.080231875,0x0D,, +12.080327375,0x54,, +12.080422875,0x80,, +12.080518375,0x08,, +12.08061375,0x00,, +12.08070925,0x2A,, +12.08080475,0x62,, +12.080900125,0xA6,, +12.080995625,0xEA,, +12.098605,0x55,, +12.0987005,0x55,, +12.098796,0x18,, +12.0988915,0x00,, +12.098986875,0xAA,, +12.099082375,0xAE,, +12.099177875,0xDC,, +12.09927325,0x00,, +12.09936875,0x2C,, +12.09946425,0x48,, +12.09955975,0x00,, +12.099655125,0x80,, +12.099750625,0x08,, +12.099846125,0x00,, +12.099941625,0xD5,, +12.100037,0x42,, +12.1001325,0xAB,, +12.100228,0x80,, +12.1003235,0x0D,, +12.100418875,0x54,, +12.100514375,0x80,, +12.100609875,0x08,, +12.100705375,0x00,, +12.10080075,0x2A,, +12.10089625,0x62,, +12.10099175,0xA6,, +12.10108725,0x1E,, +12.118714,0x55,, +12.118809375,0x55,, +12.118904875,0x18,, +12.119000375,0x00,, +12.119095875,0xAB,, +12.11919125,0x75,, +12.11928675,0xDB,, +12.11938225,0x00,, +12.119477625,0x2C,, +12.119573125,0x68,, +12.119668625,0x00,, +12.119764125,0x80,, +12.1198595,0x08,, +12.119955,0x00,, +12.1200505,0xD5,, +12.120146,0x42,, +12.120241375,0xAB,, +12.120336875,0x80,, +12.120432375,0x0D,, +12.120527875,0x54,, +12.12062325,0x80,, +12.12071875,0x08,, +12.12081425,0x00,, +12.12090975,0x2A,, +12.121005125,0x62,, +12.121100625,0xA6,, +12.121196125,0xEC,, +12.138814125,0x55,, +12.138909625,0x55,, +12.139005125,0x18,, +12.1391005,0x00,, +12.139196,0xAC,, +12.1392915,0x3B,, +12.139387,0xDC,, +12.139482375,0x00,, +12.139577875,0x2C,, +12.139673375,0x48,, +12.139768875,0x00,, +12.13986425,0x80,, +12.13995975,0x08,, +12.14005525,0x00,, +12.14015075,0xD5,, +12.140246125,0x42,, +12.140341625,0xAB,, +12.140437125,0x80,, +12.140532625,0x0D,, +12.140628,0x54,, +12.1407235,0x80,, +12.140819,0x08,, +12.1409145,0x00,, +12.141009875,0x2A,, +12.141105375,0x62,, +12.141200875,0xA6,, +12.14129625,0x65,, +12.15890575,0x55,, +12.159001125,0x55,, +12.159096625,0x18,, +12.159192125,0x00,, +12.159287625,0xAD,, +12.159383,0x02,, +12.1594785,0xDA,, +12.159574,0x00,, +12.159669375,0x2C,, +12.159764875,0x68,, +12.159860375,0x00,, +12.159955875,0x80,, +12.160051375,0x08,, +12.16014675,0x00,, +12.16024225,0xD5,, +12.16033775,0x42,, +12.160433125,0xAB,, +12.160528625,0x80,, +12.160624125,0x0D,, +12.160719625,0x54,, +12.160815,0x80,, +12.1609105,0x08,, +12.161006,0x00,, +12.1611015,0x2A,, +12.161196875,0x62,, +12.161292375,0xA6,, +12.161387875,0x42,, +12.179005875,0x55,, +12.179101375,0x55,, +12.179196875,0x18,, +12.17929225,0x00,, +12.17938775,0xAD,, +12.17948325,0xC9,, +12.17957875,0xDA,, +12.17967425,0x00,, +12.179769625,0x2C,, +12.179865125,0x68,, +12.179960625,0x00,, +12.180056125,0x80,, +12.1801515,0x08,, +12.180247,0x00,, +12.1803425,0xD5,, +12.180437875,0x42,, +12.180533375,0xAB,, +12.180628875,0x80,, +12.180724375,0x0D,, +12.18081975,0x54,, +12.18091525,0x80,, +12.18101075,0x08,, +12.18110625,0x00,, +12.181201625,0x2A,, +12.181297125,0x62,, +12.181392625,0xA6,, +12.181488125,0x85,, +12.199106125,0x55,, +12.199201625,0x55,, +12.199297125,0x18,, +12.1993925,0x00,, +12.199488,0xAE,, +12.1995835,0x90,, +12.199679,0xDB,, +12.199774375,0x00,, +12.199869875,0x2C,, +12.199965375,0x48,, +12.20006075,0x00,, +12.20015625,0x80,, +12.20025175,0x08,, +12.20034725,0x00,, +12.200442625,0xD5,, +12.200538125,0x42,, +12.200633625,0xAB,, +12.200729125,0x80,, +12.200824625,0x0D,, +12.20092,0x54,, +12.2010155,0x80,, +12.201111,0x08,, +12.201206375,0x00,, +12.201301875,0x2A,, +12.201397375,0x62,, +12.201492875,0xA6,, +12.20158825,0x9D,, +12.219206375,0x55,, +12.219301875,0x55,, +12.21939725,0x18,, +12.21949275,0x00,, +12.21958825,0xAF,, +12.21968375,0x57,, +12.219779125,0xDC,, +12.219874625,0x00,, +12.219970125,0x2C,, +12.2200655,0x68,, +12.220161,0x00,, +12.2202565,0x80,, +12.220352,0x08,, +12.2204475,0x00,, +12.220542875,0xD5,, +12.220638375,0x42,, +12.220733875,0xAB,, +12.22082925,0x80,, +12.22092475,0x0D,, +12.22102025,0x54,, +12.22111575,0x80,, +12.221211125,0x08,, +12.221306625,0x00,, +12.221402125,0x2A,, +12.221497625,0x62,, +12.221593,0xA6,, +12.2216885,0x20,, +12.239297875,0x55,, +12.239393375,0x55,, +12.239488875,0x18,, +12.23958425,0x00,, +12.23967975,0xB0,, +12.23977525,0x1E,, +12.23987075,0xDB,, +12.239966125,0x00,, +12.240061625,0x2C,, +12.240157125,0x48,, +12.240252625,0x00,, +12.240348,0x80,, +12.2404435,0x08,, +12.240539,0x00,, +12.2406345,0xD5,, +12.240729875,0x42,, +12.240825375,0xAB,, +12.240920875,0x80,, +12.241016375,0x0D,, +12.24111175,0x54,, +12.24120725,0x80,, +12.24130275,0x08,, +12.241398125,0x00,, +12.241493625,0x2A,, +12.241589125,0x62,, +12.241684625,0xA6,, +12.24178,0x5E,, +12.259797375,0x55,, +12.25989275,0x55,, +12.25998825,0x2B,, +12.26008375,0x03,, +12.26017925,0xB0,, +12.26027475,0xE5,, +12.260370125,0xD9,, +12.260465625,0x00,, +12.260561125,0x2C,, +12.2606565,0x38,, +12.260752,0x00,, +12.2608475,0x80,, +12.260943,0x08,, +12.261038375,0x00,, +12.261133875,0xD5,, +12.261229375,0x42,, +12.261324875,0xAB,, +12.26142025,0x80,, +12.26151575,0x0D,, +12.26161125,0x54,, +12.261706625,0x80,, +12.261802125,0x08,, +12.261897625,0x00,, +12.261993125,0x2A,, +12.262088625,0x62,, +12.262184,0xA6,, +12.2622795,0x00,, +12.262375,0x00,, +12.2624705,0x00,, +12.262565875,0x00,, +12.262661375,0x00,, +12.262756875,0x00,, +12.26285225,0x00,, +12.26294775,0x00,, +12.26304325,0x00,, +12.26313875,0x00,, +12.263234125,0x00,, +12.263329625,0x00,, +12.263425125,0x00,, +12.263520625,0x00,, +12.263616,0x00,, +12.2637115,0x00,, +12.263807,0x00,, +12.263902375,0x00,, +12.263997875,0x00,, +12.264093375,0x72,, +12.279498375,0x55,, +12.279593875,0x55,, +12.27968925,0x18,, +12.27978475,0x00,, +12.27988025,0xB1,, +12.279975625,0xAA,, +12.280071125,0xDB,, +12.280166625,0x00,, +12.280262125,0x2C,, +12.2803575,0x48,, +12.280453,0x00,, +12.2805485,0x80,, +12.280644,0x08,, +12.280739375,0x00,, +12.280834875,0xD5,, +12.280930375,0x42,, +12.28102575,0xAB,, +12.28112125,0x80,, +12.28121675,0x0D,, +12.28131225,0x54,, +12.28140775,0x80,, +12.281503125,0x08,, +12.281598625,0x00,, +12.281694125,0x2A,, +12.2817895,0x62,, +12.281885,0xA6,, +12.2819805,0x90,, +12.299589875,0x55,, +12.299685375,0x55,, +12.299780875,0x18,, +12.29987625,0x00,, +12.29997175,0xB2,, +12.30006725,0x70,, +12.30016275,0xDB,, +12.300258125,0x00,, +12.300353625,0x2C,, +12.300449125,0x58,, +12.3005445,0x00,, +12.30064,0x80,, +12.3007355,0x08,, +12.300831,0x00,, +12.300926375,0xD5,, +12.301021875,0x42,, +12.301117375,0xAB,, +12.301212875,0x80,, +12.30130825,0x0D,, +12.30140375,0x54,, +12.30149925,0x80,, +12.301594625,0x08,, +12.301690125,0x00,, +12.301785625,0x2A,, +12.301881125,0x62,, +12.301976625,0xA6,, +12.302072,0xE1,, +12.319690125,0x55,, +12.319785625,0x55,, +12.319881,0x18,, +12.3199765,0x00,, +12.320072,0xB3,, +12.320167375,0x36,, +12.320262875,0xDC,, +12.320358375,0x00,, +12.320453875,0x2C,, +12.32054925,0x68,, +12.32064475,0x00,, +12.32074025,0x80,, +12.32083575,0x08,, +12.320931125,0x00,, +12.321026625,0xD5,, +12.321122125,0x42,, +12.321217625,0xAB,, +12.321313,0x80,, +12.3214085,0x0D,, +12.321504,0x54,, +12.3215995,0x80,, +12.321694875,0x08,, +12.321790375,0x00,, +12.321885875,0x2A,, +12.321981375,0x62,, +12.32207675,0xA6,, +12.32217225,0x51,, +12.339781625,0x55,, +12.339877125,0x55,, +12.339972625,0x18,, +12.340068,0x00,, +12.3401635,0xB3,, +12.340259,0xFC,, +12.3403545,0xDB,, +12.340449875,0x00,, +12.340545375,0x2C,, +12.340640875,0x58,, +12.34073625,0x00,, +12.34083175,0x80,, +12.34092725,0x08,, +12.34102275,0x00,, +12.341118125,0xD5,, +12.341213625,0x42,, +12.341309125,0xAB,, +12.341404625,0x80,, +12.3415,0x0D,, +12.3415955,0x54,, +12.341691,0x80,, +12.3417865,0x08,, +12.341881875,0x00,, +12.341977375,0x2A,, +12.342072875,0x62,, +12.342168375,0xA6,, +12.34226375,0xB1,, +12.3598905,0x55,, +12.359986,0x55,, +12.3600815,0x18,, +12.360177,0x00,, +12.360272375,0xB4,, +12.360367875,0xC3,, +12.360463375,0xDC,, +12.36055875,0x00,, +12.36065425,0x2C,, +12.36074975,0x48,, +12.36084525,0x00,, +12.360940625,0x80,, +12.361036125,0x08,, +12.361131625,0x00,, +12.361227125,0xD5,, +12.3613225,0x42,, +12.361418,0xAB,, +12.3615135,0x80,, +12.361609,0x0D,, +12.361704375,0x54,, +12.361799875,0x80,, +12.361895375,0x08,, +12.361990875,0x00,, +12.36208625,0x2A,, +12.36218175,0x62,, +12.36227725,0xA6,, +12.36237275,0xE0,, +12.379982125,0x55,, +12.3800775,0x55,, +12.380173,0x18,, +12.3802685,0x00,, +12.380364,0xB5,, +12.380459375,0x89,, +12.380554875,0xDC,, +12.380650375,0x00,, +12.380745875,0x2C,, +12.38084125,0x48,, +12.38093675,0x00,, +12.38103225,0x80,, +12.381127625,0x08,, +12.381223125,0x00,, +12.381318625,0xD5,, +12.381414125,0x42,, +12.3815095,0xAB,, +12.381605,0x80,, +12.3817005,0x0D,, +12.381796,0x54,, +12.3818915,0x80,, +12.381986875,0x08,, +12.382082375,0x00,, +12.38217775,0x2A,, +12.38227325,0x62,, +12.38236875,0xA6,, +12.38246425,0x98,, +12.40008225,0x55,, +12.40017775,0x55,, +12.40027325,0x18,, +12.40036875,0x00,, +12.400464125,0xB6,, +12.400559625,0x4F,, +12.400655125,0xD9,, +12.400750625,0x00,, +12.400846,0x2C,, +12.4009415,0x58,, +12.401037,0x00,, +12.401132375,0x80,, +12.401227875,0x08,, +12.401323375,0x00,, +12.401418875,0xD5,, +12.401514375,0x42,, +12.40160975,0xAB,, +12.40170525,0x80,, +12.40180075,0x0D,, +12.401896125,0x54,, +12.401991625,0x80,, +12.402087125,0x08,, +12.402182625,0x00,, +12.402278,0x2A,, +12.4023735,0x62,, +12.402469,0xA6,, +12.4025645,0x3A,, +12.4201825,0x55,, +12.420278,0x55,, +12.4203735,0x18,, +12.420468875,0x00,, +12.420564375,0xB7,, +12.420659875,0x16,, +12.42075525,0xDB,, +12.42085075,0x00,, +12.42094625,0x2C,, +12.42104175,0x58,, +12.42113725,0x00,, +12.421232625,0x80,, +12.421328125,0x08,, +12.421423625,0x00,, +12.421519125,0xD5,, +12.4216145,0x42,, +12.42171,0xAB,, +12.4218055,0x80,, +12.421900875,0x0D,, +12.421996375,0x54,, +12.422091875,0x80,, +12.422187375,0x08,, +12.42228275,0x00,, +12.42237825,0x2A,, +12.42247375,0x62,, +12.42256925,0xA6,, +12.422664625,0x35,, +12.44028275,0x55,, +12.44037825,0x55,, +12.440473625,0x18,, +12.440569125,0x00,, +12.440664625,0xB7,, +12.440760125,0xDC,, +12.4408555,0xDB,, +12.440951,0x00,, +12.4410465,0x2C,, +12.441142,0x48,, +12.441237375,0x00,, +12.441332875,0x80,, +12.441428375,0x08,, +12.44152375,0x00,, +12.44161925,0xD5,, +12.44171475,0x42,, +12.44181025,0xAB,, +12.441905625,0x80,, +12.442001125,0x0D,, +12.442096625,0x54,, +12.442192125,0x80,, +12.4422875,0x08,, +12.442383,0x00,, +12.4424785,0x2A,, +12.442574,0x62,, +12.442669375,0xA6,, +12.442764875,0xD6,, +12.460782125,0x55,, +12.460877625,0x55,, +12.460973125,0x2B,, +12.461068625,0x03,, +12.461164,0xB8,, +12.4612595,0xA3,, +12.461355,0xDC,, +12.4614505,0x00,, +12.461545875,0x2C,, +12.461641375,0x58,, +12.461736875,0x00,, +12.46183225,0x80,, +12.46192775,0x08,, +12.46202325,0x00,, +12.46211875,0xD5,, +12.46221425,0x42,, +12.462309625,0xAB,, +12.462405125,0x80,, +12.462500625,0x0D,, +12.462596125,0x54,, +12.4626915,0x80,, +12.462787,0x08,, +12.4628825,0x00,, +12.462977875,0x2A,, +12.463073375,0x62,, +12.463168875,0xA6,, +12.463264375,0x00,, +12.46335975,0x00,, +12.46345525,0x00,, +12.46355075,0x00,, +12.46364625,0x00,, +12.463741625,0x00,, +12.463837125,0x00,, +12.463932625,0x00,, +12.464028125,0x00,, +12.4641235,0x00,, +12.464219,0x00,, +12.4643145,0x00,, +12.46441,0x00,, +12.464505375,0x00,, +12.464600875,0x00,, +12.464696375,0x00,, +12.46479175,0x00,, +12.46488725,0x00,, +12.46498275,0x00,, +12.46507825,0x5C,, +12.480483125,0x55,, +12.480578625,0x55,, +12.480674125,0x18,, +12.480769625,0x00,, +12.480865,0xB9,, +12.4809605,0x69,, +12.481056,0xDD,, +12.481151375,0x00,, +12.481246875,0x2C,, +12.481342375,0x68,, +12.481437875,0x00,, +12.481533375,0x80,, +12.48162875,0x08,, +12.48172425,0x00,, +12.48181975,0xD5,, +12.481915125,0x42,, +12.482010625,0xAB,, +12.482106125,0x80,, +12.482201625,0x0D,, +12.482297,0x54,, +12.4823925,0x80,, +12.482488,0x08,, +12.4825835,0x00,, +12.482678875,0x2A,, +12.482774375,0x62,, +12.482869875,0xA6,, +12.482965375,0xD4,, +12.50057475,0x55,, +12.500670125,0x55,, +12.500765625,0x18,, +12.500861125,0x00,, +12.500956625,0xBA,, +12.501052,0x30,, +12.5011475,0xDB,, +12.501243,0x00,, +12.5013385,0x2C,, +12.501433875,0x58,, +12.501529375,0x00,, +12.501624875,0x80,, +12.501720375,0x08,, +12.50181575,0x00,, +12.50191125,0xD5,, +12.50200675,0x42,, +12.50210225,0xAB,, +12.502197625,0x80,, +12.502293125,0x0D,, +12.502388625,0x54,, +12.502484,0x80,, +12.5025795,0x08,, +12.502675,0x00,, +12.5027705,0x2A,, +12.502865875,0x62,, +12.502961375,0xA6,, +12.503056875,0xE8,, From a18a6b3b6152622c3536704cf8b46da8e107b539 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 20 Dec 2014 18:50:36 +0100 Subject: [PATCH 160/216] Fix up unit test makefile, add option to rull all of them --- unittests/Makefile | 36 +++++++++++++++++++++--------------- 1 file changed, 21 insertions(+), 15 deletions(-) diff --git a/unittests/Makefile b/unittests/Makefile index 8742e2f7c0..dfd5ccb3fc 100644 --- a/unittests/Makefile +++ b/unittests/Makefile @@ -1,35 +1,35 @@ CC=g++ -CFLAGS=-I. -I../../src/modules -I ../../src/include -I../../src/drivers \ - -I../../src -I../../src/lib -D__EXPORT="" -Dnullptr="0" -lm +CFLAGS=-I. -I../src/modules -I ../src/include -I../src/drivers \ + -I../src -I../src/lib -D__EXPORT="" -Dnullptr="0" -lm all: mixer_test sbus2_test autodeclination_test st24_test sf0x_test -MIXER_FILES=../../src/systemcmds/tests/test_mixer.cpp \ - ../../src/systemcmds/tests/test_conv.cpp \ - ../../src/modules/systemlib/mixer/mixer_simple.cpp \ - ../../src/modules/systemlib/mixer/mixer_multirotor.cpp \ - ../../src/modules/systemlib/mixer/mixer.cpp \ - ../../src/modules/systemlib/mixer/mixer_group.cpp \ - ../../src/modules/systemlib/mixer/mixer_load.c \ - ../../src/modules/systemlib/pwm_limit/pwm_limit.c \ +MIXER_FILES=../src/systemcmds/tests/test_mixer.cpp \ + ../src/systemcmds/tests/test_conv.cpp \ + ../src/modules/systemlib/mixer/mixer_simple.cpp \ + ../src/modules/systemlib/mixer/mixer_multirotor.cpp \ + ../src/modules/systemlib/mixer/mixer.cpp \ + ../src/modules/systemlib/mixer/mixer_group.cpp \ + ../src/modules/systemlib/mixer/mixer_load.c \ + ../src/modules/systemlib/pwm_limit/pwm_limit.c \ hrt.cpp \ mixer_test.cpp -SBUS2_FILES=../../src/modules/px4iofirmware/sbus.c \ +SBUS2_FILES=../src/modules/px4iofirmware/sbus.c \ hrt.cpp \ sbus2_test.cpp -ST24_FILES=../../src/lib/rc/st24.c \ +ST24_FILES=../src/lib/rc/st24.c \ hrt.cpp \ st24_test.cpp SF0X_FILES= \ hrt.cpp \ sf0x_test.cpp \ - ../../src/drivers/sf0x/sf0x_parser.cpp + ../src/drivers/sf0x/sf0x_parser.cpp -AUTODECLINATION_FILES= ../../src/lib/geo_lookup/geo_mag_declination.c \ +AUTODECLINATION_FILES= ../src/lib/geo_lookup/geo_mag_declination.c \ hrt.cpp \ autodeclination_test.cpp @@ -48,7 +48,13 @@ autodeclination_test: $(SBUS2_FILES) st24_test: $(ST24_FILES) $(CC) -o st24_test $(ST24_FILES) $(CFLAGS) -.PHONY: clean +unittests: clean mixer_test sbus2_test sf0x_test autodeclination_test st24_test + ./mixer_test + ./sbus2_test + ./sf0x_test + ./autodeclination_test + ./st24_test +.PHONY: clean clean: rm -f $(ODIR)/*.o *~ core $(INCDIR)/*~ mixer_test sbus2_test autodeclination_test st24_test sf0x_test From a4606dc2708b4ad8e463bbd515551ad8f58ee5d5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 20 Dec 2014 18:50:51 +0100 Subject: [PATCH 161/216] Add make tests to Makefile --- Makefile | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/Makefile b/Makefile index 809f54dd31..cfc8159b1f 100644 --- a/Makefile +++ b/Makefile @@ -230,6 +230,13 @@ updatesubmodules: testbuild: $(Q) (cd $(PX4_BASE) && $(MAKE) distclean && $(MAKE) archives && $(MAKE) -j8) +# +# Unittest targets. Builds and runs the host-level +# unit tests. +.PHONY: tests +tests: + $(Q) (cd $(PX4_BASE)/unittests && $(MAKE) unittests) + # # Cleanup targets. 'clean' should remove all built products and force # a complete re-compilation, 'distclean' should remove everything From af9e62cf75f73bdfc37db286620ef353005e5f4a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 20 Dec 2014 18:51:31 +0100 Subject: [PATCH 162/216] unittests: Improve auto declination test, not great coverage yet. --- unittests/autodeclination_test.cpp | 36 +++++++++++++++++++++++++----- 1 file changed, 30 insertions(+), 6 deletions(-) diff --git a/unittests/autodeclination_test.cpp b/unittests/autodeclination_test.cpp index 93bc340bb3..1bda6eb795 100644 --- a/unittests/autodeclination_test.cpp +++ b/unittests/autodeclination_test.cpp @@ -3,26 +3,50 @@ #include #include #include +#include #include #include #include #include -#include "../../src/systemcmds/tests/tests.h" +// #include "../../src/systemcmds/tests/tests.h" #include int main(int argc, char *argv[]) { warnx("autodeclination test started"); - if (argc < 3) - errx(1, "Need lat/lon!"); + char* latstr = 0; + char* lonstr = 0; + char* declstr = 0; + + if (argc < 4) { + warnx("Too few arguments. Using default lat / lon and declination"); + latstr = "47.0"; + lonstr = "8.0"; + declstr = "0.6"; + } else { + latstr = argv[1]; + lonstr = argv[2]; + declstr = argv[3]; + } char* p_end; - float lat = strtod(argv[1], &p_end); - float lon = strtod(argv[2], &p_end); + float lat = strtod(latstr, &p_end); + float lon = strtod(lonstr, &p_end); + float decl_truth = strtod(declstr, &p_end); float declination = get_mag_declination(lat, lon); - printf("lat: %f lon: %f, dec: %f\n", lat, lon, declination); + printf("lat: %f lon: %f, expected dec: %f, estimated dec: %f\n", lat, lon, declination, decl_truth); + int ret = 0; + + // Fail if the declination differs by more than one degree + float decldiff = fabs(decl_truth - declination); + if (decldiff > 0.5f) { + warnx("declination differs more than 1 degree: difference: %12.8f", decldiff); + ret = 1; + } + + return ret; } From 55112fd0169b941a0d6fab21d4a11ddfcdc0c577 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 20 Dec 2014 18:51:52 +0100 Subject: [PATCH 163/216] Improve mixer test, no firm checks yet --- unittests/mixer_test.cpp | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/unittests/mixer_test.cpp b/unittests/mixer_test.cpp index 06499afd0e..4919e325c8 100644 --- a/unittests/mixer_test.cpp +++ b/unittests/mixer_test.cpp @@ -3,12 +3,17 @@ #include "../../src/systemcmds/tests/tests.h" int main(int argc, char *argv[]) { + + int ret; + warnx("Host execution started"); - char* args[] = {argv[0], "../../ROMFS/px4fmu_common/mixers/IO_pass.mix", - "../../ROMFS/px4fmu_common/mixers/FMU_quad_w.mix"}; + char* args[] = {argv[0], "../ROMFS/px4fmu_common/mixers/IO_pass.mix", + "../ROMFS/px4fmu_common/mixers/FMU_quad_w.mix"}; - test_mixer(3, args); + if (ret = test_mixer(3, args)); test_conv(1, args); + + return 0; } From 3ac4ef4ab30c88bb7c5d4be14219d2be98ec2bbc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 20 Dec 2014 18:52:16 +0100 Subject: [PATCH 164/216] Add S.BUS2 unit test, needs better coverage against logfile --- unittests/sbus2_test.cpp | 18 +++++++++++++----- 1 file changed, 13 insertions(+), 5 deletions(-) diff --git a/unittests/sbus2_test.cpp b/unittests/sbus2_test.cpp index e2c18369c0..ba075f8b3b 100644 --- a/unittests/sbus2_test.cpp +++ b/unittests/sbus2_test.cpp @@ -11,14 +11,20 @@ int main(int argc, char *argv[]) { warnx("SBUS2 test started"); - if (argc < 2) - errx(1, "Need a filename for the input file"); + char *filepath = 0; - warnx("loading data from: %s", argv[1]); + if (argc < 2) { + warnx("Using default input file"); + filepath = "testdata/sbus2_r7008SB.txt"; + } else { + filepath = argv[1]; + } + + warnx("loading data from: %s", filepath); FILE *fp; - fp = fopen(argv[1],"rt"); + fp = fopen(filepath,"rt"); if (!fp) errx(1, "failed opening file"); @@ -47,7 +53,7 @@ int main(int argc, char *argv[]) { while (EOF != (ret = fscanf(fp, "%f,%x,,", &f, &x))) { if (((f - last_time) * 1000 * 1000) > 3000) { partial_frame_count = 0; - warnx("FRAME RESET\n\n"); + //warnx("FRAME RESET\n\n"); } frame[partial_frame_count] = x; @@ -69,8 +75,10 @@ int main(int argc, char *argv[]) { if (ret == EOF) { warnx("Test finished, reached end of file"); + ret = 0; } else { warnx("Test aborted, errno: %d", ret); } + return ret; } From 243b682b7fa8e8b7bd628bbfe415ad0000d40093 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 20 Dec 2014 18:52:44 +0100 Subject: [PATCH 165/216] unittests: SF0x parse test improvements --- unittests/sf0x_test.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/unittests/sf0x_test.cpp b/unittests/sf0x_test.cpp index 82d19fcbe1..1af128bbe4 100644 --- a/unittests/sf0x_test.cpp +++ b/unittests/sf0x_test.cpp @@ -43,7 +43,7 @@ int main(int argc, char *argv[]) for (unsigned l = 0; l < sizeof(lines) / sizeof(lines[0]); l++) { - printf("\n%s", _linebuf); + //printf("\n%s", _linebuf); int parse_ret; @@ -51,11 +51,11 @@ int main(int argc, char *argv[]) parse_ret = sf0x_parser(lines[l][i], _parserbuf, &_parsebuf_index, &state, &dist_m); if (parse_ret == 0) { - printf("\nparsed: %f %s\n", dist_m, (parse_ret == 0) ? "OK" : ""); + //printf("\nparsed: %f %s\n", dist_m, (parse_ret == 0) ? "OK" : ""); } } - printf("%s", lines[l]); + //printf("%s", lines[l]); } From 2e3356694b545b9b15ef8d283c3cbc25e0b0b79d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 20 Dec 2014 18:53:02 +0100 Subject: [PATCH 166/216] unittests: ST24: Improve test return codes --- unittests/st24_test.cpp | 20 ++++++++++++++------ 1 file changed, 14 insertions(+), 6 deletions(-) diff --git a/unittests/st24_test.cpp b/unittests/st24_test.cpp index 25a9355e2e..0c56df1732 100644 --- a/unittests/st24_test.cpp +++ b/unittests/st24_test.cpp @@ -11,15 +11,22 @@ int main(int argc, char *argv[]) { warnx("ST24 test started"); + char* defaultfile = "testdata/st24_data.txt"; + + char* filepath = 0; + if (argc < 2) { - errx(1, "Need a filename for the input file"); + warnx("Too few arguments. Using default file: %s", defaultfile); + filepath = defaultfile; + } else { + filepath = argv[1]; } - warnx("loading data from: %s", argv[1]); + warnx("loading data from: %s", filepath); FILE *fp; - fp = fopen(argv[1], "rt"); + fp = fopen(filepath, "rt"); if (!fp) { errx(1, "failed opening file"); @@ -56,21 +63,22 @@ int main(int argc, char *argv[]) if (!st24_decode(b, &rssi, &rx_count, &channel_count, channels, sizeof(channels) / sizeof(channels[0]))) { - warnx("decoded: %u channels (converted to PPM range)", (unsigned)channel_count); + //warnx("decoded: %u channels (converted to PPM range)", (unsigned)channel_count); for (unsigned i = 0; i < channel_count; i++) { int16_t val = channels[i]; - warnx("channel %u: %d 0x%03X", i, static_cast(val), static_cast(val)); + //warnx("channel %u: %d 0x%03X", i, static_cast(val), static_cast(val)); } } } if (ret == EOF) { warnx("Test finished, reached end of file"); - + ret = 0; } else { warnx("Test aborted, errno: %d", ret); } + return ret; } From 47e071a6f2cd6d6aa66e5d1ec395d7a23f67dad9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 20 Dec 2014 18:54:35 +0100 Subject: [PATCH 167/216] Add Travis CI support, ready for testing --- .travis.yml | 20 ++++++++++++++++++++ 1 file changed, 20 insertions(+) create mode 100644 .travis.yml diff --git a/.travis.yml b/.travis.yml new file mode 100644 index 0000000000..e453a1f240 --- /dev/null +++ b/.travis.yml @@ -0,0 +1,20 @@ + +# Build and autotest script for PX4 Firmware +# http://travis-ci.org + +language: cpp + +before_script: + - sudo add-apt-repository 'ppa:terry.guo/gcc-arm-embedded' -y + - sudo apt-get update -q + - sudo apt-get install gcc-arm-none-eabi python-serial python-argparse + - sudo apt-get install flex bison libncurses5-dev autoconf texinfo build-essential + - sudo apt-get install libtool zlib1g-dev genromfs git wget + +# Clone only as much as we need +git: + depth: 500 + +script: + - make testbuild + - make tests From 3367a67adebd34ca02b1652f817457278c85297c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 20 Dec 2014 19:02:37 +0100 Subject: [PATCH 168/216] Travis file: tabs to spaces --- .travis.yml | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/.travis.yml b/.travis.yml index e453a1f240..f60faebc21 100644 --- a/.travis.yml +++ b/.travis.yml @@ -5,16 +5,16 @@ language: cpp before_script: - - sudo add-apt-repository 'ppa:terry.guo/gcc-arm-embedded' -y - - sudo apt-get update -q - - sudo apt-get install gcc-arm-none-eabi python-serial python-argparse - - sudo apt-get install flex bison libncurses5-dev autoconf texinfo build-essential - - sudo apt-get install libtool zlib1g-dev genromfs git wget + - sudo add-apt-repository 'ppa:terry.guo/gcc-arm-embedded' -y + - sudo apt-get update -q + - sudo apt-get install gcc-arm-none-eabi python-serial python-argparse + - sudo apt-get install flex bison libncurses5-dev autoconf texinfo build-essential + - sudo apt-get install libtool zlib1g-dev genromfs git wget # Clone only as much as we need git: - depth: 500 + depth: 500 script: - - make testbuild - - make tests + - make testbuild + - make tests From 2fe344099027fcdedbd7f76e526584d856f60f7c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 20 Dec 2014 19:06:25 +0100 Subject: [PATCH 169/216] Add Travis badge --- README.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/README.md b/README.md index 532e9aab25..74e887d6a0 100644 --- a/README.md +++ b/README.md @@ -1,5 +1,7 @@ ## PX4 Aerial Middleware and Flight Control Stack ## +[![Travis CI](https://travis-ci.org/PX4/Firmware.svg](https://travis-ci.org/PX4/Firmware) + [![Gitter](https://badges.gitter.im/Join%20Chat.svg)](https://gitter.im/PX4/Firmware?utm_source=badge&utm_medium=badge&utm_campaign=pr-badge&utm_content=badge) * Official Website: http://px4.io From a20b8d037bd91fddaebba7a61353c584dcf99314 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 20 Dec 2014 19:22:53 +0100 Subject: [PATCH 170/216] PX4IO firmware: Add required include. --- src/modules/px4iofirmware/protocol.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index c7e9ae3eb3..b25a9507f6 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -33,6 +33,8 @@ #pragma once +#include + /** * @file protocol.h * From 54fb16535d00c7b657d587c58bbf1814dd3474bd Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 20 Dec 2014 19:23:18 +0100 Subject: [PATCH 171/216] README: Fix up build status badge --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 74e887d6a0..a90bbc3140 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,6 @@ ## PX4 Aerial Middleware and Flight Control Stack ## -[![Travis CI](https://travis-ci.org/PX4/Firmware.svg](https://travis-ci.org/PX4/Firmware) +[![Build Status](https://travis-ci.org/PX4/Firmware.svg?branch=master)](https://travis-ci.org/PX4/Firmware) [![Gitter](https://badges.gitter.im/Join%20Chat.svg)](https://gitter.im/PX4/Firmware?utm_source=badge&utm_medium=badge&utm_campaign=pr-badge&utm_content=badge) From 90ff0f24bc8ff7bb7ee6c9e79d56296f4e03b82b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 20 Dec 2014 19:30:49 +0100 Subject: [PATCH 172/216] Updated NuttX --- NuttX | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/NuttX b/NuttX index c7b0638592..b66de65069 160000 --- a/NuttX +++ b/NuttX @@ -1 +1 @@ -Subproject commit c7b06385926349e10b3739314d1d56eec7efb8be +Subproject commit b66de6506941dc7628efcf65e5543c0e3cb05a8f From e9f139234d79eb23c3c5731b0fa4e9008265bf31 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 20 Dec 2014 19:31:13 +0100 Subject: [PATCH 173/216] Output GCC version --- .travis.yml | 3 +++ 1 file changed, 3 insertions(+) diff --git a/.travis.yml b/.travis.yml index f60faebc21..33660d165e 100644 --- a/.travis.yml +++ b/.travis.yml @@ -11,6 +11,9 @@ before_script: - sudo apt-get install flex bison libncurses5-dev autoconf texinfo build-essential - sudo apt-get install libtool zlib1g-dev genromfs git wget +# Output GCC version + arm-none-eabi-gcc --version + # Clone only as much as we need git: depth: 500 From 752198352ebaa397caeb1c8cd18454231ffed389 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 20 Dec 2014 19:32:43 +0100 Subject: [PATCH 174/216] IO firmware: Change to inttypes header --- src/modules/px4iofirmware/protocol.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index b25a9507f6..bd777428f4 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -33,7 +33,7 @@ #pragma once -#include +#include /** * @file protocol.h From 99c6586e665417beeb1f1b1f832de333aadebe85 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 20 Dec 2014 19:33:38 +0100 Subject: [PATCH 175/216] Fix Travis CI config file --- .travis.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.travis.yml b/.travis.yml index 33660d165e..4d697a83bb 100644 --- a/.travis.yml +++ b/.travis.yml @@ -12,7 +12,7 @@ before_script: - sudo apt-get install libtool zlib1g-dev genromfs git wget # Output GCC version - arm-none-eabi-gcc --version + - arm-none-eabi-gcc --version # Clone only as much as we need git: From 051c0c12a2e4f9fbdacfa338c98d485e52107332 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 20 Dec 2014 19:45:42 +0100 Subject: [PATCH 176/216] Travis CI: Pick right GCC version --- .travis.yml | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/.travis.yml b/.travis.yml index 4d697a83bb..6c8557c4de 100644 --- a/.travis.yml +++ b/.travis.yml @@ -7,9 +7,8 @@ language: cpp before_script: - sudo add-apt-repository 'ppa:terry.guo/gcc-arm-embedded' -y - sudo apt-get update -q - - sudo apt-get install gcc-arm-none-eabi python-serial python-argparse - - sudo apt-get install flex bison libncurses5-dev autoconf texinfo build-essential - - sudo apt-get install libtool zlib1g-dev genromfs git wget + - sudo apt-get install gcc-arm-none-eabi=4-8-2014q2-0saucy9 python-serial python-argparse + - sudo apt-get install flex bison libncurses5-dev autoconf texinfo build-essential libtool zlib1g-dev genromfs git wget # Output GCC version - arm-none-eabi-gcc --version From 94ad98d59bab98702b790980a86c2b04d3b084ec Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 20 Dec 2014 19:55:24 +0100 Subject: [PATCH 177/216] Manually install a known GCC version --- .travis.yml | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/.travis.yml b/.travis.yml index 6c8557c4de..eabb3ea64c 100644 --- a/.travis.yml +++ b/.travis.yml @@ -5,10 +5,19 @@ language: cpp before_script: - - sudo add-apt-repository 'ppa:terry.guo/gcc-arm-embedded' -y + #- sudo add-apt-repository 'ppa:terry.guo/gcc-arm-embedded' -y - sudo apt-get update -q - - sudo apt-get install gcc-arm-none-eabi=4-8-2014q2-0saucy9 python-serial python-argparse + # Note: we do not want a random, auto-updated GCC version - sudo apt-get install gcc-arm-none-eabi + - sudo apt-get install ia32-libs python-serial python-argparse grep - sudo apt-get install flex bison libncurses5-dev autoconf texinfo build-essential libtool zlib1g-dev genromfs git wget + - pushd . + - cd ~ + - wget https://launchpadlibrarian.net/174121628/gcc-arm-none-eabi-4_7-2014q2-20140408-linux.tar.bz2 + - tar -jxf gcc-arm-none-eabi-4_7-2014q2-20140408-linux.tar.bz2 + - exportline="export PATH=$HOME/gcc-arm-none-eabi-4_7-2014q2/bin:\$PATH" + - if grep -Fxq "$exportline" ~/.profile; then echo nothing to do ; else echo $exportline >> ~/.profile; fi + - . ~/.profile + - popd # Output GCC version - arm-none-eabi-gcc --version From d627a1e6638855c09f2d957d70e6e1a31a477e25 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 20 Dec 2014 19:58:45 +0100 Subject: [PATCH 178/216] Travis CI: Install 32 bit support libs first --- .travis.yml | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/.travis.yml b/.travis.yml index eabb3ea64c..1ad5df2927 100644 --- a/.travis.yml +++ b/.travis.yml @@ -8,7 +8,10 @@ before_script: #- sudo add-apt-repository 'ppa:terry.guo/gcc-arm-embedded' -y - sudo apt-get update -q # Note: we do not want a random, auto-updated GCC version - sudo apt-get install gcc-arm-none-eabi - - sudo apt-get install ia32-libs python-serial python-argparse grep + - sudo apt-get install ia32-libs-multiarch:i386 + - sudo apt-get install ia32-libs-multiarch + - sudo apt-get install ia32-libs + - sudo apt-get install python-serial python-argparse grep - sudo apt-get install flex bison libncurses5-dev autoconf texinfo build-essential libtool zlib1g-dev genromfs git wget - pushd . - cd ~ From 7adb737b99501dd6bab03b1151660dc564e7aa73 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 20 Dec 2014 20:01:09 +0100 Subject: [PATCH 179/216] Travis CI: Yet another pass at 32 bit support libs --- .travis.yml | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/.travis.yml b/.travis.yml index 1ad5df2927..2d1b29a762 100644 --- a/.travis.yml +++ b/.travis.yml @@ -8,9 +8,7 @@ before_script: #- sudo add-apt-repository 'ppa:terry.guo/gcc-arm-embedded' -y - sudo apt-get update -q # Note: we do not want a random, auto-updated GCC version - sudo apt-get install gcc-arm-none-eabi - - sudo apt-get install ia32-libs-multiarch:i386 - - sudo apt-get install ia32-libs-multiarch - - sudo apt-get install ia32-libs + - sudo apt-get install libc6:i386 libgcc1:i386 gcc-4.6-base:i386 libstdc++5:i386 libstdc++6:i386 - sudo apt-get install python-serial python-argparse grep - sudo apt-get install flex bison libncurses5-dev autoconf texinfo build-essential libtool zlib1g-dev genromfs git wget - pushd . From 178a2e82c294081fffcae4ee0c67fa9962f82985 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 20 Dec 2014 21:49:52 +0100 Subject: [PATCH 180/216] Travis CI: Enable release integration --- .travis.yml | 17 +++++++++++------ 1 file changed, 11 insertions(+), 6 deletions(-) diff --git a/.travis.yml b/.travis.yml index 2d1b29a762..1dcfd16566 100644 --- a/.travis.yml +++ b/.travis.yml @@ -5,9 +5,7 @@ language: cpp before_script: - #- sudo add-apt-repository 'ppa:terry.guo/gcc-arm-embedded' -y - sudo apt-get update -q - # Note: we do not want a random, auto-updated GCC version - sudo apt-get install gcc-arm-none-eabi - sudo apt-get install libc6:i386 libgcc1:i386 gcc-4.6-base:i386 libstdc++5:i386 libstdc++6:i386 - sudo apt-get install python-serial python-argparse grep - sudo apt-get install flex bison libncurses5-dev autoconf texinfo build-essential libtool zlib1g-dev genromfs git wget @@ -19,14 +17,21 @@ before_script: - if grep -Fxq "$exportline" ~/.profile; then echo nothing to do ; else echo $exportline >> ~/.profile; fi - . ~/.profile - popd - -# Output GCC version - arm-none-eabi-gcc --version - -# Clone only as much as we need git: depth: 500 script: - make testbuild - make tests + + deploy: + provider: releases + api_key: + secure: cdHWLRBxA5UlYpOS0Sp891QK7PFmMgQ5ZWs1aPt+sw0rIrowyWMHCwXNBEdUqaExHYNYgXCUDI0EzNgfB7ZcR63Qv1MQeoyamV4jsxlyAqDqmxNtWO82S6RhHGeMLk26VgFKzynVcEk1IYlQP2nqzMQLdu+jTrngERuAIrCdRuc= + file: + - Images/px4fmu-v1_default.px4 + - Images/px4fmu-v2_default.px4 + - Images/aerocore_default.px4 + on: + repo: PX4/Firmware From a2e662f05ca4c65234bff2cf27e8d85666e277c0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 21 Dec 2014 00:50:06 +0100 Subject: [PATCH 181/216] Travis CI: Fix YML format --- .travis.yml | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/.travis.yml b/.travis.yml index 1dcfd16566..7005401c16 100644 --- a/.travis.yml +++ b/.travis.yml @@ -18,6 +18,7 @@ before_script: - . ~/.profile - popd - arm-none-eabi-gcc --version + git: depth: 500 @@ -25,13 +26,13 @@ script: - make testbuild - make tests - deploy: +deploy: provider: releases api_key: secure: cdHWLRBxA5UlYpOS0Sp891QK7PFmMgQ5ZWs1aPt+sw0rIrowyWMHCwXNBEdUqaExHYNYgXCUDI0EzNgfB7ZcR63Qv1MQeoyamV4jsxlyAqDqmxNtWO82S6RhHGeMLk26VgFKzynVcEk1IYlQP2nqzMQLdu+jTrngERuAIrCdRuc= - file: - - Images/px4fmu-v1_default.px4 - - Images/px4fmu-v2_default.px4 - - Images/aerocore_default.px4 + file: "Images/px4fmu-v2_default.px4" + skip_cleanup: true on: + tags: true + all_branches: true repo: PX4/Firmware From 54b68b73fd919d9fa376cca5297bb6ff6ea631f5 Mon Sep 17 00:00:00 2001 From: "M.H.Kabir" Date: Sun, 21 Dec 2014 09:03:18 +0530 Subject: [PATCH 182/216] Remove vel reset --- .../position_estimator_inav/position_estimator_inav_main.c | 4 ---- 1 file changed, 4 deletions(-) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 296919c042..0af01cba1a 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -1052,10 +1052,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) landed = false; landed_time = 0; } - /* reset xy velocity estimates when landed */ - x_est[1] = 0.0f; - y_est[1] = 0.0f; - } else { if (alt_disp2 < land_disp2 && thrust < params.land_thr) { if (landed_time == 0) { From 032b25c81baf8473cc2b909d8d6049437b3a5ed3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 21 Dec 2014 12:09:16 +0100 Subject: [PATCH 183/216] Creating Firmware ZIP file and ignoring it in GIT --- .gitignore | 1 + Makefile | 11 ++++++----- 2 files changed, 7 insertions(+), 5 deletions(-) diff --git a/.gitignore b/.gitignore index 8b09e4783f..5328de0098 100644 --- a/.gitignore +++ b/.gitignore @@ -38,3 +38,4 @@ tags .pydevproject .ropeproject *.orig +Firmware.zip diff --git a/Makefile b/Makefile index cfc8159b1f..0f68e7b4be 100644 --- a/Makefile +++ b/Makefile @@ -229,6 +229,7 @@ updatesubmodules: # testbuild: $(Q) (cd $(PX4_BASE) && $(MAKE) distclean && $(MAKE) archives && $(MAKE) -j8) + $(Q) (zip -r Firmware.zip $(PX4_BASE)/Images) # # Unittest targets. Builds and runs the host-level @@ -244,14 +245,14 @@ tests: # .PHONY: clean clean: - $(Q) $(RMDIR) $(BUILD_DIR)*.build - $(Q) $(REMOVE) $(IMAGE_DIR)*.px4 + $(Q) $(RMDIR) $(BUILD_DIR)*.build > /dev/null + $(Q) $(REMOVE) $(IMAGE_DIR)*.px4 > /dev/null .PHONY: distclean distclean: clean - $(Q) $(REMOVE) $(ARCHIVE_DIR)*.export - $(Q) $(MAKE) -C $(NUTTX_SRC) -r $(MQUIET) distclean - $(Q) (cd $(NUTTX_SRC)/configs && $(FIND) . -maxdepth 1 -type l -delete) + $(Q) $(REMOVE) $(ARCHIVE_DIR)*.export > /dev/null + $(Q) $(MAKE) -C $(NUTTX_SRC) -r $(MQUIET) distclean > /dev/null + $(Q) (cd $(NUTTX_SRC)/configs && $(FIND) . -maxdepth 1 -type l -delete) > /dev/null # # Print some help text From 42a70da401f487eaa220a1b41da415b5eb5bb3fb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 21 Dec 2014 12:09:47 +0100 Subject: [PATCH 184/216] Travis CI: Teach Travis how to upload all builds to AWS --- .travis.yml | 17 ++++++++++++++--- 1 file changed, 14 insertions(+), 3 deletions(-) diff --git a/.travis.yml b/.travis.yml index 7005401c16..1632e1674b 100644 --- a/.travis.yml +++ b/.travis.yml @@ -1,4 +1,3 @@ - # Build and autotest script for PX4 Firmware # http://travis-ci.org @@ -7,7 +6,7 @@ language: cpp before_script: - sudo apt-get update -q - sudo apt-get install libc6:i386 libgcc1:i386 gcc-4.6-base:i386 libstdc++5:i386 libstdc++6:i386 - - sudo apt-get install python-serial python-argparse grep + - sudo apt-get install python-serial python-argparse grep zip - sudo apt-get install flex bison libncurses5-dev autoconf texinfo build-essential libtool zlib1g-dev genromfs git wget - pushd . - cd ~ @@ -30,9 +29,21 @@ deploy: provider: releases api_key: secure: cdHWLRBxA5UlYpOS0Sp891QK7PFmMgQ5ZWs1aPt+sw0rIrowyWMHCwXNBEdUqaExHYNYgXCUDI0EzNgfB7ZcR63Qv1MQeoyamV4jsxlyAqDqmxNtWO82S6RhHGeMLk26VgFKzynVcEk1IYlQP2nqzMQLdu+jTrngERuAIrCdRuc= - file: "Images/px4fmu-v2_default.px4" + file: + paths: + - "Firmware.zip" skip_cleanup: true on: tags: true all_branches: true repo: PX4/Firmware + +addons: + artifacts: + paths: + - "Firmware.zip" + key: + secure: j4y9x9KXUiarGrnpFBLPIkEKIH8X6oSRUO61TwxTOamsE0eEKnIaCz1Xq83q7DoqzomHBD3qXAFPV9dhLr1zdKEPJDIyV45GVD4ClIQIzh/P3Uc7kDNxKzdmxY12SH6D0orMpC4tCf1sNK7ETepltWfcnjaDk1Rjs9+TVY7LuzM= + secret: + secure: CJC7VPGtEhJu8Pix85iPF8xUvMPZvTgnHyd9MrSlPKCFFMrlgz9eMT0WWW/TPQ+s4LPwJIfEQx2Q0BRT5tOXuvsTLuOG68mplVddhTWbHb0m0qTQErXFHEppvW4ayuSdeLJ4TjTWphBVainL0mcLLRwQfuAJJDDs/sGan3WrG+Y= + bucket: travisbinaries From 388350108aa0539f8d3a9407d8970390a1dc36eb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 21 Dec 2014 12:10:18 +0100 Subject: [PATCH 185/216] FMUv1: Ignore float suffixes in NuttX --- nuttx-configs/px4fmu-v1/nsh/Make.defs | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/nuttx-configs/px4fmu-v1/nsh/Make.defs b/nuttx-configs/px4fmu-v1/nsh/Make.defs index e7318e5196..f055cfddf8 100644 --- a/nuttx-configs/px4fmu-v1/nsh/Make.defs +++ b/nuttx-configs/px4fmu-v1/nsh/Make.defs @@ -139,8 +139,7 @@ ARCHCWARNINGS = $(ARCHWARNINGS) \ -Wold-style-declaration \ -Wmissing-parameter-type \ -Wmissing-prototypes \ - -Wnested-externs \ - -Wunsuffixed-float-constants + -Wnested-externs ARCHWARNINGSXX = $(ARCHWARNINGS) \ -Wno-psabi ARCHDEFINES = From 648e14b9c33cf5521c85dc778121ae025f358d39 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 21 Dec 2014 12:10:28 +0100 Subject: [PATCH 186/216] FMUv2: Ignore float suffixes in NuttX --- nuttx-configs/px4fmu-v2/nsh/Make.defs | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/nuttx-configs/px4fmu-v2/nsh/Make.defs b/nuttx-configs/px4fmu-v2/nsh/Make.defs index f3ce53b4ae..f660deeca7 100644 --- a/nuttx-configs/px4fmu-v2/nsh/Make.defs +++ b/nuttx-configs/px4fmu-v2/nsh/Make.defs @@ -139,8 +139,7 @@ ARCHCWARNINGS = $(ARCHWARNINGS) \ -Wold-style-declaration \ -Wmissing-parameter-type \ -Wmissing-prototypes \ - -Wnested-externs \ - -Wunsuffixed-float-constants + -Wnested-externs ARCHWARNINGSXX = $(ARCHWARNINGS) \ -Wno-psabi ARCHDEFINES = From dc469cc51aa66568e4df1f07a331667638cdfb3e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 21 Dec 2014 12:10:41 +0100 Subject: [PATCH 187/216] IOv1: Ignore float suffixes in NuttX --- nuttx-configs/px4io-v1/nsh/Make.defs | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/nuttx-configs/px4io-v1/nsh/Make.defs b/nuttx-configs/px4io-v1/nsh/Make.defs index 7a0792ff64..e96be6a1f2 100644 --- a/nuttx-configs/px4io-v1/nsh/Make.defs +++ b/nuttx-configs/px4io-v1/nsh/Make.defs @@ -128,8 +128,7 @@ ARCHCWARNINGS = $(ARCHWARNINGS) \ -Wold-style-declaration \ -Wmissing-parameter-type \ -Wmissing-prototypes \ - -Wnested-externs \ - -Wunsuffixed-float-constants + -Wnested-externs ARCHWARNINGSXX = $(ARCHWARNINGS) ARCHDEFINES = ARCHPICFLAGS = -fpic -msingle-pic-base -mpic-register=r10 From 923346c8ed03374aaa8883309eb45d70730b0e0b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 21 Dec 2014 12:10:51 +0100 Subject: [PATCH 188/216] IOv2: Ignore float suffixes in NuttX --- nuttx-configs/px4io-v2/nsh/Make.defs | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/nuttx-configs/px4io-v2/nsh/Make.defs b/nuttx-configs/px4io-v2/nsh/Make.defs index 1717464d2f..4a8df27388 100644 --- a/nuttx-configs/px4io-v2/nsh/Make.defs +++ b/nuttx-configs/px4io-v2/nsh/Make.defs @@ -128,8 +128,7 @@ ARCHCWARNINGS = $(ARCHWARNINGS) \ -Wold-style-declaration \ -Wmissing-parameter-type \ -Wmissing-prototypes \ - -Wnested-externs \ - -Wunsuffixed-float-constants + -Wnested-externs ARCHWARNINGSXX = $(ARCHWARNINGS) ARCHDEFINES = ARCHPICFLAGS = -fpic -msingle-pic-base -mpic-register=r10 From 84bf1814fe8ec55026b48aa09bd9e4be1ddd6db3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 21 Dec 2014 12:24:51 +0100 Subject: [PATCH 189/216] Travis CI: Provide the right AWS region --- .travis.yml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/.travis.yml b/.travis.yml index 1632e1674b..3858f144e9 100644 --- a/.travis.yml +++ b/.travis.yml @@ -47,3 +47,5 @@ addons: secret: secure: CJC7VPGtEhJu8Pix85iPF8xUvMPZvTgnHyd9MrSlPKCFFMrlgz9eMT0WWW/TPQ+s4LPwJIfEQx2Q0BRT5tOXuvsTLuOG68mplVddhTWbHb0m0qTQErXFHEppvW4ayuSdeLJ4TjTWphBVainL0mcLLRwQfuAJJDDs/sGan3WrG+Y= bucket: travisbinaries + region: eu-central-1 + # endpoint: XXX might be required as well From 3041b913e5c6e9f14040d210d86ddd9875ef594e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 21 Dec 2014 12:56:40 +0100 Subject: [PATCH 190/216] Travis CI: Fix up AWS paths --- .travis.yml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/.travis.yml b/.travis.yml index 3858f144e9..1ddbff2338 100644 --- a/.travis.yml +++ b/.travis.yml @@ -46,6 +46,6 @@ addons: secure: j4y9x9KXUiarGrnpFBLPIkEKIH8X6oSRUO61TwxTOamsE0eEKnIaCz1Xq83q7DoqzomHBD3qXAFPV9dhLr1zdKEPJDIyV45GVD4ClIQIzh/P3Uc7kDNxKzdmxY12SH6D0orMpC4tCf1sNK7ETepltWfcnjaDk1Rjs9+TVY7LuzM= secret: secure: CJC7VPGtEhJu8Pix85iPF8xUvMPZvTgnHyd9MrSlPKCFFMrlgz9eMT0WWW/TPQ+s4LPwJIfEQx2Q0BRT5tOXuvsTLuOG68mplVddhTWbHb0m0qTQErXFHEppvW4ayuSdeLJ4TjTWphBVainL0mcLLRwQfuAJJDDs/sGan3WrG+Y= - bucket: travisbinaries - region: eu-central-1 - # endpoint: XXX might be required as well + bucket: px4-travis + region: us-east-1 + endpoint: s3-website-us-east-1.amazonaws.com From 20d75dc657429cab4c7d02518f15bc1468a91a17 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 21 Dec 2014 13:07:32 +0100 Subject: [PATCH 191/216] Travis CI: Fold log output --- .travis.yml | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/.travis.yml b/.travis.yml index 1ddbff2338..482fbb80ea 100644 --- a/.travis.yml +++ b/.travis.yml @@ -22,8 +22,15 @@ git: depth: 500 script: - - make testbuild + - echo 'Running Tests..' && echo -en 'travis_fold:start:script.1\\r' - make tests + - echo -en 'travis_fold:end:script.1\\r' + - echo 'Building NuttX..' && echo -en 'travis_fold:start:script.1\\r' + - make archives + - echo -en 'travis_fold:end:script.1\\r' + - echo 'Building Firmware..' && echo -en 'travis_fold:start:script.1\\r' + - make -j6 + - echo -en 'travis_fold:end:script.1\\r' deploy: provider: releases From 6ae26b44ed71759abd1ff7f11b94c230a5106938 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 21 Dec 2014 13:18:01 +0100 Subject: [PATCH 192/216] Travis CI: Zip only relevant files --- .travis.yml | 1 + 1 file changed, 1 insertion(+) diff --git a/.travis.yml b/.travis.yml index 482fbb80ea..29c95c0d83 100644 --- a/.travis.yml +++ b/.travis.yml @@ -31,6 +31,7 @@ script: - echo 'Building Firmware..' && echo -en 'travis_fold:start:script.1\\r' - make -j6 - echo -en 'travis_fold:end:script.1\\r' + - zip Firmware.zip Images/*.px4 deploy: provider: releases From b4dacfba3a033d4d5c810c99d085e9444a360601 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 21 Dec 2014 13:23:26 +0100 Subject: [PATCH 193/216] Travis CI: Fix fold syntax --- .travis.yml | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/.travis.yml b/.travis.yml index 29c95c0d83..21f910f83b 100644 --- a/.travis.yml +++ b/.travis.yml @@ -25,12 +25,12 @@ script: - echo 'Running Tests..' && echo -en 'travis_fold:start:script.1\\r' - make tests - echo -en 'travis_fold:end:script.1\\r' - - echo 'Building NuttX..' && echo -en 'travis_fold:start:script.1\\r' + - echo 'Building NuttX..' && echo -en 'travis_fold:start:script.2\\r' - make archives - - echo -en 'travis_fold:end:script.1\\r' - - echo 'Building Firmware..' && echo -en 'travis_fold:start:script.1\\r' + - echo -en 'travis_fold:end:script.2\\r' + - echo 'Building Firmware..' && echo -en 'travis_fold:start:script.3\\r' - make -j6 - - echo -en 'travis_fold:end:script.1\\r' + - echo -en 'travis_fold:end:script.3\\r' - zip Firmware.zip Images/*.px4 deploy: From 1910b7e88be1bd92dc47e5b0457d50b0274f6297 Mon Sep 17 00:00:00 2001 From: Grant Morphett Date: Sat, 20 Dec 2014 15:21:16 +1100 Subject: [PATCH 194/216] MPU6000: Add regdump command Add mpu6000 regdump command for debugging mpu6000. --- src/drivers/mpu6000/mpu6000.cpp | 44 +++++++++++++++++++++++++++++++-- 1 file changed, 42 insertions(+), 2 deletions(-) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index a95e041a11..c9c27892f4 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -194,6 +194,8 @@ public: */ void print_info(); + void print_registers(); + protected: virtual int probe(); @@ -1414,6 +1416,21 @@ MPU6000::print_info() _gyro_reports->print_info("gyro queue"); } +void +MPU6000::print_registers() +{ + printf("MPU6000 registers\n"); + for (uint8_t reg=MPUREG_PRODUCT_ID; reg<=108; reg++) { + uint8_t v = read_reg(reg); + printf("%02x:%02x ",(unsigned)reg, (unsigned)v); + if ((reg - (MPUREG_PRODUCT_ID-1)) % 13 == 0) { + printf("\n"); + } + } + printf("\n"); +} + + MPU6000_gyro::MPU6000_gyro(MPU6000 *parent, const char *path) : CDev("MPU6000_gyro", path), _parent(parent), @@ -1479,6 +1496,7 @@ void start(bool, enum Rotation); void test(bool); void reset(bool); void info(bool); +void regdump(bool); void usage(); /** @@ -1654,10 +1672,26 @@ info(bool external_bus) exit(0); } +/** + * Dump the register information + */ +void +regdump(bool external_bus) +{ + MPU6000 **g_dev_ptr = external_bus?&g_dev_ext:&g_dev_int; + if (*g_dev_ptr == nullptr) + errx(1, "driver not running"); + + printf("regdump @ %p\n", *g_dev_ptr); + (*g_dev_ptr)->print_registers(); + + exit(0); +} + void usage() { - warnx("missing command: try 'start', 'info', 'test', 'reset'"); + warnx("missing command: try 'start', 'info', 'test', 'reset', 'regdump'"); warnx("options:"); warnx(" -X (external bus)"); warnx(" -R rotation"); @@ -1714,5 +1748,11 @@ mpu6000_main(int argc, char *argv[]) if (!strcmp(verb, "info")) mpu6000::info(external_bus); - errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'"); + /* + * Print register information. + */ + if (!strcmp(verb, "regdump")) + mpu6000::regdump(external_bus); + + errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info' or 'regdump'"); } From b1f462a26638f252b0849083f1c3a81c52d49053 Mon Sep 17 00:00:00 2001 From: Ban Siesta Date: Sun, 21 Dec 2014 14:09:04 +0000 Subject: [PATCH 195/216] geofence: don't fall over lines containing just a LF --- src/modules/navigator/geofence.cpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index 0f431ded26..406137169b 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -279,8 +279,14 @@ Geofence::loadFromFile(const char *filename) while((textStart < sizeof(line)/sizeof(char)) && isspace(line[textStart])) textStart++; /* if the line starts with #, skip */ - if (line[textStart] == commentChar) + if (line[textStart] == commentChar) { continue; + } + + /* if there is only a linefeed, skip it */ + if (line[0] == '\n') { + continue; + } if (gotVertical) { /* Parse the line as a geofence point */ From 82383533c169e44ac769ce77f9e8ddfbd3082ed9 Mon Sep 17 00:00:00 2001 From: Ban Siesta Date: Sun, 21 Dec 2014 14:09:30 +0000 Subject: [PATCH 196/216] geofence: be more verbose if import fails --- src/modules/navigator/geofence.cpp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index 406137169b..4482fb36b7 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -297,8 +297,10 @@ Geofence::loadFromFile(const char *filename) /* Handle degree minute second format */ float lat_d, lat_m, lat_s, lon_d, lon_m, lon_s; - if (sscanf(line, "DMS %f %f %f %f %f %f", &lat_d, &lat_m, &lat_s, &lon_d, &lon_m, &lon_s) != 6) + if (sscanf(line, "DMS %f %f %f %f %f %f", &lat_d, &lat_m, &lat_s, &lon_d, &lon_m, &lon_s) != 6) { + warnx("Scanf to parse DMS geofence vertex failed."); return ERROR; + } // warnx("Geofence DMS: %.5f %.5f %.5f ; %.5f %.5f %.5f", (double)lat_d, (double)lat_m, (double)lat_s, (double)lon_d, (double)lon_m, (double)lon_s); @@ -307,9 +309,10 @@ Geofence::loadFromFile(const char *filename) } else { /* Handle decimal degree format */ - - if (sscanf(line, "%f %f", &(vertex.lat), &(vertex.lon)) != 2) + if (sscanf(line, "%f %f", &(vertex.lat), &(vertex.lon)) != 2) { + warnx("Scanf to parse geofence vertex failed."); return ERROR; + } } if (dm_write(DM_KEY_FENCE_POINTS, pointCounter, DM_PERSIST_POWER_ON_RESET, &vertex, sizeof(vertex)) != sizeof(vertex)) From 750ef5007fdf97a014bbf79fe926c6c6591da02a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 21 Dec 2014 16:26:54 +0100 Subject: [PATCH 197/216] Improve README --- README.md | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index a90bbc3140..302ea08c32 100644 --- a/README.md +++ b/README.md @@ -7,8 +7,9 @@ * Official Website: http://px4.io * License: BSD 3-clause (see LICENSE.md) * Supported airframes: - * Multicopters - * Fixed wing + * [Multicopters](http://px4.io/platforms/multicopters/start) + * [Fixed wing](http://px4.io/platforms/planes/start) + * [VTOL](http://px4.io/platforms/vtol/start) * Binaries (always up-to-date from master): * [Downloads](https://pixhawk.org/downloads) * Mailing list: [Google Groups](http://groups.google.com/group/px4users) From 4d85373b3504c8bce1c3fa19eb531926c3bca6c8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 21 Dec 2014 17:01:26 +0100 Subject: [PATCH 198/216] Travis CI: Test flight tester email notification --- .travis.yml | 23 +++++++++++++++++++++-- 1 file changed, 21 insertions(+), 2 deletions(-) diff --git a/.travis.yml b/.travis.yml index 21f910f83b..f53500a3d0 100644 --- a/.travis.yml +++ b/.travis.yml @@ -5,8 +5,11 @@ language: cpp before_script: - sudo apt-get update -q +# Travis specific tools + - sudo apt-get install s3cmd grep zip +# General toolchain dependencies - sudo apt-get install libc6:i386 libgcc1:i386 gcc-4.6-base:i386 libstdc++5:i386 libstdc++6:i386 - - sudo apt-get install python-serial python-argparse grep zip + - sudo apt-get install python-serial python-argparse - sudo apt-get install flex bison libncurses5-dev autoconf texinfo build-essential libtool zlib1g-dev genromfs git wget - pushd . - cd ~ @@ -16,12 +19,23 @@ before_script: - if grep -Fxq "$exportline" ~/.profile; then echo nothing to do ; else echo $exportline >> ~/.profile; fi - . ~/.profile - popd - - arm-none-eabi-gcc --version git: depth: 500 +env: + global: +# AWS KEY: $PX4_AWS_KEY + - secure: "XknnZHWBbpHbN4f3fuAVwUztdLIu8ej4keC3aQSDofo3uw8AFEzojfsQsN9u77ShWSIV4iYJWh9C9ALkCx7TocJ+xYjiboo10YhM9lH/8u+EXjYWG6GHS8ua0wkir+cViSxoLNaMtmcb/rPTicJecAGANxLsIHyBAgTL3fkbLSA=" +# AWS SECRET: $PX4_AWS_SECRET + - secure: "h6oajlW68dWIr+wZhO58Dv6e68dZHrBLVA6lPXZmheFQBW6Xam1HuLGA0LOW6cL9TnrAsOZ8g4goB58eMQnMEijFZKi3mhRwZhd/Xjq/ZGJOWBUrLoQHZUw2dQk5ja5vmUlKEoQnFZjDuMjx8KfX5ZMNy8A3yssWZtJYHD8c+bk=" + - PX4_AWS_BUCKET=px4-travis + - PX4_EMAIL_SUBJECT="Travis CI result" +# Email address: $PX4_EMAIL + - secure: "ei3hKAw6Pk+vEkQBI5Y2Ak74BRAaXcK2UHVnVadviBHI4EVPwn1YGP6A4Y0wnLe4U7ETTl0UiijRoVxyDW0Mq896Pv0siw02amNpjSZZYu+RfN1+//MChB48OxsLDirUdHVrULhl/bOARM02h2Bg28jDE2g7IqmJwg3em3oMbjU=" + script: + - arm-none-eabi-gcc --version - echo 'Running Tests..' && echo -en 'travis_fold:start:script.1\\r' - make tests - echo -en 'travis_fold:end:script.1\\r' @@ -33,6 +47,11 @@ script: - echo -en 'travis_fold:end:script.3\\r' - zip Firmware.zip Images/*.px4 +# We use an encrypted env variable to ensure this only executes when artifacts are uploaded. +after_script: + - /usr/bin/mail -s "$SUBJECT ($TRAVIS_COMMIT)" "$PX4_EMAIL" < Branch $TRAVIS_BRANCH or pull request $TRAVIS_PULL_REQUEST ready for flight testin. Files available at https://px4-travis.s3.amazonaws.com/PX4/Firmware/$TRAVIS_BUILD_NUMBER/$TRAVIS_BUILD_NUMBER.1/Firmware.zip + #- s3cmd put --acl-public --guess-mime-type --config=.s3cfg Firmware.zip s3://s3-website-us-east-1.amazonaws.com/#$TRAVIS_JOB_ID/ + deploy: provider: releases api_key: From 8df43524c0c456ec2a87979971d8dcb1fd417da2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 21 Dec 2014 17:11:30 +0100 Subject: [PATCH 199/216] Travis CI: Fix email line --- .travis.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.travis.yml b/.travis.yml index f53500a3d0..ef267d0d58 100644 --- a/.travis.yml +++ b/.travis.yml @@ -49,7 +49,7 @@ script: # We use an encrypted env variable to ensure this only executes when artifacts are uploaded. after_script: - - /usr/bin/mail -s "$SUBJECT ($TRAVIS_COMMIT)" "$PX4_EMAIL" < Branch $TRAVIS_BRANCH or pull request $TRAVIS_PULL_REQUEST ready for flight testin. Files available at https://px4-travis.s3.amazonaws.com/PX4/Firmware/$TRAVIS_BUILD_NUMBER/$TRAVIS_BUILD_NUMBER.1/Firmware.zip + - /usr/bin/mail -s "$SUBJECT ($TRAVIS_COMMIT)" "$PX4_EMAIL" < "Branch $TRAVIS_BRANCH or pull request $TRAVIS_PULL_REQUEST ready for flight testin. Files available at https://px4-travis.s3.amazonaws.com/PX4/Firmware/$TRAVIS_BUILD_NUMBER/$TRAVIS_BUILD_NUMBER.1/Firmware.zip. Test description available at https://github.com/PX4/Firmware/pull/$TRAVIS_PULL_REQUEST." #- s3cmd put --acl-public --guess-mime-type --config=.s3cfg Firmware.zip s3://s3-website-us-east-1.amazonaws.com/#$TRAVIS_JOB_ID/ deploy: From 8162dd9f901cb4835c40c01d3a008f46fed0f6d6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 21 Dec 2014 17:32:24 +0100 Subject: [PATCH 200/216] Travis CI: Next attempt at email notification --- .travis.yml | 14 +++++++++++++- 1 file changed, 13 insertions(+), 1 deletion(-) diff --git a/.travis.yml b/.travis.yml index ef267d0d58..993632a4eb 100644 --- a/.travis.yml +++ b/.travis.yml @@ -33,6 +33,7 @@ env: - PX4_EMAIL_SUBJECT="Travis CI result" # Email address: $PX4_EMAIL - secure: "ei3hKAw6Pk+vEkQBI5Y2Ak74BRAaXcK2UHVnVadviBHI4EVPwn1YGP6A4Y0wnLe4U7ETTl0UiijRoVxyDW0Mq896Pv0siw02amNpjSZZYu+RfN1+//MChB48OxsLDirUdHVrULhl/bOARM02h2Bg28jDE2g7IqmJwg3em3oMbjU=" + - PX4_REPORT=report.txt script: - arm-none-eabi-gcc --version @@ -49,7 +50,18 @@ script: # We use an encrypted env variable to ensure this only executes when artifacts are uploaded. after_script: - - /usr/bin/mail -s "$SUBJECT ($TRAVIS_COMMIT)" "$PX4_EMAIL" < "Branch $TRAVIS_BRANCH or pull request $TRAVIS_PULL_REQUEST ready for flight testin. Files available at https://px4-travis.s3.amazonaws.com/PX4/Firmware/$TRAVIS_BUILD_NUMBER/$TRAVIS_BUILD_NUMBER.1/Firmware.zip. Test description available at https://github.com/PX4/Firmware/pull/$TRAVIS_PULL_REQUEST." + - echo "Branch $TRAVIS_BRANCH (pull request: $TRAVIS_PULL_REQUEST) ready for flight testing." >> $PX4_REPORT + - git log -n1 > $PX4_REPORT + - echo " " >> $PX4_REPORT + - echo "Files available at:" >> $PX4_REPORT + - echo "https://px4-travis.s3.amazonaws.com/PX4/Firmware/$TRAVIS_BUILD_NUMBER/$TRAVIS_BUILD_NUMBER.1/Firmware.zip" >> $PX4_REPORT + - echo "Description of desired tests is available at:" >> $PX4_REPORT + - echo "https://github.com/PX4/Firmware/pull/$TRAVIS_PULL_REQUEST" >> $PX4_REPORT + - echo " " >> $PX4_REPORT + - echo "Thanks for testing!" >> $PX4_REPORT + - echo " " >> $PX4_REPORT + - $PX4_EMAILTEXT="$PX4_REPORT" + - /usr/bin/mail -s "$SUBJECT ($TRAVIS_COMMIT)" "$PX4_EMAIL" < $PX4_EMAILTEXT #- s3cmd put --acl-public --guess-mime-type --config=.s3cfg Firmware.zip s3://s3-website-us-east-1.amazonaws.com/#$TRAVIS_JOB_ID/ deploy: From d974570783c729dfb97e4a72d5cfe9901b69c3ec Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 21 Dec 2014 17:45:52 +0100 Subject: [PATCH 201/216] Travis CI: Next attempt at email notification - avoid env var. --- .travis.yml | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/.travis.yml b/.travis.yml index 993632a4eb..61c811b8f6 100644 --- a/.travis.yml +++ b/.travis.yml @@ -60,8 +60,7 @@ after_script: - echo " " >> $PX4_REPORT - echo "Thanks for testing!" >> $PX4_REPORT - echo " " >> $PX4_REPORT - - $PX4_EMAILTEXT="$PX4_REPORT" - - /usr/bin/mail -s "$SUBJECT ($TRAVIS_COMMIT)" "$PX4_EMAIL" < $PX4_EMAILTEXT + - /usr/bin/mail -s "$SUBJECT ($TRAVIS_COMMIT)" "$PX4_EMAIL" < "$PX4_REPORT" #- s3cmd put --acl-public --guess-mime-type --config=.s3cfg Firmware.zip s3://s3-website-us-east-1.amazonaws.com/#$TRAVIS_JOB_ID/ deploy: From f0da9e8fc1fc5a590a904865d299b29a099f5955 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 21 Dec 2014 17:59:51 +0100 Subject: [PATCH 202/216] Travis CI: We need mailutils --- .travis.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.travis.yml b/.travis.yml index 61c811b8f6..25f5ea5aad 100644 --- a/.travis.yml +++ b/.travis.yml @@ -6,7 +6,7 @@ language: cpp before_script: - sudo apt-get update -q # Travis specific tools - - sudo apt-get install s3cmd grep zip + - sudo apt-get install s3cmd grep zip mailutils # General toolchain dependencies - sudo apt-get install libc6:i386 libgcc1:i386 gcc-4.6-base:i386 libstdc++5:i386 libstdc++6:i386 - sudo apt-get install python-serial python-argparse From 04bf1304416a4c1159acc41e2c9d2503ed84bea0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 21 Dec 2014 18:30:10 +0100 Subject: [PATCH 203/216] Travis CI: Change binary file name --- .travis.yml | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/.travis.yml b/.travis.yml index 25f5ea5aad..d25080bed3 100644 --- a/.travis.yml +++ b/.travis.yml @@ -67,9 +67,7 @@ deploy: provider: releases api_key: secure: cdHWLRBxA5UlYpOS0Sp891QK7PFmMgQ5ZWs1aPt+sw0rIrowyWMHCwXNBEdUqaExHYNYgXCUDI0EzNgfB7ZcR63Qv1MQeoyamV4jsxlyAqDqmxNtWO82S6RhHGeMLk26VgFKzynVcEk1IYlQP2nqzMQLdu+jTrngERuAIrCdRuc= - file: - paths: - - "Firmware.zip" + file: "Firmware.zip" skip_cleanup: true on: tags: true From d511e39ea7a3f1e0cb672b14598e18a7df18156a Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 22 Dec 2014 17:09:43 -0500 Subject: [PATCH 204/216] turn on -Werror and fix resulting errors --- makefiles/toolchain_gnu-arm-eabi.mk | 1 + src/drivers/ardrone_interface/ardrone_interface.c | 2 +- src/drivers/boards/aerocore/aerocore_led.c | 3 ++- src/drivers/frsky_telemetry/frsky_telemetry.c | 2 +- src/drivers/hott/hott_sensors/hott_sensors.cpp | 2 +- src/drivers/hott/hott_telemetry/hott_telemetry.cpp | 2 +- src/drivers/px4flow/module.mk | 2 ++ src/drivers/px4flow/px4flow.cpp | 2 +- src/drivers/roboclaw/roboclaw_main.cpp | 2 +- src/drivers/trone/trone.cpp | 2 +- src/examples/fixedwing_control/main.c | 2 +- src/examples/fixedwing_control/module.mk | 2 ++ .../flow_position_estimator_main.c | 2 +- src/examples/flow_position_estimator/module.mk | 2 ++ src/examples/hwtest/hwtest.c | 12 +++++++----- src/examples/matlab_csv_serial/matlab_csv_serial.c | 2 +- src/examples/px4_daemon_app/px4_daemon_app.c | 9 ++++++--- src/lib/conversion/rotation.cpp | 5 +++++ src/lib/rc/st24.c | 2 +- .../attitude_estimator_ekf_main.cpp | 5 +++-- src/modules/attitude_estimator_ekf/module.mk | 5 +++++ .../attitude_estimator_so3_main.cpp | 2 +- src/modules/attitude_estimator_so3/module.mk | 2 ++ src/modules/bottle_drop/bottle_drop.cpp | 7 +++++++ src/modules/commander/commander.cpp | 2 +- src/modules/commander/module.mk | 2 ++ src/modules/ekf_att_pos_estimator/module.mk | 2 +- .../fixedwing_backside/fixedwing_backside_main.cpp | 2 +- src/modules/fw_pos_control_l1/module.mk | 2 ++ src/modules/mavlink/mavlink_ftp.cpp | 2 +- src/modules/mavlink/mavlink_ftp.h | 2 +- src/modules/mavlink/mavlink_main.cpp | 2 +- src/modules/mavlink/mavlink_messages.cpp | 3 +++ src/modules/navigator/module.mk | 2 ++ src/modules/position_estimator_inav/module.mk | 2 ++ .../position_estimator_inav_main.c | 2 +- src/modules/sdlog2/module.mk | 2 ++ src/modules/sdlog2/sdlog2.c | 4 +++- src/modules/segway/segway_main.cpp | 2 +- src/modules/sensors/module.mk | 2 ++ src/modules/systemlib/module.mk | 2 ++ src/modules/systemlib/systemlib.c | 2 +- src/modules/systemlib/systemlib.h | 2 +- src/modules/vtol_att_control/module.mk | 2 ++ src/systemcmds/mixer/module.mk | 2 ++ src/systemcmds/mtd/module.mk | 2 ++ src/systemcmds/param/param.c | 2 +- src/systemcmds/tests/module.mk | 2 ++ 48 files changed, 95 insertions(+), 35 deletions(-) diff --git a/makefiles/toolchain_gnu-arm-eabi.mk b/makefiles/toolchain_gnu-arm-eabi.mk index 84e5cd08a2..7119c723b0 100644 --- a/makefiles/toolchain_gnu-arm-eabi.mk +++ b/makefiles/toolchain_gnu-arm-eabi.mk @@ -127,6 +127,7 @@ ARCHCXXFLAGS = -fno-exceptions -fno-rtti -std=gnu++0x -fno-threadsafe-statics # ARCHWARNINGS = -Wall \ -Wextra \ + -Werror \ -Wdouble-promotion \ -Wshadow \ -Wfloat-equal \ diff --git a/src/drivers/ardrone_interface/ardrone_interface.c b/src/drivers/ardrone_interface/ardrone_interface.c index 9d2c1441de..7f1b21a95c 100644 --- a/src/drivers/ardrone_interface/ardrone_interface.c +++ b/src/drivers/ardrone_interface/ardrone_interface.c @@ -121,7 +121,7 @@ int ardrone_interface_main(int argc, char *argv[]) SCHED_PRIORITY_MAX - 15, 1100, ardrone_interface_thread_main, - (argv) ? (const char **)&argv[2] : (const char **)NULL); + (argv) ? (char * const *)&argv[2] : (char * const *)NULL); exit(0); } diff --git a/src/drivers/boards/aerocore/aerocore_led.c b/src/drivers/boards/aerocore/aerocore_led.c index e40d1730c6..94a716029f 100644 --- a/src/drivers/boards/aerocore/aerocore_led.c +++ b/src/drivers/boards/aerocore/aerocore_led.c @@ -45,6 +45,7 @@ #include "board_config.h" #include +#include /* * Ideally we'd be able to get these from up_internal.h, @@ -54,7 +55,7 @@ * CONFIG_ARCH_LEDS configuration switch. */ __BEGIN_DECLS -extern void led_init(); +extern void led_init(void); extern void led_on(int led); extern void led_off(int led); extern void led_toggle(int led); diff --git a/src/drivers/frsky_telemetry/frsky_telemetry.c b/src/drivers/frsky_telemetry/frsky_telemetry.c index bccdf11906..5035600ef2 100644 --- a/src/drivers/frsky_telemetry/frsky_telemetry.c +++ b/src/drivers/frsky_telemetry/frsky_telemetry.c @@ -221,7 +221,7 @@ int frsky_telemetry_main(int argc, char *argv[]) SCHED_PRIORITY_DEFAULT, 2000, frsky_telemetry_thread_main, - (const char **)argv); + (char * const *)argv); while (!thread_running) { usleep(200); diff --git a/src/drivers/hott/hott_sensors/hott_sensors.cpp b/src/drivers/hott/hott_sensors/hott_sensors.cpp index 8ab9d8d55f..4b8e0c0b0c 100644 --- a/src/drivers/hott/hott_sensors/hott_sensors.cpp +++ b/src/drivers/hott/hott_sensors/hott_sensors.cpp @@ -214,7 +214,7 @@ hott_sensors_main(int argc, char *argv[]) SCHED_PRIORITY_DEFAULT, 1024, hott_sensors_thread_main, - (argv) ? (const char **)&argv[2] : (const char **)NULL); + (argv) ? (char * const *)&argv[2] : (char * const *)NULL); exit(0); } diff --git a/src/drivers/hott/hott_telemetry/hott_telemetry.cpp b/src/drivers/hott/hott_telemetry/hott_telemetry.cpp index edbb14172e..17a24d1041 100644 --- a/src/drivers/hott/hott_telemetry/hott_telemetry.cpp +++ b/src/drivers/hott/hott_telemetry/hott_telemetry.cpp @@ -240,7 +240,7 @@ hott_telemetry_main(int argc, char *argv[]) SCHED_PRIORITY_DEFAULT, 2048, hott_telemetry_thread_main, - (argv) ? (const char **)&argv[2] : (const char **)NULL); + (argv) ? (char * const *)&argv[2] : (char * const *)NULL); exit(0); } diff --git a/src/drivers/px4flow/module.mk b/src/drivers/px4flow/module.mk index 460bec7b96..ecd3e804a1 100644 --- a/src/drivers/px4flow/module.mk +++ b/src/drivers/px4flow/module.mk @@ -40,3 +40,5 @@ MODULE_COMMAND = px4flow SRCS = px4flow.cpp MAXOPTIMIZATION = -Os + +EXTRACXXFLAGS = -Wno-attributes diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp index 09ec4bf964..62308fc654 100644 --- a/src/drivers/px4flow/px4flow.cpp +++ b/src/drivers/px4flow/px4flow.cpp @@ -106,7 +106,7 @@ struct i2c_frame { }; struct i2c_frame f; -typedef struct i2c_integral_frame { +struct i2c_integral_frame { uint16_t frame_count_since_last_readout; int16_t pixel_flow_x_integral; int16_t pixel_flow_y_integral; diff --git a/src/drivers/roboclaw/roboclaw_main.cpp b/src/drivers/roboclaw/roboclaw_main.cpp index 44ed03254f..83086fd7c8 100644 --- a/src/drivers/roboclaw/roboclaw_main.cpp +++ b/src/drivers/roboclaw/roboclaw_main.cpp @@ -109,7 +109,7 @@ int roboclaw_main(int argc, char *argv[]) SCHED_PRIORITY_MAX - 10, 2048, roboclaw_thread_main, - (const char **)argv); + (char * const *)argv); exit(0); } else if (!strcmp(argv[1], "test")) { diff --git a/src/drivers/trone/trone.cpp b/src/drivers/trone/trone.cpp index 83b5c987e8..cf35466694 100644 --- a/src/drivers/trone/trone.cpp +++ b/src/drivers/trone/trone.cpp @@ -206,7 +206,7 @@ static const uint8_t crc_table[] = { 0xfa, 0xfd, 0xf4, 0xf3 }; -uint8_t crc8(uint8_t *p, uint8_t len){ +static uint8_t crc8(uint8_t *p, uint8_t len) { uint16_t i; uint16_t crc = 0x0; diff --git a/src/examples/fixedwing_control/main.c b/src/examples/fixedwing_control/main.c index 6a5cbcd307..fcbb54c8ea 100644 --- a/src/examples/fixedwing_control/main.c +++ b/src/examples/fixedwing_control/main.c @@ -423,7 +423,7 @@ int ex_fixedwing_control_main(int argc, char *argv[]) SCHED_PRIORITY_MAX - 20, 2048, fixedwing_control_thread_main, - (argv) ? (const char **)&argv[2] : (const char **)NULL); + (argv) ? (char * const *)&argv[2] : (char * const *)NULL); thread_running = true; exit(0); } diff --git a/src/examples/fixedwing_control/module.mk b/src/examples/fixedwing_control/module.mk index a2a9eb1132..3fab9c8d4d 100644 --- a/src/examples/fixedwing_control/module.mk +++ b/src/examples/fixedwing_control/module.mk @@ -41,3 +41,5 @@ SRCS = main.c \ params.c MODULE_STACKSIZE = 1200 + +EXTRACFLAGS = -Wno-error diff --git a/src/examples/flow_position_estimator/flow_position_estimator_main.c b/src/examples/flow_position_estimator/flow_position_estimator_main.c index 0b8c01f798..a89ccf9332 100644 --- a/src/examples/flow_position_estimator/flow_position_estimator_main.c +++ b/src/examples/flow_position_estimator/flow_position_estimator_main.c @@ -114,7 +114,7 @@ int flow_position_estimator_main(int argc, char *argv[]) SCHED_PRIORITY_MAX - 5, 4000, flow_position_estimator_thread_main, - (argv) ? (const char **)&argv[2] : (const char **)NULL); + (argv) ? (char * const *)&argv[2] : (char * const *)NULL); exit(0); } diff --git a/src/examples/flow_position_estimator/module.mk b/src/examples/flow_position_estimator/module.mk index 88c9ceb937..5c6e29f8f6 100644 --- a/src/examples/flow_position_estimator/module.mk +++ b/src/examples/flow_position_estimator/module.mk @@ -39,3 +39,5 @@ MODULE_COMMAND = flow_position_estimator SRCS = flow_position_estimator_main.c \ flow_position_estimator_params.c + +EXTRACFLAGS = -Wno-float-equal diff --git a/src/examples/hwtest/hwtest.c b/src/examples/hwtest/hwtest.c index d3b10f46ed..95ff346bbb 100644 --- a/src/examples/hwtest/hwtest.c +++ b/src/examples/hwtest/hwtest.c @@ -39,13 +39,15 @@ * @author Lorenz Meier */ -#include #include -#include +#include + #include -#include -#include +#include +#include #include +#include +#include __EXPORT int ex_hwtest_main(int argc, char *argv[]); @@ -53,7 +55,7 @@ int ex_hwtest_main(int argc, char *argv[]) { warnx("DO NOT FORGET TO STOP THE COMMANDER APP!"); warnx("(run to do so)"); - warnx("usage: http://px4.io/dev/examples/write_output"); + warnx("usage: http://px4.io/dev/examples/write_output"); struct actuator_controls_s actuators; memset(&actuators, 0, sizeof(actuators)); diff --git a/src/examples/matlab_csv_serial/matlab_csv_serial.c b/src/examples/matlab_csv_serial/matlab_csv_serial.c index c66bebeecd..a95f45d1ad 100644 --- a/src/examples/matlab_csv_serial/matlab_csv_serial.c +++ b/src/examples/matlab_csv_serial/matlab_csv_serial.c @@ -107,7 +107,7 @@ int matlab_csv_serial_main(int argc, char *argv[]) SCHED_PRIORITY_MAX - 5, 2000, matlab_csv_serial_thread_main, - (argv) ? (const char **)&argv[2] : (const char **)NULL); + (argv) ? (char * const *)&argv[2] : (char * const *)NULL); exit(0); } diff --git a/src/examples/px4_daemon_app/px4_daemon_app.c b/src/examples/px4_daemon_app/px4_daemon_app.c index 3eaf14148a..45d541137a 100644 --- a/src/examples/px4_daemon_app/px4_daemon_app.c +++ b/src/examples/px4_daemon_app/px4_daemon_app.c @@ -38,10 +38,13 @@ * @author Example User */ +#include +#include +#include +#include + #include #include -#include -#include #include #include @@ -100,7 +103,7 @@ int px4_daemon_app_main(int argc, char *argv[]) SCHED_PRIORITY_DEFAULT, 2000, px4_daemon_thread_main, - (argv) ? (const char **)&argv[2] : (const char **)NULL); + (argv) ? (char * const *)&argv[2] : (char * const *)NULL); exit(0); } diff --git a/src/lib/conversion/rotation.cpp b/src/lib/conversion/rotation.cpp index e177157333..3d3e13355b 100644 --- a/src/lib/conversion/rotation.cpp +++ b/src/lib/conversion/rotation.cpp @@ -181,6 +181,11 @@ rotate_3f(enum Rotation rot, float &x, float &y, float &z) x = tmp; return; } + case ROTATION_ROLL_270_YAW_270: { + // unimplemented + ASSERT(0 == 1); + return; + } case ROTATION_PITCH_90: { tmp = z; z = -x; x = tmp; return; diff --git a/src/lib/rc/st24.c b/src/lib/rc/st24.c index e8a791b8f6..5c53a1602f 100644 --- a/src/lib/rc/st24.c +++ b/src/lib/rc/st24.c @@ -72,7 +72,7 @@ const char *decode_states[] = {"UNSYNCED", #define ST24_SCALE_OFFSET (int)(ST24_TARGET_MIN - (ST24_SCALE_FACTOR * ST24_RANGE_MIN + 0.5f)) static enum ST24_DECODE_STATE _decode_state = ST24_DECODE_STATE_UNSYNCED; -static unsigned _rxlen; +static uint8_t _rxlen; static ReceiverFcPacket _rxpacket; diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index 6068ab0822..a9205b6fd6 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -134,7 +134,7 @@ int attitude_estimator_ekf_main(int argc, char *argv[]) SCHED_PRIORITY_MAX - 5, 7200, attitude_estimator_ekf_thread_main, - (argv) ? (const char **)&argv[2] : (const char **)NULL); + (argv) ? (char * const *)&argv[2] : (char * const *)NULL); exit(0); } @@ -275,7 +275,8 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds /* keep track of sensor updates */ uint64_t sensor_last_timestamp[3] = {0, 0, 0}; - struct attitude_estimator_ekf_params ekf_params = { 0 }; + struct attitude_estimator_ekf_params ekf_params; + memset(&ekf_params, 0, sizeof(ekf_params)); struct attitude_estimator_ekf_param_handles ekf_param_handles = { 0 }; diff --git a/src/modules/attitude_estimator_ekf/module.mk b/src/modules/attitude_estimator_ekf/module.mk index 749b0a91c3..c4bf342295 100644 --- a/src/modules/attitude_estimator_ekf/module.mk +++ b/src/modules/attitude_estimator_ekf/module.mk @@ -42,3 +42,8 @@ SRCS = attitude_estimator_ekf_main.cpp \ codegen/AttitudeEKF.c MODULE_STACKSIZE = 1200 + +EXTRACFLAGS = -Wno-float-equal -Wno-error + +EXTRACXXFLAGS = -Wno-error + diff --git a/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp b/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp index e49027e470..9414225ca0 100755 --- a/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp +++ b/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp @@ -153,7 +153,7 @@ int attitude_estimator_so3_main(int argc, char *argv[]) SCHED_PRIORITY_MAX - 5, 14000, attitude_estimator_so3_thread_main, - (argv) ? (const char **)&argv[2] : (const char **)NULL); + (argv) ? (char * const *)&argv[2] : (char * const *)NULL); exit(0); } diff --git a/src/modules/attitude_estimator_so3/module.mk b/src/modules/attitude_estimator_so3/module.mk index f52715abb1..7b2e206cc5 100644 --- a/src/modules/attitude_estimator_so3/module.mk +++ b/src/modules/attitude_estimator_so3/module.mk @@ -8,3 +8,5 @@ SRCS = attitude_estimator_so3_main.cpp \ attitude_estimator_so3_params.c MODULE_STACKSIZE = 1200 + +EXTRACXXFLAGS = -Wno-float-equal diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp index e0bcbc6e9e..4580b338de 100644 --- a/src/modules/bottle_drop/bottle_drop.cpp +++ b/src/modules/bottle_drop/bottle_drop.cpp @@ -523,6 +523,9 @@ BottleDrop::task_main() } switch (_drop_state) { + case DROP_STATE_INIT: + // do nothing + break; case DROP_STATE_TARGET_VALID: { @@ -689,6 +692,10 @@ BottleDrop::task_main() orb_publish(ORB_ID(onboard_mission), _onboard_mission_pub, &_onboard_mission); } break; + + case DROP_STATE_BAY_CLOSED: + // do nothing + break; } counter++; diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 744323c011..325ab2a678 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -267,7 +267,7 @@ int commander_main(int argc, char *argv[]) SCHED_PRIORITY_MAX - 40, 3200, commander_thread_main, - (argv) ? (const char **)&argv[2] : (const char **)NULL); + (argv) ? (char * const *)&argv[2] : (char * const *)NULL); while (!thread_running) { usleep(200); diff --git a/src/modules/commander/module.mk b/src/modules/commander/module.mk index 27ca5c1823..caec1e8694 100644 --- a/src/modules/commander/module.mk +++ b/src/modules/commander/module.mk @@ -51,3 +51,5 @@ SRCS = commander.cpp \ MODULE_STACKSIZE = 1200 MAXOPTIMIZATION = -Os + +EXTRACXXFLAGS = -Wno-error diff --git a/src/modules/ekf_att_pos_estimator/module.mk b/src/modules/ekf_att_pos_estimator/module.mk index 36d854dddb..07e70ddb3b 100644 --- a/src/modules/ekf_att_pos_estimator/module.mk +++ b/src/modules/ekf_att_pos_estimator/module.mk @@ -42,4 +42,4 @@ SRCS = ekf_att_pos_estimator_main.cpp \ estimator_23states.cpp \ estimator_utilities.cpp -EXTRACXXFLAGS = -Weffc++ +EXTRACXXFLAGS = -Weffc++ -Wno-error diff --git a/src/modules/fixedwing_backside/fixedwing_backside_main.cpp b/src/modules/fixedwing_backside/fixedwing_backside_main.cpp index f4ea050880..71b387b1e7 100644 --- a/src/modules/fixedwing_backside/fixedwing_backside_main.cpp +++ b/src/modules/fixedwing_backside/fixedwing_backside_main.cpp @@ -115,7 +115,7 @@ int fixedwing_backside_main(int argc, char *argv[]) SCHED_PRIORITY_MAX - 10, 5120, control_demo_thread_main, - (argv) ? (const char **)&argv[2] : (const char **)NULL); + (argv) ? (char * const *)&argv[2] : (char * const *)NULL); exit(0); } diff --git a/src/modules/fw_pos_control_l1/module.mk b/src/modules/fw_pos_control_l1/module.mk index 440bab2c5a..98e5c0a1ef 100644 --- a/src/modules/fw_pos_control_l1/module.mk +++ b/src/modules/fw_pos_control_l1/module.mk @@ -47,3 +47,5 @@ SRCS = fw_pos_control_l1_main.cpp \ MODULE_STACKSIZE = 1200 MAXOPTIMIZATION = -Os + +EXTRACXXFLAGS = -Wno-float-equal diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index f17497aa8f..4ba595a87b 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -801,7 +801,7 @@ MavlinkFTP::_return_request(Request *req) /// @brief Copy file (with limited space) int -MavlinkFTP::_copy_file(const char *src_path, const char *dst_path, ssize_t length) +MavlinkFTP::_copy_file(const char *src_path, const char *dst_path, size_t length) { char buff[512]; int src_fd = -1, dst_fd = -1; diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h index bef6775a94..9693a92a97 100644 --- a/src/modules/mavlink/mavlink_ftp.h +++ b/src/modules/mavlink/mavlink_ftp.h @@ -142,7 +142,7 @@ private: static void _worker_trampoline(void *arg); void _process_request(Request *req); void _reply(Request *req); - int _copy_file(const char *src_path, const char *dst_path, ssize_t length); + int _copy_file(const char *src_path, const char *dst_path, size_t length); ErrorCode _workList(PayloadHeader *payload); ErrorCode _workOpen(PayloadHeader *payload, int oflag); diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 29b7ec7b74..08a9ac76be 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1638,7 +1638,7 @@ Mavlink::start(int argc, char *argv[]) SCHED_PRIORITY_DEFAULT, 2800, (main_t)&Mavlink::start_helper, - (const char **)argv); + (char * const *)argv); // Ensure that this shell command // does not return before the instance diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 378e3427d0..2203ea1e74 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1764,6 +1764,9 @@ protected: case RC_INPUT_SOURCE_PX4IO_ST24: msg.rssi |= (3 << 4); break; + case RC_INPUT_SOURCE_UNKNOWN: + // do nothing + break; } if (rc.rc_lost) { diff --git a/src/modules/navigator/module.mk b/src/modules/navigator/module.mk index c44d4c35e6..0d7d6b9ef1 100644 --- a/src/modules/navigator/module.mk +++ b/src/modules/navigator/module.mk @@ -62,3 +62,5 @@ INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink MODULE_STACKSIZE = 1200 MAXOPTIMIZATION = -Os + +EXTRACXXFLAGS = -Wno-sign-compare diff --git a/src/modules/position_estimator_inav/module.mk b/src/modules/position_estimator_inav/module.mk index 0658d3f09c..5ebdb721c9 100644 --- a/src/modules/position_estimator_inav/module.mk +++ b/src/modules/position_estimator_inav/module.mk @@ -41,3 +41,5 @@ SRCS = position_estimator_inav_main.c \ inertial_filter.c MODULE_STACKSIZE = 1200 + +EXTRACFLAGS = -Wno-error diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 0af01cba1a..43829ede5e 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -151,7 +151,7 @@ int position_estimator_inav_main(int argc, char *argv[]) position_estimator_inav_task = task_spawn_cmd("position_estimator_inav", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 5000, position_estimator_inav_thread_main, - (argv) ? (const char **) &argv[2] : (const char **) NULL); + (argv) ? (char * const *) &argv[2] : (char * const *) NULL); exit(0); } diff --git a/src/modules/sdlog2/module.mk b/src/modules/sdlog2/module.mk index d4a00af39d..d7224b77a1 100644 --- a/src/modules/sdlog2/module.mk +++ b/src/modules/sdlog2/module.mk @@ -45,3 +45,5 @@ SRCS = sdlog2.c \ MODULE_STACKSIZE = 1200 MAXOPTIMIZATION = -Os + +EXTRACFLAGS = -Wno-error diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 6df677338e..0a8564da6a 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -304,7 +304,7 @@ int sdlog2_main(int argc, char *argv[]) SCHED_PRIORITY_DEFAULT - 30, 3000, sdlog2_thread_main, - (const char **)argv); + (char * const *)argv); exit(0); } @@ -1089,6 +1089,8 @@ int sdlog2_thread_main(int argc, char *argv[]) if (_extended_logging) { subs.sat_info_sub = orb_subscribe(ORB_ID(satellite_info)); + } else { + subs.sat_info_sub = 0; } /* close non-needed fd's */ diff --git a/src/modules/segway/segway_main.cpp b/src/modules/segway/segway_main.cpp index 061fbf9b9d..ee492f85a2 100644 --- a/src/modules/segway/segway_main.cpp +++ b/src/modules/segway/segway_main.cpp @@ -110,7 +110,7 @@ int segway_main(int argc, char *argv[]) SCHED_PRIORITY_MAX - 10, 5120, segway_thread_main, - (argv) ? (const char **)&argv[2] : (const char **)NULL); + (argv) ? (char * const *)&argv[2] : (char * const *)NULL); exit(0); } diff --git a/src/modules/sensors/module.mk b/src/modules/sensors/module.mk index dfbba92d91..f37bc93277 100644 --- a/src/modules/sensors/module.mk +++ b/src/modules/sensors/module.mk @@ -44,3 +44,5 @@ SRCS = sensors.cpp \ MODULE_STACKSIZE = 1200 MAXOPTIMIZATION = -Os + +EXTRACXXFLAGS = -Wno-type-limits diff --git a/src/modules/systemlib/module.mk b/src/modules/systemlib/module.mk index fe8b7e75a0..f4dff2838c 100644 --- a/src/modules/systemlib/module.mk +++ b/src/modules/systemlib/module.mk @@ -57,3 +57,5 @@ SRCS = err.c \ mcu_version.c MAXOPTIMIZATION = -Os + +EXTRACFLAGS = -Wno-sign-compare diff --git a/src/modules/systemlib/systemlib.c b/src/modules/systemlib/systemlib.c index 90d8dd77cd..82183b0d72 100644 --- a/src/modules/systemlib/systemlib.c +++ b/src/modules/systemlib/systemlib.c @@ -87,7 +87,7 @@ static void kill_task(FAR struct tcb_s *tcb, FAR void *arg) kill(tcb->pid, SIGUSR1); } -int task_spawn_cmd(const char *name, int scheduler, int priority, int stack_size, main_t entry, const char *argv[]) +int task_spawn_cmd(const char *name, int scheduler, int priority, int stack_size, main_t entry, char * const argv[]) { int pid; diff --git a/src/modules/systemlib/systemlib.h b/src/modules/systemlib/systemlib.h index 6e22a557ea..2f24215a9f 100644 --- a/src/modules/systemlib/systemlib.h +++ b/src/modules/systemlib/systemlib.h @@ -64,7 +64,7 @@ __EXPORT int task_spawn_cmd(const char *name, int scheduler, int stack_size, main_t entry, - const char *argv[]); + char * const argv[]); enum MULT_PORTS { MULT_0_US2_RXTX = 0, diff --git a/src/modules/vtol_att_control/module.mk b/src/modules/vtol_att_control/module.mk index 0d5155e901..02403ef764 100644 --- a/src/modules/vtol_att_control/module.mk +++ b/src/modules/vtol_att_control/module.mk @@ -39,3 +39,5 @@ MODULE_COMMAND = vtol_att_control SRCS = vtol_att_control_main.cpp \ vtol_att_control_params.c + +EXTRACXXFLAGS = -Wno-error diff --git a/src/systemcmds/mixer/module.mk b/src/systemcmds/mixer/module.mk index cdbff75f04..40ffb8f12c 100644 --- a/src/systemcmds/mixer/module.mk +++ b/src/systemcmds/mixer/module.mk @@ -41,3 +41,5 @@ SRCS = mixer.cpp MODULE_STACKSIZE = 4096 MAXOPTIMIZATION = -Os + +EXTRACXXFLAGS = -Wno-error diff --git a/src/systemcmds/mtd/module.mk b/src/systemcmds/mtd/module.mk index 1bc4f414ec..c3baffc5dd 100644 --- a/src/systemcmds/mtd/module.mk +++ b/src/systemcmds/mtd/module.mk @@ -6,3 +6,5 @@ MODULE_COMMAND = mtd SRCS = mtd.c 24xxxx_mtd.c MAXOPTIMIZATION = -Os + +EXTRACFLAGS = -Wno-error \ No newline at end of file diff --git a/src/systemcmds/param/param.c b/src/systemcmds/param/param.c index e110335e7c..80ee204e8c 100644 --- a/src/systemcmds/param/param.c +++ b/src/systemcmds/param/param.c @@ -212,7 +212,7 @@ static void do_show(const char* search_string) { printf(" + = saved, * = unsaved\n"); - param_foreach(do_show_print, search_string, false); + param_foreach(do_show_print, (char *)search_string, false); exit(0); } diff --git a/src/systemcmds/tests/module.mk b/src/systemcmds/tests/module.mk index 622a0faf3d..1ea2a6620d 100644 --- a/src/systemcmds/tests/module.mk +++ b/src/systemcmds/tests/module.mk @@ -34,3 +34,5 @@ SRCS = test_adc.c \ test_conv.cpp \ test_mount.c \ test_mtd.c + +EXTRACXXFLAGS = -Wno-error From 84d744707d07ae60dcf201a828d529057061fe47 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 23 Dec 2014 14:34:53 +0100 Subject: [PATCH 205/216] UAVCAN: Move into lib directory --- .gitmodules | 2 +- Tools/px4params/srcparser.py | 2 +- Tools/px4params/srcscanner.py | 7 ++++++- uavcan => src/lib/uavcan | 0 src/modules/uavcan/module.mk | 4 ++-- 5 files changed, 10 insertions(+), 5 deletions(-) rename uavcan => src/lib/uavcan (100%) diff --git a/.gitmodules b/.gitmodules index 4b84afac2a..17a0984317 100644 --- a/.gitmodules +++ b/.gitmodules @@ -5,5 +5,5 @@ path = NuttX url = git://github.com/PX4/NuttX.git [submodule "uavcan"] - path = uavcan + path = src/lib/uavcan url = git://github.com/pavel-kirienko/uavcan.git diff --git a/Tools/px4params/srcparser.py b/Tools/px4params/srcparser.py index 0a4d21d264..8e60921951 100644 --- a/Tools/px4params/srcparser.py +++ b/Tools/px4params/srcparser.py @@ -103,7 +103,7 @@ class SourceParser(object): Returns list of supported file extensions that can be parsed by this parser. """ - return ["cpp", "c"] + return [".cpp", ".c"] def Parse(self, contents): """ diff --git a/Tools/px4params/srcscanner.py b/Tools/px4params/srcscanner.py index d7eca72d72..1f0ea4e89e 100644 --- a/Tools/px4params/srcscanner.py +++ b/Tools/px4params/srcscanner.py @@ -26,5 +26,10 @@ class SourceScanner(object): parser.Parse method. """ with codecs.open(path, 'r', 'utf-8') as f: - contents = f.read() + try: + contents = f.read() + except: + contents = '' + print('Failed reading file: %s, skipping content.' % path) + pass parser.Parse(contents) diff --git a/uavcan b/src/lib/uavcan similarity index 100% rename from uavcan rename to src/lib/uavcan diff --git a/src/modules/uavcan/module.mk b/src/modules/uavcan/module.mk index f92bc754f0..e5d30f6c47 100644 --- a/src/modules/uavcan/module.mk +++ b/src/modules/uavcan/module.mk @@ -57,7 +57,7 @@ SRCS += sensors/sensor_bridge.cpp \ # # libuavcan # -include $(UAVCAN_DIR)/libuavcan/include.mk +include $(PX4_LIB_DIR)/uavcan/libuavcan/include.mk SRCS += $(LIBUAVCAN_SRC) INCLUDE_DIRS += $(LIBUAVCAN_INC) # Since actual compiler mode is C++11, the library will default to UAVCAN_CPP11, but it will fail to compile @@ -67,7 +67,7 @@ override EXTRADEFINES := $(EXTRADEFINES) -DUAVCAN_CPP_VERSION=UAVCAN_CPP03 -DUAV # # libuavcan drivers for STM32 # -include $(UAVCAN_DIR)/libuavcan_drivers/stm32/driver/include.mk +include $(PX4_LIB_DIR)/uavcan/libuavcan_drivers/stm32/driver/include.mk SRCS += $(LIBUAVCAN_STM32_SRC) INCLUDE_DIRS += $(LIBUAVCAN_STM32_INC) override EXTRADEFINES := $(EXTRADEFINES) -DUAVCAN_STM32_NUTTX -DUAVCAN_STM32_NUM_IFACES=2 From 0aaabaee0974aaca354e95f403cca3317fd1a83e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 23 Dec 2014 15:02:01 +0100 Subject: [PATCH 206/216] VTOL: Remove unneeded headers --- src/modules/vtol_att_control/vtol_att_control_main.cpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index 958907d1ba..9a80562f3e 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -74,11 +74,8 @@ #include #include "drivers/drv_pwm_output.h" -#include #include -#include - #include From 78fa3564004287eb53dc3dc91ee1407e27c6e492 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 23 Dec 2014 15:19:43 +0100 Subject: [PATCH 207/216] Unittests: add debug --- unittests/debug.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/unittests/debug.h b/unittests/debug.h index 9824d13fc3..ac321312f5 100644 --- a/unittests/debug.h +++ b/unittests/debug.h @@ -2,4 +2,5 @@ #pragma once #include -#define lowsyslog warnx \ No newline at end of file +#define lowsyslog warnx +#define dbg warnx From d14fdf896b0081403908599a3164d09941d363c9 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 23 Dec 2014 10:56:32 -0500 Subject: [PATCH 208/216] rotate_3f() implement missing ROTATION_ROLL_270_YAW_270 --- src/examples/fixedwing_control/module.mk | 2 +- src/lib/conversion/rotation.cpp | 4 ++-- src/modules/attitude_estimator_ekf/module.mk | 5 ++--- 3 files changed, 5 insertions(+), 6 deletions(-) diff --git a/src/examples/fixedwing_control/module.mk b/src/examples/fixedwing_control/module.mk index 3fab9c8d4d..8707776aef 100644 --- a/src/examples/fixedwing_control/module.mk +++ b/src/examples/fixedwing_control/module.mk @@ -42,4 +42,4 @@ SRCS = main.c \ MODULE_STACKSIZE = 1200 -EXTRACFLAGS = -Wno-error +EXTRACFLAGS = -Wno-frame-larger-than diff --git a/src/lib/conversion/rotation.cpp b/src/lib/conversion/rotation.cpp index 3d3e13355b..1fe36d3950 100644 --- a/src/lib/conversion/rotation.cpp +++ b/src/lib/conversion/rotation.cpp @@ -182,8 +182,8 @@ rotate_3f(enum Rotation rot, float &x, float &y, float &z) return; } case ROTATION_ROLL_270_YAW_270: { - // unimplemented - ASSERT(0 == 1); + tmp = z; z = -y; y = tmp; + tmp = x; x = y; y = -tmp; return; } case ROTATION_PITCH_90: { diff --git a/src/modules/attitude_estimator_ekf/module.mk b/src/modules/attitude_estimator_ekf/module.mk index c4bf342295..96ba452423 100644 --- a/src/modules/attitude_estimator_ekf/module.mk +++ b/src/modules/attitude_estimator_ekf/module.mk @@ -43,7 +43,6 @@ SRCS = attitude_estimator_ekf_main.cpp \ MODULE_STACKSIZE = 1200 -EXTRACFLAGS = -Wno-float-equal -Wno-error - -EXTRACXXFLAGS = -Wno-error +EXTRACFLAGS = -Wno-float-equal +EXTRACXXFLAGS = -Wno-frame-larger-than From 5b600a815c13af56e879029196a24e544c110b97 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 23 Dec 2014 11:15:45 -0500 Subject: [PATCH 209/216] Replace use of -Wno-error and only ignore specific warnings --- src/examples/fixedwing_control/module.mk | 2 +- src/modules/attitude_estimator_ekf/module.mk | 4 ++-- src/modules/commander/module.mk | 3 ++- src/modules/ekf_att_pos_estimator/module.mk | 3 ++- src/modules/position_estimator_inav/module.mk | 3 ++- src/modules/sdlog2/module.mk | 3 ++- src/modules/vtol_att_control/module.mk | 3 ++- src/systemcmds/mixer/module.mk | 3 ++- src/systemcmds/mtd/module.mk | 3 ++- src/systemcmds/tests/module.mk | 3 ++- 10 files changed, 19 insertions(+), 11 deletions(-) diff --git a/src/examples/fixedwing_control/module.mk b/src/examples/fixedwing_control/module.mk index 8707776aef..f6c882ead4 100644 --- a/src/examples/fixedwing_control/module.mk +++ b/src/examples/fixedwing_control/module.mk @@ -42,4 +42,4 @@ SRCS = main.c \ MODULE_STACKSIZE = 1200 -EXTRACFLAGS = -Wno-frame-larger-than +EXTRACFLAGS = -Wframe-larger-than=1200 diff --git a/src/modules/attitude_estimator_ekf/module.mk b/src/modules/attitude_estimator_ekf/module.mk index 96ba452423..3a3e1cc698 100644 --- a/src/modules/attitude_estimator_ekf/module.mk +++ b/src/modules/attitude_estimator_ekf/module.mk @@ -43,6 +43,6 @@ SRCS = attitude_estimator_ekf_main.cpp \ MODULE_STACKSIZE = 1200 -EXTRACFLAGS = -Wno-float-equal +EXTRACFLAGS = -Wno-float-equal -Wframe-larger-than=3600 -EXTRACXXFLAGS = -Wno-frame-larger-than +EXTRACXXFLAGS = -Wframe-larger-than=2200 diff --git a/src/modules/commander/module.mk b/src/modules/commander/module.mk index caec1e8694..dac2ce59a6 100644 --- a/src/modules/commander/module.mk +++ b/src/modules/commander/module.mk @@ -52,4 +52,5 @@ MODULE_STACKSIZE = 1200 MAXOPTIMIZATION = -Os -EXTRACXXFLAGS = -Wno-error +EXTRACXXFLAGS = -Wframe-larger-than=1900 + diff --git a/src/modules/ekf_att_pos_estimator/module.mk b/src/modules/ekf_att_pos_estimator/module.mk index 07e70ddb3b..4311fb3f9d 100644 --- a/src/modules/ekf_att_pos_estimator/module.mk +++ b/src/modules/ekf_att_pos_estimator/module.mk @@ -42,4 +42,5 @@ SRCS = ekf_att_pos_estimator_main.cpp \ estimator_23states.cpp \ estimator_utilities.cpp -EXTRACXXFLAGS = -Weffc++ -Wno-error +EXTRACXXFLAGS = -Weffc++ -Wframe-larger-than=3000 + diff --git a/src/modules/position_estimator_inav/module.mk b/src/modules/position_estimator_inav/module.mk index 5ebdb721c9..45c8762996 100644 --- a/src/modules/position_estimator_inav/module.mk +++ b/src/modules/position_estimator_inav/module.mk @@ -42,4 +42,5 @@ SRCS = position_estimator_inav_main.c \ MODULE_STACKSIZE = 1200 -EXTRACFLAGS = -Wno-error +EXTRACFLAGS = -Wframe-larger-than=3500 + diff --git a/src/modules/sdlog2/module.mk b/src/modules/sdlog2/module.mk index d7224b77a1..f1118000e8 100644 --- a/src/modules/sdlog2/module.mk +++ b/src/modules/sdlog2/module.mk @@ -46,4 +46,5 @@ MODULE_STACKSIZE = 1200 MAXOPTIMIZATION = -Os -EXTRACFLAGS = -Wno-error +EXTRACFLAGS = -Wframe-larger-than=1200 + diff --git a/src/modules/vtol_att_control/module.mk b/src/modules/vtol_att_control/module.mk index 02403ef764..0cf3072c8f 100644 --- a/src/modules/vtol_att_control/module.mk +++ b/src/modules/vtol_att_control/module.mk @@ -40,4 +40,5 @@ MODULE_COMMAND = vtol_att_control SRCS = vtol_att_control_main.cpp \ vtol_att_control_params.c -EXTRACXXFLAGS = -Wno-error +EXTRACXXFLAGS = -Wno-write-strings + diff --git a/src/systemcmds/mixer/module.mk b/src/systemcmds/mixer/module.mk index 40ffb8f12c..0fb899c671 100644 --- a/src/systemcmds/mixer/module.mk +++ b/src/systemcmds/mixer/module.mk @@ -42,4 +42,5 @@ MODULE_STACKSIZE = 4096 MAXOPTIMIZATION = -Os -EXTRACXXFLAGS = -Wno-error +EXTRACXXFLAGS = -Wframe-larger-than=2048 + diff --git a/src/systemcmds/mtd/module.mk b/src/systemcmds/mtd/module.mk index c3baffc5dd..bca1cdcc18 100644 --- a/src/systemcmds/mtd/module.mk +++ b/src/systemcmds/mtd/module.mk @@ -7,4 +7,5 @@ SRCS = mtd.c 24xxxx_mtd.c MAXOPTIMIZATION = -Os -EXTRACFLAGS = -Wno-error \ No newline at end of file +EXTRACFLAGS = -Wno-error + diff --git a/src/systemcmds/tests/module.mk b/src/systemcmds/tests/module.mk index 1ea2a6620d..6eed3922c4 100644 --- a/src/systemcmds/tests/module.mk +++ b/src/systemcmds/tests/module.mk @@ -35,4 +35,5 @@ SRCS = test_adc.c \ test_mount.c \ test_mtd.c -EXTRACXXFLAGS = -Wno-error +EXTRACXXFLAGS = -Wframe-larger-than=2500 + From d64c223416858b0024d70f871f46356434dc8145 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 24 Dec 2014 09:44:52 +0100 Subject: [PATCH 210/216] Travis CI: Notify Gitter --- .travis.yml | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/.travis.yml b/.travis.yml index d25080bed3..e7dc6029cc 100644 --- a/.travis.yml +++ b/.travis.yml @@ -85,3 +85,11 @@ addons: bucket: px4-travis region: us-east-1 endpoint: s3-website-us-east-1.amazonaws.com + +notifications: + webhooks: + urls: + - https://webhooks.gitter.im/e/2b9c4a4cb2211f8befba + on_success: always # options: [always|never|change] default: always + on_failure: always # options: [always|never|change] default: always + on_start: false # default: false From 92eb637d00dc7bec347ddb6d2ddfd90bb27a0b34 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 24 Dec 2014 10:21:26 +0100 Subject: [PATCH 211/216] README: Give developers a quickstart in finding dev docs --- README.md | 20 +++++++++++++++++++- 1 file changed, 19 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index 302ea08c32..5e4bc478d6 100644 --- a/README.md +++ b/README.md @@ -1,4 +1,4 @@ -## PX4 Aerial Middleware and Flight Control Stack ## +## PX4 Flight Control Stack and Middleware ## [![Build Status](https://travis-ci.org/PX4/Firmware.svg?branch=master)](https://travis-ci.org/PX4/Firmware) @@ -13,3 +13,21 @@ * Binaries (always up-to-date from master): * [Downloads](https://pixhawk.org/downloads) * Mailing list: [Google Groups](http://groups.google.com/group/px4users) + +### Developers ### + +Contributing guide: +http://px4.io/dev/contributing + +Developer guide: +http://px4.io/dev/ + +This repository contains code supporting these boards: + * PX4FMUv1.x + * PX4FMUv2.x + * AeroCore + +## NuttShell (NSH) ## + +NSH usage documentation: +http://px4.io/users/serial_connection From 9292c8f405b0ed208443df0b1f9ebd497bb518ab Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Sat, 20 Dec 2014 10:27:02 -0700 Subject: [PATCH 212/216] add interrupt latency printout command and mean/variance to interval performance counter --- src/drivers/stm32/drv_hrt.c | 8 +++-- src/modules/systemlib/perf_counter.c | 53 ++++++++++++++++++++++------ src/modules/systemlib/perf_counter.h | 9 ++++- src/systemcmds/perf/perf.c | 6 +++- 4 files changed, 60 insertions(+), 16 deletions(-) diff --git a/src/drivers/stm32/drv_hrt.c b/src/drivers/stm32/drv_hrt.c index 281f918d70..603c2eb9da 100644 --- a/src/drivers/stm32/drv_hrt.c +++ b/src/drivers/stm32/drv_hrt.c @@ -253,9 +253,11 @@ static uint16_t latency_baseline; static uint16_t latency_actual; /* latency histogram */ -#define LATENCY_BUCKET_COUNT 8 -static const uint16_t latency_buckets[LATENCY_BUCKET_COUNT] = { 1, 2, 5, 10, 20, 50, 100, 1000 }; -static uint32_t latency_counters[LATENCY_BUCKET_COUNT + 1]; +#define LATENCY_BUCKET_COUNT 8 +__EXPORT const uint16_t latency_bucket_count = LATENCY_BUCKET_COUNT; +__EXPORT const uint16_t latency_buckets[LATENCY_BUCKET_COUNT] = { 1, 2, 5, 10, 20, 50, 100, 1000 }; +__EXPORT uint32_t latency_counters[LATENCY_BUCKET_COUNT + 1]; + /* timer-specific functions */ static void hrt_tim_init(void); diff --git a/src/modules/systemlib/perf_counter.c b/src/modules/systemlib/perf_counter.c index d6d8284d24..f9e90652d7 100644 --- a/src/modules/systemlib/perf_counter.c +++ b/src/modules/systemlib/perf_counter.c @@ -41,7 +41,7 @@ #include #include #include - +#include #include "perf_counter.h" /** @@ -84,7 +84,8 @@ struct perf_ctr_interval { uint64_t time_last; uint64_t time_least; uint64_t time_most; - + float mean; + float M2; }; /** @@ -109,6 +110,7 @@ perf_alloc(enum perf_counter_type type, const char *name) case PC_INTERVAL: ctr = (perf_counter_t)calloc(sizeof(struct perf_ctr_interval), 1); + break; default: @@ -156,15 +158,23 @@ perf_count(perf_counter_t handle) case 1: pci->time_least = now - pci->time_last; pci->time_most = now - pci->time_last; + pci->mean = pci->time_least / 1e6f; + pci->M2 = 0; break; default: { - hrt_abstime interval = now - pci->time_last; - if (interval < pci->time_least) - pci->time_least = interval; - if (interval > pci->time_most) - pci->time_most = interval; - break; - } + hrt_abstime interval = now - pci->time_last; + if (interval < pci->time_least) + pci->time_least = interval; + if (interval > pci->time_most) + pci->time_most = interval; + // maintain mean and variance of interval in seconds + // Knuth/Welford recursive mean and variance of update intervals (via Wikipedia) + float dt = interval / 1e6f; + float delta_intvl = dt - pci->mean; + pci->mean += delta_intvl / pci->event_count; + pci->M2 += delta_intvl * (dt - pci->mean); + break; + } } pci->time_last = now; pci->event_count++; @@ -313,13 +323,16 @@ perf_print_counter_fd(int fd, perf_counter_t handle) case PC_INTERVAL: { struct perf_ctr_interval *pci = (struct perf_ctr_interval *)handle; + float rms = sqrtf(pci->M2 / (pci->event_count-1)); - dprintf(fd, "%s: %llu events, %lluus avg, min %lluus max %lluus\n", + dprintf(fd, "%s: %llu events, %lluus avg, min %lluus max %lluus %5.3f msec mean %5.3f msec rms\n", handle->name, pci->event_count, (pci->time_last - pci->time_first) / pci->event_count, pci->time_least, - pci->time_most); + pci->time_most, + (double)(1000 * pci->mean), + (double)(1000 * rms)); break; } @@ -365,6 +378,21 @@ perf_print_all(int fd) } } +extern const uint16_t latency_bucket_count; +extern uint32_t latency_counters[]; +extern const uint16_t latency_buckets[]; + +void +perf_print_latency(int fd) +{ + dprintf(fd, "bucket : events\n"); + for (int i = 0; i < latency_bucket_count; i++) { + printf(" %4i : %i\n", latency_buckets[i], latency_counters[i]); + } + // print the overflow bucket value + dprintf(fd, " >%4i : %i\n", latency_buckets[latency_bucket_count-1], latency_counters[latency_bucket_count]); +} + void perf_reset_all(void) { @@ -374,4 +402,7 @@ perf_reset_all(void) perf_reset(handle); handle = (perf_counter_t)sq_next(&handle->link); } + for (int i = 0; i <= latency_bucket_count; i++) { + latency_counters[i] = 0; + } } diff --git a/src/modules/systemlib/perf_counter.h b/src/modules/systemlib/perf_counter.h index 668d9dfdf6..d06606a5d0 100644 --- a/src/modules/systemlib/perf_counter.h +++ b/src/modules/systemlib/perf_counter.h @@ -94,7 +94,7 @@ __EXPORT extern void perf_begin(perf_counter_t handle); * End a performance event. * * This call applies to counters that operate over ranges of time; PC_ELAPSED etc. - * If a call is made without a corresopnding perf_begin call, or if perf_cancel + * If a call is made without a corresponding perf_begin call, or if perf_cancel * has been called subsequently, no change is made to the counter. * * @param handle The handle returned from perf_alloc. @@ -142,6 +142,13 @@ __EXPORT extern void perf_print_counter_fd(int fd, perf_counter_t handle); */ __EXPORT extern void perf_print_all(int fd); +/** + * Print hrt latency counters. + * + * @param fd File descriptor to print to - e.g. 0 for stdout + */ +__EXPORT extern void perf_print_latency(int fd); + /** * Reset all of the performance counters. */ diff --git a/src/systemcmds/perf/perf.c b/src/systemcmds/perf/perf.c index b08a2e3b75..a788dfc11c 100644 --- a/src/systemcmds/perf/perf.c +++ b/src/systemcmds/perf/perf.c @@ -68,8 +68,12 @@ int perf_main(int argc, char *argv[]) if (strcmp(argv[1], "reset") == 0) { perf_reset_all(); return 0; + } else if (strcmp(argv[1], "latency") == 0) { + perf_print_latency(0 /* stdout */); + fflush(stdout); + return 0; } - printf("Usage: perf \n"); + printf("Usage: perf [reset | latency]\n"); return -1; } From 2f332f0e9296e57187c152673fe4df5919cd6d87 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Thu, 25 Dec 2014 08:10:21 +0100 Subject: [PATCH 213/216] Integrate the Google Test framework. --- .gitmodules | 3 + unittests/Makefile | 55 ++++++++++++- unittests/gtest | 1 + unittests/sample.cc | 68 ++++++++++++++++ unittests/sample.h | 43 ++++++++++ unittests/sample_unittest.cc | 153 +++++++++++++++++++++++++++++++++++ 6 files changed, 321 insertions(+), 2 deletions(-) create mode 160000 unittests/gtest create mode 100644 unittests/sample.cc create mode 100644 unittests/sample.h create mode 100644 unittests/sample_unittest.cc diff --git a/.gitmodules b/.gitmodules index 17a0984317..0c8c91da4c 100644 --- a/.gitmodules +++ b/.gitmodules @@ -7,3 +7,6 @@ [submodule "uavcan"] path = src/lib/uavcan url = git://github.com/pavel-kirienko/uavcan.git +[submodule "unittests/gtest"] + path = unittests/gtest + url = https://github.com/sjwilks/gtest.git diff --git a/unittests/Makefile b/unittests/Makefile index dfd5ccb3fc..e55411a753 100644 --- a/unittests/Makefile +++ b/unittests/Makefile @@ -3,7 +3,44 @@ CC=g++ CFLAGS=-I. -I../src/modules -I ../src/include -I../src/drivers \ -I../src -I../src/lib -D__EXPORT="" -Dnullptr="0" -lm -all: mixer_test sbus2_test autodeclination_test st24_test sf0x_test +# Points to the root of Google Test, relative to where this file is. +# Remember to tweak this if you move this file. +GTEST_DIR = gtest + +# Flags passed to the preprocessor. +# Set Google Test's header directory as a system directory, such that +# the compiler doesn't generate warnings in Google Test headers. +CFLAGS += -isystem $(GTEST_DIR)/include + +# All Google Test headers. Usually you shouldn't change this +# definition. +GTEST_HEADERS = $(GTEST_DIR)/include/gtest/*.h \ + $(GTEST_DIR)/include/gtest/internal/*.h + +# Usually you shouldn't tweak such internal variables, indicated by a +# trailing _. +GTEST_SRCS_ = $(GTEST_DIR)/src/*.cc $(GTEST_DIR)/src/*.h $(GTEST_HEADERS) + +# For simplicity and to avoid depending on Google Test's +# implementation details, the dependencies specified below are +# conservative and not optimized. This is fine as Google Test +# compiles fast and for ordinary users its source rarely changes. +gtest-all.o: $(GTEST_SRCS_) + $(CC) $(CFLAGS) -I$(GTEST_DIR) -c \ + $(GTEST_DIR)/src/gtest-all.cc + +gtest_main.o: $(GTEST_SRCS_) + $(CC) $(CFLAGS) -I$(GTEST_DIR) -c \ + $(GTEST_DIR)/src/gtest_main.cc + +gtest.a: gtest-all.o + $(AR) $(ARFLAGS) $@ $^ + +gtest_main.a: gtest-all.o gtest_main.o + $(AR) $(ARFLAGS) $@ $^ + + +all: sample_unittest mixer_test sbus2_test autodeclination_test st24_test sf0x_test MIXER_FILES=../src/systemcmds/tests/test_mixer.cpp \ ../src/systemcmds/tests/test_conv.cpp \ @@ -33,6 +70,20 @@ AUTODECLINATION_FILES= ../src/lib/geo_lookup/geo_mag_declination.c \ hrt.cpp \ autodeclination_test.cpp +# Builds a sample test. A test should link with either gtest.a or +# gtest_main.a, depending on whether it defines its own main() +# function. +sample.o: sample.cc sample.h $(GTEST_HEADERS) + $(CC) $(CFLAGS) -c sample.cc + +sample_unittest.o: sample_unittest.cc \ + sample.h $(GTEST_HEADERS) + $(CC) $(CFLAGS) -c sample_unittest.cc + +sample_unittest: sample.o sample_unittest.o gtest_main.a + $(CC) $(CFLAGS) $^ -o $@ + + mixer_test: $(MIXER_FILES) $(CC) -o mixer_test $(MIXER_FILES) $(CFLAGS) @@ -57,4 +108,4 @@ unittests: clean mixer_test sbus2_test sf0x_test autodeclination_test st24_test .PHONY: clean clean: - rm -f $(ODIR)/*.o *~ core $(INCDIR)/*~ mixer_test sbus2_test autodeclination_test st24_test sf0x_test + rm -f gtest.a gtest_main.a *.o $(ODIR)/*.o *~ core $(INCDIR)/*~ sample_unittest mixer_test sbus2_test autodeclination_test st24_test sf0x_test diff --git a/unittests/gtest b/unittests/gtest new file mode 160000 index 0000000000..cdef6e4ce0 --- /dev/null +++ b/unittests/gtest @@ -0,0 +1 @@ +Subproject commit cdef6e4ce097f953445446e8da4cb8bb68478bc5 diff --git a/unittests/sample.cc b/unittests/sample.cc new file mode 100644 index 0000000000..3f0c838f29 --- /dev/null +++ b/unittests/sample.cc @@ -0,0 +1,68 @@ +// Copyright 2005, Google Inc. +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are +// met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above +// copyright notice, this list of conditions and the following disclaimer +// in the documentation and/or other materials provided with the +// distribution. +// * Neither the name of Google Inc. nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR +// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, +// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY +// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +// A sample program demonstrating using Google C++ testing framework. +// +// Author: wan@google.com (Zhanyong Wan) + +#include "sample.h" + +// Returns n! (the factorial of n). For negative n, n! is defined to be 1. +int Factorial(int n) { + int result = 1; + for (int i = 1; i <= n; i++) { + result *= i; + } + + return result; +} + +// Returns true iff n is a prime number. +bool IsPrime(int n) { + // Trivial case 1: small numbers + if (n <= 1) return false; + + // Trivial case 2: even numbers + if (n % 2 == 0) return n == 2; + + // Now, we have that n is odd and n >= 3. + + // Try to divide n by every odd number i, starting from 3 + for (int i = 3; ; i += 2) { + // We only have to try i up to the squre root of n + if (i > n/i) break; + + // Now, we have i <= n/i < n. + // If n is divisible by i, n is not prime. + if (n % i == 0) return false; + } + + // n has no integer factor in the range (1, n), and thus is prime. + return true; +} diff --git a/unittests/sample.h b/unittests/sample.h new file mode 100644 index 0000000000..3dfeb98c45 --- /dev/null +++ b/unittests/sample.h @@ -0,0 +1,43 @@ +// Copyright 2005, Google Inc. +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are +// met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above +// copyright notice, this list of conditions and the following disclaimer +// in the documentation and/or other materials provided with the +// distribution. +// * Neither the name of Google Inc. nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR +// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, +// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY +// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +// A sample program demonstrating using Google C++ testing framework. +// +// Author: wan@google.com (Zhanyong Wan) + +#ifndef GTEST_SAMPLES_SAMPLE1_H_ +#define GTEST_SAMPLES_SAMPLE1_H_ + +// Returns n! (the factorial of n). For negative n, n! is defined to be 1. +int Factorial(int n); + +// Returns true iff n is a prime number. +bool IsPrime(int n); + +#endif // GTEST_SAMPLES_SAMPLE1_H_ diff --git a/unittests/sample_unittest.cc b/unittests/sample_unittest.cc new file mode 100644 index 0000000000..8dc3f5c38f --- /dev/null +++ b/unittests/sample_unittest.cc @@ -0,0 +1,153 @@ +// Copyright 2005, Google Inc. +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are +// met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above +// copyright notice, this list of conditions and the following disclaimer +// in the documentation and/or other materials provided with the +// distribution. +// * Neither the name of Google Inc. nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR +// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, +// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY +// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +// A sample program demonstrating using Google C++ testing framework. +// +// Author: wan@google.com (Zhanyong Wan) + + +// This sample shows how to write a simple unit test for a function, +// using Google C++ testing framework. +// +// Writing a unit test using Google C++ testing framework is easy as 1-2-3: + + +// Step 1. Include necessary header files such that the stuff your +// test logic needs is declared. +// +// Don't forget gtest.h, which declares the testing framework. + +#include +#include "sample.h" +#include "gtest/gtest.h" + + +// Step 2. Use the TEST macro to define your tests. +// +// TEST has two parameters: the test case name and the test name. +// After using the macro, you should define your test logic between a +// pair of braces. You can use a bunch of macros to indicate the +// success or failure of a test. EXPECT_TRUE and EXPECT_EQ are +// examples of such macros. For a complete list, see gtest.h. +// +// +// +// In Google Test, tests are grouped into test cases. This is how we +// keep test code organized. You should put logically related tests +// into the same test case. +// +// The test case name and the test name should both be valid C++ +// identifiers. And you should not use underscore (_) in the names. +// +// Google Test guarantees that each test you define is run exactly +// once, but it makes no guarantee on the order the tests are +// executed. Therefore, you should write your tests in such a way +// that their results don't depend on their order. +// +// + + +// Tests Factorial(). + +// Tests factorial of negative numbers. +TEST(FactorialTest, Negative) { + // This test is named "Negative", and belongs to the "FactorialTest" + // test case. + EXPECT_EQ(1, Factorial(-5)); + EXPECT_EQ(1, Factorial(-1)); + EXPECT_GT(Factorial(-10), 0); + + // + // + // EXPECT_EQ(expected, actual) is the same as + // + // EXPECT_TRUE((expected) == (actual)) + // + // except that it will print both the expected value and the actual + // value when the assertion fails. This is very helpful for + // debugging. Therefore in this case EXPECT_EQ is preferred. + // + // On the other hand, EXPECT_TRUE accepts any Boolean expression, + // and is thus more general. + // + // +} + +// Tests factorial of 0. +TEST(FactorialTest, Zero) { + EXPECT_EQ(1, Factorial(0)); +} + +// Tests factorial of positive numbers. +TEST(FactorialTest, Positive) { + EXPECT_EQ(1, Factorial(1)); + EXPECT_EQ(2, Factorial(2)); + EXPECT_EQ(6, Factorial(3)); + EXPECT_EQ(40320, Factorial(8)); +} + + +// Tests IsPrime() + +// Tests negative input. +TEST(IsPrimeTest, Negative) { + // This test belongs to the IsPrimeTest test case. + + EXPECT_FALSE(IsPrime(-1)); + EXPECT_FALSE(IsPrime(-2)); + EXPECT_FALSE(IsPrime(INT_MIN)); +} + +// Tests some trivial cases. +TEST(IsPrimeTest, Trivial) { + EXPECT_FALSE(IsPrime(0)); + EXPECT_FALSE(IsPrime(1)); + EXPECT_TRUE(IsPrime(2)); + EXPECT_TRUE(IsPrime(3)); +} + +// Tests positive input. +TEST(IsPrimeTest, Positive) { + EXPECT_FALSE(IsPrime(4)); + EXPECT_TRUE(IsPrime(5)); + EXPECT_FALSE(IsPrime(6)); + EXPECT_TRUE(IsPrime(23)); +} + +// Step 3. Call RUN_ALL_TESTS() in main(). +// +// We do this by linking in src/gtest_main.cc file, which consists of +// a main() function which calls RUN_ALL_TESTS() for us. +// +// This runs all the tests you've defined, prints the result, and +// returns 0 if successful, or 1 otherwise. +// +// Did you notice that we didn't register the tests? The +// RUN_ALL_TESTS() macro magically knows about all the tests we +// defined. Isn't this convenient? From 5682526737e9aca75fa30aea8e8f18e49c0b48a7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 26 Dec 2014 00:09:02 +0100 Subject: [PATCH 214/216] NuttX: Update version, addresses clock speed error message. Fixes #1510 --- NuttX | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/NuttX b/NuttX index b66de65069..3c36467c0d 160000 --- a/NuttX +++ b/NuttX @@ -1 +1 @@ -Subproject commit b66de6506941dc7628efcf65e5543c0e3cb05a8f +Subproject commit 3c36467c0d5572431a09ae50013328a4693ee070 From 123d651ff52ea523486cbae98ac13aca8dcfb2b8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 26 Dec 2014 14:47:53 +0100 Subject: [PATCH 215/216] Unit tests: Improve git ignore file --- unittests/.gitignore | 2 ++ 1 file changed, 2 insertions(+) diff --git a/unittests/.gitignore b/unittests/.gitignore index 37e923b5e4..aa316941f4 100644 --- a/unittests/.gitignore +++ b/unittests/.gitignore @@ -1,6 +1,8 @@ ./obj/* +gtest_main.a mixer_test sf0x_test sbus2_test autodeclination_test st24_test +sample_unittest From 5c51adf5f79266de2b483c2461babd4d673cfffb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 26 Dec 2014 14:49:18 +0100 Subject: [PATCH 216/216] bl_update: Improve bootloader error reporting --- src/systemcmds/bl_update/bl_update.c | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/src/systemcmds/bl_update/bl_update.c b/src/systemcmds/bl_update/bl_update.c index 0569d89f50..ec9269d39d 100644 --- a/src/systemcmds/bl_update/bl_update.c +++ b/src/systemcmds/bl_update/bl_update.c @@ -52,6 +52,8 @@ #include "systemlib/systemlib.h" #include "systemlib/err.h" +#define BL_FILE_SIZE_LIMIT 16384 + __EXPORT int bl_update_main(int argc, char *argv[]); static void setopt(void); @@ -72,12 +74,12 @@ bl_update_main(int argc, char *argv[]) struct stat s; - if (stat(argv[1], &s) < 0) + if (!stat(argv[1], &s)) err(1, "stat %s", argv[1]); /* sanity-check file size */ - if (s.st_size > 16384) - errx(1, "%s: file too large", argv[1]); + if (s.st_size > BL_FILE_SIZE_LIMIT) + errx(1, "%s: file too large (limit: %u, actual: %d)", argv[1], BL_FILE_SIZE_LIMIT, s.st_size); uint8_t *buf = malloc(s.st_size);