From 034d0a6a610c8838d6c43b51a4401ce818b77624 Mon Sep 17 00:00:00 2001 From: Luis Rodrigues Date: Wed, 23 Jul 2014 18:19:49 +0200 Subject: [PATCH 001/652] 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/652] 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/652] 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/652] 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 03e6dbed80f9739c44626c1c04508a1c522ffba5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 13 Aug 2014 18:47:07 +0200 Subject: [PATCH 005/652] Start TELEM2 MAVLink instance with 230400 baud --- ROMFS/px4fmu_common/init.d/rcS | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index c9e6a27cac..11e6995037 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -447,6 +447,11 @@ then mavlink start $MAVLINK_FLAGS + # + # MAVLink onboard / TELEM2 + # + mavlink start -d /dev/ttyS2 -b 230400 + # # Sensors, Logging, GPS # From 58aabe648acf296267bd1e29e8a02b33ea9e7cf7 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 21 Aug 2014 11:25:09 +0200 Subject: [PATCH 006/652] 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 8a2089e257e6e3762601aad6eaca3245cbd59a46 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 21 Aug 2014 14:30:17 +0200 Subject: [PATCH 007/652] Enable TELEM2 MAVLink, forward local pos at 30 Hz if connected via USB --- ROMFS/px4fmu_common/init.d/rc.usb | 1 + ROMFS/px4fmu_common/init.d/rcS | 5 ++++- 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.usb b/ROMFS/px4fmu_common/init.d/rc.usb index ee040a706d..b3de8e5907 100644 --- a/ROMFS/px4fmu_common/init.d/rc.usb +++ b/ROMFS/px4fmu_common/init.d/rc.usb @@ -25,6 +25,7 @@ mavlink stream -d /dev/ttyACM0 -s SERVO_OUTPUT_RAW_0 -r 20 usleep 100000 mavlink stream -d /dev/ttyACM0 -s POSITION_TARGET_GLOBAL_INT -r 10 usleep 100000 +mavlink stream -d /dev/ttyACM0 -s LOCAL_POSITION_NED -r 30 # Exit shell to make it available to MAVLink exit diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 11e6995037..5f7a205dcb 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -450,7 +450,10 @@ then # # MAVLink onboard / TELEM2 # - mavlink start -d /dev/ttyS2 -b 230400 + if ver hwcmp PX4FMU_V2 + then + mavlink start -d /dev/ttyS2 -b 230400 + fi # # Sensors, Logging, GPS From fcebafba778ff981f1b485d7cb30fed5dce6295c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 22 Aug 2014 09:24:37 +0200 Subject: [PATCH 008/652] Obey vision estimates in attitude estimator if available --- .../attitude_estimator_ekf_main.cpp | 52 +++++++++++++------ 1 file changed, 36 insertions(+), 16 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..6bbc26a650 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -62,6 +62,7 @@ #include #include #include +#include #include #include @@ -206,10 +207,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 */ @@ -222,6 +219,8 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds memset(&raw, 0, sizeof(raw)); struct vehicle_gps_position_s gps; memset(&gps, 0, sizeof(gps)); + gps.eph = 100000; + gps.epv = 100000; struct vehicle_global_position_s global_pos; memset(&global_pos, 0, sizeof(global_pos)); struct vehicle_attitude_s att; @@ -260,9 +259,12 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds /* subscribe to param changes */ int sub_params = orb_subscribe(ORB_ID(parameter_update)); - /* subscribe to control mode*/ + /* subscribe to control mode */ int sub_control_mode = orb_subscribe(ORB_ID(vehicle_control_mode)); + /* subscribe to vision estimate */ + int vision_sub = orb_subscribe(ORB_ID(vision_position_estimate)); + /* advertise attitude */ orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att); @@ -270,16 +272,12 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds thread_running = true; - /* advertise debug value */ - // struct debug_key_value_s dbg = { .key = "", .value = 0.0f }; - // orb_advert_t pub_dbg = -1; - /* 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 {}; - struct attitude_estimator_ekf_param_handles ekf_param_handles; + struct attitude_estimator_ekf_param_handles ekf_param_handles {}; /* initialize parameter handles */ parameters_init(&ekf_param_handles); @@ -295,6 +293,8 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds math::Matrix<3, 3> R_decl; R_decl.identity(); + struct vision_position_estimate vision {}; + /* register the perf counter */ perf_counter_t ekf_loop_perf = perf_alloc(PC_ELAPSED, "attitude_estimator_ekf"); @@ -315,8 +315,7 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds orb_copy(ORB_ID(vehicle_control_mode), sub_control_mode, &control_mode); if (!control_mode.flag_system_hil_enabled) { - fprintf(stderr, - "[att ekf] WARNING: Not getting sensors - sensor app running?\n"); + warnx("WARNING: Not getting sensors - sensor app running?"); } } else { @@ -451,9 +450,30 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds sensor_last_timestamp[2] = raw.magnetometer_timestamp; } - z_k[6] = raw.magnetometer_ga[0]; - z_k[7] = raw.magnetometer_ga[1]; - z_k[8] = raw.magnetometer_ga[2]; + bool vision_updated = false; + orb_check(vision_sub, &vision_updated); + + if (vision_updated) { + orb_copy(ORB_ID(vision_position_estimate), vision_sub, &vision); + } + + if (vision.timestamp_boot > 0 && (hrt_elapsed_time(&vision.timestamp_boot) < 500000) && gps.eph > 3.0f) { + + math::Quaternion q(vision.q); + math::Matrix<3, 3> R = q.to_dcm(); + + math::Vector<3> v(1.0f, 0.0f, 0.4f); + + math::Vector<3> vn = R * v; + + z_k[6] = vn(0); + z_k[7] = vn(1); + z_k[8] = vn(2); + } else { + z_k[6] = raw.magnetometer_ga[0]; + z_k[7] = raw.magnetometer_ga[1]; + z_k[8] = raw.magnetometer_ga[2]; + } uint64_t now = hrt_absolute_time(); unsigned int time_elapsed = now - last_run; From 368afa1c1d1cad13b4dfaa3f7215dd399300d817 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 27 Aug 2014 09:47:31 +0200 Subject: [PATCH 009/652] Start the laser on a port not offending the 2nd MAVLink instance --- src/drivers/sf0x/sf0x.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp index 80ecab2ee1..5f7c0a84a7 100644 --- a/src/drivers/sf0x/sf0x.cpp +++ b/src/drivers/sf0x/sf0x.cpp @@ -88,7 +88,9 @@ static const int ERROR = -1; #define SF0X_TAKE_RANGE_REG 'd' #define SF02F_MIN_DISTANCE 0.0f #define SF02F_MAX_DISTANCE 40.0f -#define SF0X_DEFAULT_PORT "/dev/ttyS2" + +// designated SERIAL4/5 on Pixhawk +#define SF0X_DEFAULT_PORT "/dev/ttyS6" class SF0X : public device::CDev { From 0df878d2d5dd0a4632a8c6f5399d053dee49fe59 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 27 Aug 2014 09:49:19 +0200 Subject: [PATCH 010/652] Start flow driver, start MAVLink instance with right arguments --- ROMFS/px4fmu_common/init.d/rc.sensors | 5 +++++ ROMFS/px4fmu_common/init.d/rcS | 2 +- 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index ecb408a543..739df7ac0b 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -68,6 +68,11 @@ else fi fi +# Check for flow sensor +if px4flow start +then +fi + # # Start the sensor collection task. # IMPORTANT: this also loads param offsets diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 6aa542a295..fa5e8bf7c3 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -450,7 +450,7 @@ then # if ver hwcmp PX4FMU_V2 then - mavlink start -d /dev/ttyS2 -b 230400 + mavlink start -d /dev/ttyS2 -b 230400 -m onboard fi # UAVCAN From 8eb5aeaae3154f3faa6ac493ddc3d6aefdbaecc0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 1 Sep 2014 21:04:27 +0200 Subject: [PATCH 011/652] Use vision for heading reference whenever available, independent of GPS --- .../attitude_estimator_ekf/attitude_estimator_ekf_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 6bbc26a650..cd7c56b878 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -457,7 +457,7 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds orb_copy(ORB_ID(vision_position_estimate), vision_sub, &vision); } - if (vision.timestamp_boot > 0 && (hrt_elapsed_time(&vision.timestamp_boot) < 500000) && gps.eph > 3.0f) { + if (vision.timestamp_boot > 0 && (hrt_elapsed_time(&vision.timestamp_boot) < 500000)) { math::Quaternion q(vision.q); math::Matrix<3, 3> R = q.to_dcm(); From 428b9612abd3ca892a7240d284916c50e2496cea Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 1 Sep 2014 21:04:44 +0200 Subject: [PATCH 012/652] INAV: Adjust vision default params --- .../position_estimator_inav/position_estimator_inav_main.c | 4 ++-- .../position_estimator_inav/position_estimator_inav_params.c | 2 +- 2 files changed, 3 insertions(+), 3 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..3f59eb5970 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -235,8 +235,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float eph_flow = 1.0f; - float eph_vision = 0.5f; - float epv_vision = 0.5f; + float eph_vision = 0.2f; + float epv_vision = 0.2f; float x_est_prev[2], y_est_prev[2], z_est_prev[2]; memset(x_est_prev, 0, sizeof(x_est_prev)); diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.c b/src/modules/position_estimator_inav/position_estimator_inav_params.c index cc0fb4155e..b7f6509d73 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -116,7 +116,7 @@ PARAM_DEFINE_FLOAT(INAV_W_XY_GPS_V, 2.0f); * @max 10.0 * @group Position Estimator INAV */ -PARAM_DEFINE_FLOAT(INAV_W_XY_VIS_P, 5.0f); +PARAM_DEFINE_FLOAT(INAV_W_XY_VIS_P, 7.0f); /** * XY axis weight for vision velocity From 821c06f7cc58b50afe80442ee2258bf99cbe1fd2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 5 Sep 2014 17:46:02 +0200 Subject: [PATCH 013/652] Support speed estimate --- .../position_estimator_inav_main.c | 37 +++++++++++++++++-- 1 file changed, 33 insertions(+), 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 3f59eb5970..6160200f39 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -641,6 +641,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(vision_position_estimate), vision_position_estimate_sub, &vision); + static float last_vision_x = 0.0f; + static float last_vision_y = 0.0f; + static float last_vision_z = 0.0f; + /* reset position estimate on first vision update */ if (!vision_valid) { x_est[0] = vision.x; @@ -655,6 +659,11 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } vision_valid = true; + + last_vision_x = vision.x; + last_vision_y = vision.y; + last_vision_z = vision.z; + warnx("VISION estimate valid"); mavlink_log_info(mavlink_fd, "[inav] VISION estimate valid"); } @@ -664,10 +673,30 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) corr_vision[1][0] = vision.y - y_est[0]; corr_vision[2][0] = vision.z - z_est[0]; - /* calculate correction for velocity */ - corr_vision[0][1] = vision.vx - x_est[1]; - corr_vision[1][1] = vision.vy - y_est[1]; - corr_vision[2][1] = vision.vz - z_est[1]; + static hrt_abstime last_vision_time = 0; + + float vision_dt = (vision.timestamp_boot - last_vision_time) / 1e6f; + last_vision_time = vision.timestamp_boot; + + if (vision_dt > 0.000001f && vision_dt < 0.2f) { + vision.vx = (vision.x - last_vision_x) / vision_dt; + vision.vy = (vision.y - last_vision_y) / vision_dt; + vision.vz = (vision.z - last_vision_z) / vision_dt; + + last_vision_x = vision.x; + last_vision_y = vision.y; + last_vision_z = vision.z; + + /* calculate correction for velocity */ + corr_vision[0][1] = vision.vx - x_est[1]; + corr_vision[1][1] = vision.vy - y_est[1]; + corr_vision[2][1] = vision.vz - z_est[1]; + } else { + /* assume zero motion */ + corr_vision[0][1] = 0.0f - x_est[1]; + corr_vision[1][1] = 0.0f - y_est[1]; + corr_vision[2][1] = 0.0f - z_est[1]; + } } } From 2c023c5d015fc63f7499a3b60751c771ec8a72b1 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 16 Oct 2014 22:52:08 +0200 Subject: [PATCH 014/652] 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 22f1b784525622ac39c9db50675c3937c7aeecf0 Mon Sep 17 00:00:00 2001 From: Denis Yeldandi Date: Fri, 17 Oct 2014 21:07:34 +0400 Subject: [PATCH 015/652] Adding vertical (Z) velocity to inav estimator --- .../position_estimator_inav_main.c | 3 +++ .../position_estimator_inav_params.c | 12 ++++++++++++ .../position_estimator_inav_params.h | 2 ++ 3 files changed, 17 insertions(+) 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..a1bfa2dfc1 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -846,6 +846,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float w_xy_gps_p = params.w_xy_gps_p * w_gps_xy; float w_xy_gps_v = params.w_xy_gps_v * w_gps_xy; float w_z_gps_p = params.w_z_gps_p * w_gps_z; + float w_z_gps_v = params.w_z_gps_v * w_gps_z; float w_xy_vision_p = params.w_xy_vision_p; float w_xy_vision_v = params.w_xy_vision_v; @@ -876,6 +877,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (use_gps_z) { accel_bias_corr[2] -= corr_gps[2][0] * w_z_gps_p * w_z_gps_p; + accel_bias_corr[2] -= corr_gps[2][1] * w_z_gps_v; } /* transform error vector from NED frame to body frame */ @@ -960,6 +962,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) epv = fminf(epv, gps.epv); inertial_filter_correct(corr_gps[2][0], dt, z_est, 0, w_z_gps_p); + inertial_filter_correct(corr_gps[2][1], dt, z_est, 1, w_z_gps_v); } if (use_vision_z) { diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.c b/src/modules/position_estimator_inav/position_estimator_inav_params.c index cc0fb4155e..1a1b3d3102 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -63,6 +63,17 @@ PARAM_DEFINE_FLOAT(INAV_W_Z_BARO, 0.5f); */ PARAM_DEFINE_FLOAT(INAV_W_Z_GPS_P, 0.005f); +/** + * Z velocity weight for GPS + * + * Weight (cutoff frequency) for GPS altitude velocity measurements. + * + * @min 0.0 + * @max 10.0 + * @group Position Estimator INAV + */ +PARAM_DEFINE_FLOAT(INAV_W_Z_GPS_V, 1.0f); + /** * Z axis weight for vision * @@ -281,6 +292,7 @@ int parameters_init(struct position_estimator_inav_param_handles *h) { h->w_z_baro = param_find("INAV_W_Z_BARO"); h->w_z_gps_p = param_find("INAV_W_Z_GPS_P"); + h->w_z_gps_v = param_find("INAV_W_Z_GPS_V"); h->w_z_vision_p = param_find("INAV_W_Z_VIS_P"); h->w_z_sonar = param_find("INAV_W_Z_SONAR"); h->w_xy_gps_p = param_find("INAV_W_XY_GPS_P"); diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.h b/src/modules/position_estimator_inav/position_estimator_inav_params.h index d0a65e42ea..51bbda412a 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.h +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.h @@ -44,6 +44,7 @@ struct position_estimator_inav_params { float w_z_baro; float w_z_gps_p; + float w_z_gps_v; float w_z_vision_p; float w_z_sonar; float w_xy_gps_p; @@ -68,6 +69,7 @@ struct position_estimator_inav_params { struct position_estimator_inav_param_handles { param_t w_z_baro; param_t w_z_gps_p; + param_t w_z_gps_v; param_t w_z_vision_p; param_t w_z_sonar; param_t w_xy_gps_p; From 5c77fc0012eaaad5b92e1b31cedf8bf5b035b988 Mon Sep 17 00:00:00 2001 From: philipoe Date: Mon, 20 Oct 2014 15:55:45 +0200 Subject: [PATCH 016/652] 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 017/652] 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 018/652] 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 019/652] 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 020/652] 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 021/652] 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 022/652] 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 023/652] 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 024/652] 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 025/652] 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 026/652] 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 027/652] 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 028/652] 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 029/652] 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 030/652] 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 031/652] 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 032/652] 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 4d489ef7f4c7626a86b700b6d7144e35000ce387 Mon Sep 17 00:00:00 2001 From: Holger Steinhaus Date: Tue, 11 Nov 2014 11:19:15 +0100 Subject: [PATCH 033/652] UAVCAN: improve mag compatibility --- src/modules/uavcan/sensors/mag.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/modules/uavcan/sensors/mag.cpp b/src/modules/uavcan/sensors/mag.cpp index 0d9ea08c5f..4f56038b53 100644 --- a/src/modules/uavcan/sensors/mag.cpp +++ b/src/modules/uavcan/sensors/mag.cpp @@ -74,6 +74,9 @@ int UavcanMagnetometerBridge::init() int UavcanMagnetometerBridge::ioctl(struct file *filp, int cmd, unsigned long arg) { switch (cmd) { + case SENSORIOCSQUEUEDEPTH: { + return OK; // Pretend that this stuff is supported to keep APM happy + } case MAGIOCSSCALE: { std::memcpy(&_scale, reinterpret_cast(arg), sizeof(_scale)); return 0; From 0fa622f22b90048ad366ac304c2c9339a5c376d3 Mon Sep 17 00:00:00 2001 From: Holger Steinhaus Date: Tue, 11 Nov 2014 11:20:03 +0100 Subject: [PATCH 034/652] UAVCAN: declare mag external again to allow different rotation than the internal mag --- src/modules/uavcan/sensors/mag.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/uavcan/sensors/mag.cpp b/src/modules/uavcan/sensors/mag.cpp index 4f56038b53..4d473291c4 100644 --- a/src/modules/uavcan/sensors/mag.cpp +++ b/src/modules/uavcan/sensors/mag.cpp @@ -89,7 +89,7 @@ int UavcanMagnetometerBridge::ioctl(struct file *filp, int cmd, unsigned long ar return 0; // Nothing to do } case MAGIOCGEXTERNAL: { - return 0; // We don't want anyone to transform the coordinate frame, so we declare it onboard + return 1; // We don't want anyone to transform the coordinate frame, so we declare it onboard } case MAGIOCSSAMPLERATE: { return 0; // Pretend that this stuff is supported to keep the sensor app happy From 16d74e3c31b9c2cf4f2de254956a58885b5c1fbf Mon Sep 17 00:00:00 2001 From: Holger Steinhaus Date: Wed, 12 Nov 2014 13:14:05 +0100 Subject: [PATCH 035/652] UACVAN: add read()-style interface to mag device --- src/modules/uavcan/sensors/mag.cpp | 48 +++++++++++++++++++++++------- src/modules/uavcan/sensors/mag.hpp | 2 ++ 2 files changed, 39 insertions(+), 11 deletions(-) diff --git a/src/modules/uavcan/sensors/mag.cpp b/src/modules/uavcan/sensors/mag.cpp index 4d473291c4..a529e7e759 100644 --- a/src/modules/uavcan/sensors/mag.cpp +++ b/src/modules/uavcan/sensors/mag.cpp @@ -37,6 +37,8 @@ #include "mag.hpp" +#include + static const orb_id_t MAG_TOPICS[3] = { ORB_ID(sensor_mag0), ORB_ID(sensor_mag1), @@ -71,6 +73,30 @@ int UavcanMagnetometerBridge::init() return 0; } +ssize_t UavcanMagnetometerBridge::read(struct file *filp, char *buffer, size_t buflen) +{ + static uint64_t last_read = 0; + struct mag_report *mag_buf = reinterpret_cast(buffer); + + /* buffer must be large enough */ + unsigned count = buflen / sizeof(struct mag_report); + if (count < 1) { + return -ENOSPC; + } + + if (last_read < _report.timestamp) { + /* copy report */ + lock(); + *mag_buf = _report; + last_read = _report.timestamp; + unlock(); + return sizeof(struct mag_report); + } else { + /* no new data available, warn caller */ + return -EAGAIN; + } +} + int UavcanMagnetometerBridge::ioctl(struct file *filp, int cmd, unsigned long arg) { switch (cmd) { @@ -89,7 +115,7 @@ int UavcanMagnetometerBridge::ioctl(struct file *filp, int cmd, unsigned long ar return 0; // Nothing to do } case MAGIOCGEXTERNAL: { - return 1; // We don't want anyone to transform the coordinate frame, so we declare it onboard + return 1; // declare it external rise it's priority and to allow for correct orientation compensation } case MAGIOCSSAMPLERATE: { return 0; // Pretend that this stuff is supported to keep the sensor app happy @@ -111,18 +137,18 @@ int UavcanMagnetometerBridge::ioctl(struct file *filp, int cmd, unsigned long ar void UavcanMagnetometerBridge::mag_sub_cb(const uavcan::ReceivedDataStructure &msg) { - auto report = ::mag_report(); + lock(); + _report.range_ga = 1.3F; // Arbitrary number, doesn't really mean anything - report.range_ga = 1.3F; // Arbitrary number, doesn't really mean anything - - report.timestamp = msg.getUtcTimestamp().toUSec(); - if (report.timestamp == 0) { - report.timestamp = msg.getMonotonicTimestamp().toUSec(); + _report.timestamp = msg.getUtcTimestamp().toUSec(); + if (_report.timestamp == 0) { + _report.timestamp = msg.getMonotonicTimestamp().toUSec(); } - report.x = (msg.magnetic_field[0] - _scale.x_offset) * _scale.x_scale; - report.y = (msg.magnetic_field[1] - _scale.y_offset) * _scale.y_scale; - report.z = (msg.magnetic_field[2] - _scale.z_offset) * _scale.z_scale; + _report.x = (msg.magnetic_field[0] - _scale.x_offset) * _scale.x_scale; + _report.y = (msg.magnetic_field[1] - _scale.y_offset) * _scale.y_scale; + _report.z = (msg.magnetic_field[2] - _scale.z_offset) * _scale.z_scale; + unlock(); - publish(msg.getSrcNodeID().get(), &report); + publish(msg.getSrcNodeID().get(), &_report); } diff --git a/src/modules/uavcan/sensors/mag.hpp b/src/modules/uavcan/sensors/mag.hpp index 6d413a8f71..74077d883e 100644 --- a/src/modules/uavcan/sensors/mag.hpp +++ b/src/modules/uavcan/sensors/mag.hpp @@ -54,6 +54,7 @@ public: int init() override; private: + ssize_t read(struct file *filp, char *buffer, size_t buflen); int ioctl(struct file *filp, int cmd, unsigned long arg) override; void mag_sub_cb(const uavcan::ReceivedDataStructure &msg); @@ -65,4 +66,5 @@ private: uavcan::Subscriber _sub_mag; mag_scale _scale = {}; + mag_report _report = {}; }; From e026324784409bd16a561544cb608e7089d500a0 Mon Sep 17 00:00:00 2001 From: Holger Steinhaus Date: Thu, 13 Nov 2014 16:58:41 +0100 Subject: [PATCH 036/652] UAVCAN: fix mag report timestamp --- src/modules/uavcan/sensors/mag.cpp | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/src/modules/uavcan/sensors/mag.cpp b/src/modules/uavcan/sensors/mag.cpp index a529e7e759..5cbb96433b 100644 --- a/src/modules/uavcan/sensors/mag.cpp +++ b/src/modules/uavcan/sensors/mag.cpp @@ -37,6 +37,7 @@ #include "mag.hpp" +#include #include static const orb_id_t MAG_TOPICS[3] = { @@ -139,11 +140,7 @@ void UavcanMagnetometerBridge::mag_sub_cb(const uavcan::ReceivedDataStructure Date: Mon, 17 Nov 2014 14:56:11 +0100 Subject: [PATCH 037/652] 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 038/652] 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 039/652] 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 040/652] 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 041/652] 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 5f6d03099e07e20abe9cec685fee1244d7c9a0d4 Mon Sep 17 00:00:00 2001 From: Steven Blass Date: Mon, 17 Nov 2014 19:58:18 -0500 Subject: [PATCH 042/652] fixed yaw/yawrate bit masking. fixed navigator overriding offboard setpoint --- src/modules/mavlink/mavlink_receiver.cpp | 4 ++-- src/modules/navigator/navigator_main.cpp | 7 ++++--- 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index bc092c7e9c..c0a8d7322f 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -540,10 +540,10 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t } offboard_control_sp.ignore &= ~(1 << OFB_IGN_BIT_YAW); offboard_control_sp.ignore |= (set_position_target_local_ned.type_mask & (1 << 10)) << - OFB_IGN_BIT_YAW; + (OFB_IGN_BIT_YAW - 10); offboard_control_sp.ignore &= ~(1 << OFB_IGN_BIT_YAWRATE); offboard_control_sp.ignore |= (set_position_target_local_ned.type_mask & (1 << 11)) << - OFB_IGN_BIT_YAWRATE; + (OFB_IGN_BIT_YAWRATE - 11); offboard_control_sp.timestamp = hrt_absolute_time(); diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 10a4ee88f6..5e98064996 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -468,9 +468,10 @@ Navigator::task_main() _navigation_mode_array[i]->run(_navigation_mode == _navigation_mode_array[i]); } - /* if nothing is running, set position setpoint triplet invalid */ - if (_navigation_mode == nullptr) { - // TODO publish empty sp only once + /* if nothing is running, set position setpoint triplet invalid once */ + static bool published_once = false; + if (_navigation_mode == nullptr && !published_once) { + published_once = true; _pos_sp_triplet.previous.valid = false; _pos_sp_triplet.current.valid = false; _pos_sp_triplet.next.valid = false; From 4eb930f704018253a4cff3604cb9faee8468283b Mon Sep 17 00:00:00 2001 From: Steven Blass Date: Tue, 18 Nov 2014 08:26:29 -0500 Subject: [PATCH 043/652] improved readability of offboard fixes --- src/modules/mavlink/mavlink_receiver.cpp | 12 ++++++++---- src/modules/navigator/navigator.h | 1 + src/modules/navigator/navigator_main.cpp | 6 +++--- 3 files changed, 12 insertions(+), 7 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index c0a8d7322f..ca00d1a67d 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -538,12 +538,16 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t offboard_control_sp.ignore &= ~(1 << i); offboard_control_sp.ignore |= (set_position_target_local_ned.type_mask & (1 << i)); } + offboard_control_sp.ignore &= ~(1 << OFB_IGN_BIT_YAW); - offboard_control_sp.ignore |= (set_position_target_local_ned.type_mask & (1 << 10)) << - (OFB_IGN_BIT_YAW - 10); + if (set_position_target_local_ned.type_mask & (1 << 10)) { + offboard_control_sp.ignore |= (1 << OFB_IGN_BIT_YAW); + } + offboard_control_sp.ignore &= ~(1 << OFB_IGN_BIT_YAWRATE); - offboard_control_sp.ignore |= (set_position_target_local_ned.type_mask & (1 << 11)) << - (OFB_IGN_BIT_YAWRATE - 11); + if (set_position_target_local_ned.type_mask & (1 << 11)) { + offboard_control_sp.ignore |= (1 << OFB_IGN_BIT_YAWRATE); + } offboard_control_sp.timestamp = hrt_absolute_time(); diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index d550dcc4cb..9cd609955b 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -205,6 +205,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 */ control::BlockParamFloat _param_loiter_radius; /**< loiter radius for fixedwing */ control::BlockParamFloat _param_acceptance_radius; /**< acceptance for takeoff */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 5e98064996..035b0d18c8 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -137,6 +137,7 @@ Navigator::Navigator() : _gpsFailure(this, "GPSF"), _can_loiter_at_sp(false), _pos_sp_triplet_updated(false), + _pos_sp_triplet_published_invalid_once(false), _param_loiter_radius(this, "LOITER_RAD"), _param_acceptance_radius(this, "ACC_RAD"), _param_datalinkloss_obc(this, "DLL_OBC"), @@ -469,9 +470,8 @@ Navigator::task_main() } /* if nothing is running, set position setpoint triplet invalid once */ - static bool published_once = false; - if (_navigation_mode == nullptr && !published_once) { - published_once = true; + if (_navigation_mode == nullptr && !_pos_sp_triplet_published_invalid_once) { + _pos_sp_triplet_published_invalid_once = true; _pos_sp_triplet.previous.valid = false; _pos_sp_triplet.current.valid = false; _pos_sp_triplet.next.valid = false; From 69271a7251a0bcfd980c3534194b73fc45fa5f23 Mon Sep 17 00:00:00 2001 From: Steven Blass Date: Tue, 18 Nov 2014 08:40:52 -0500 Subject: [PATCH 044/652] made invalid setpoints publish once every time it enters an invalid state --- src/modules/navigator/navigator_main.cpp | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 035b0d18c8..df620e5e7f 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -428,12 +428,15 @@ Navigator::task_main() _can_loiter_at_sp = false; break; case NAVIGATION_STATE_AUTO_MISSION: + _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_mission; break; case NAVIGATION_STATE_AUTO_LOITER: + _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_loiter; break; case NAVIGATION_STATE_AUTO_RCRECOVER: + _pos_sp_triplet_published_invalid_once = false; if (_param_rcloss_obc.get() != 0) { _navigation_mode = &_rcLoss; } else { @@ -441,11 +444,13 @@ Navigator::task_main() } break; case NAVIGATION_STATE_AUTO_RTL: - _navigation_mode = &_rtl; + _pos_sp_triplet_published_invalid_once = false; + _navigation_mode = &_rtl; break; case NAVIGATION_STATE_AUTO_RTGS: /* Use complex data link loss mode only when enabled via param * otherwise use rtl */ + _pos_sp_triplet_published_invalid_once = false; if (_param_datalinkloss_obc.get() != 0) { _navigation_mode = &_dataLinkLoss; } else { @@ -453,9 +458,11 @@ Navigator::task_main() } break; case NAVIGATION_STATE_AUTO_LANDENGFAIL: + _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_engineFailure; break; case NAVIGATION_STATE_AUTO_LANDGPSFAIL: + _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_gpsFailure; break; default: From 2ce2d26d52884f868ecbcb102f38cbc853c2dbf6 Mon Sep 17 00:00:00 2001 From: Holger Steinhaus Date: Tue, 18 Nov 2014 14:58:04 +0100 Subject: [PATCH 045/652] UAVCAN: preserve original UAVCAN message timestamps --- src/modules/uavcan/sensors/baro.cpp | 6 +----- src/modules/uavcan/sensors/gnss.cpp | 2 +- src/modules/uavcan/sensors/gnss.hpp | 2 -- src/modules/uavcan/sensors/mag.cpp | 3 +-- 4 files changed, 3 insertions(+), 10 deletions(-) diff --git a/src/modules/uavcan/sensors/baro.cpp b/src/modules/uavcan/sensors/baro.cpp index 80c5e3828e..8741ae20dd 100644 --- a/src/modules/uavcan/sensors/baro.cpp +++ b/src/modules/uavcan/sensors/baro.cpp @@ -91,11 +91,7 @@ void UavcanBarometerBridge::air_data_sub_cb(const uavcan::ReceivedDataStructure< { auto report = ::baro_report(); - report.timestamp = msg.getUtcTimestamp().toUSec(); - if (report.timestamp == 0) { - report.timestamp = msg.getMonotonicTimestamp().toUSec(); - } - + report.timestamp = msg.getMonotonicTimestamp().toUSec(); report.temperature = msg.static_temperature; report.pressure = msg.static_pressure / 100.0F; // Convert to millibar diff --git a/src/modules/uavcan/sensors/gnss.cpp b/src/modules/uavcan/sensors/gnss.cpp index 24afe6aaf4..a375db37f8 100644 --- a/src/modules/uavcan/sensors/gnss.cpp +++ b/src/modules/uavcan/sensors/gnss.cpp @@ -92,7 +92,7 @@ void UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure - #include #include diff --git a/src/modules/uavcan/sensors/mag.cpp b/src/modules/uavcan/sensors/mag.cpp index 5cbb96433b..35ebee5426 100644 --- a/src/modules/uavcan/sensors/mag.cpp +++ b/src/modules/uavcan/sensors/mag.cpp @@ -37,7 +37,6 @@ #include "mag.hpp" -#include #include static const orb_id_t MAG_TOPICS[3] = { @@ -140,7 +139,7 @@ void UavcanMagnetometerBridge::mag_sub_cb(const uavcan::ReceivedDataStructure Date: Tue, 18 Nov 2014 17:20:50 -0800 Subject: [PATCH 046/652] Parameter xml metadata in .px4 --- Tools/px_mkfw.py | 5 +++++ makefiles/config_px4fmu-v1_default.mk | 3 +++ makefiles/config_px4fmu-v2_default.mk | 3 +++ makefiles/firmware.mk | 9 +++++++++ 4 files changed, 20 insertions(+) diff --git a/Tools/px_mkfw.py b/Tools/px_mkfw.py index b598a65a1a..c2da8a2038 100755 --- a/Tools/px_mkfw.py +++ b/Tools/px_mkfw.py @@ -73,6 +73,7 @@ parser.add_argument("--version", action="store", help="set a version string") parser.add_argument("--summary", action="store", help="set a brief description") parser.add_argument("--description", action="store", help="set a longer description") parser.add_argument("--git_identity", action="store", help="the working directory to check for git identity") +parser.add_argument("--parameter_xml", action="store", help="the parameters.xml file") parser.add_argument("--image", action="store", help="the firmware image") args = parser.parse_args() @@ -101,6 +102,10 @@ if args.git_identity != None: p = subprocess.Popen(cmd, shell=True, stdout=subprocess.PIPE).stdout desc['git_identity'] = str(p.read().strip()) p.close() +if args.parameter_xml != None: + f = open(args.parameter_xml, "rb") + bytes = f.read() + desc['parameter_xml'] = base64.b64encode(zlib.compress(bytes,9)).decode('utf-8') if args.image != None: f = open(args.image, "rb") bytes = f.read() diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index 9fe16fbb6c..4507b506c6 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -132,6 +132,9 @@ MODULES += lib/launchdetection # Hardware test #MODULES += examples/hwtest +# Generate parameter XML file +GEN_PARAM_XML = 1 + # # Transitional support - add commands from the NuttX export archive. # diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index 3c65b19e02..d3b8ee93e8 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -141,6 +141,9 @@ MODULES += modules/bottle_drop # Hardware test #MODULES += examples/hwtest +# Generate parameter XML file +GEN_PARAM_XML = 1 + # # Transitional support - add commands from the NuttX export archive. # diff --git a/makefiles/firmware.mk b/makefiles/firmware.mk index 60602e76fe..21e8739aa1 100644 --- a/makefiles/firmware.mk +++ b/makefiles/firmware.mk @@ -467,6 +467,7 @@ endif PRODUCT_BUNDLE = $(WORK_DIR)firmware.px4 PRODUCT_BIN = $(WORK_DIR)firmware.bin PRODUCT_ELF = $(WORK_DIR)firmware.elf +PRODUCT_PARAMXML = $(WORK_DIR)/parameters.xml .PHONY: firmware firmware: $(PRODUCT_BUNDLE) @@ -497,9 +498,17 @@ $(filter %.S.o,$(OBJS)): $(WORK_DIR)%.S.o: %.S $(GLOBAL_DEPS) $(PRODUCT_BUNDLE): $(PRODUCT_BIN) @$(ECHO) %% Generating $@ +ifdef GEN_PARAM_XML + python $(PX4_BASE)/Tools/px_process_params.py --src-path $(PX4_BASE)/src --xml + $(Q) $(MKFW) --prototype $(IMAGE_DIR)/$(BOARD).prototype \ + --git_identity $(PX4_BASE) \ + --parameter_xml $(PRODUCT_PARAMXML) \ + --image $< > $@ +else $(Q) $(MKFW) --prototype $(IMAGE_DIR)/$(BOARD).prototype \ --git_identity $(PX4_BASE) \ --image $< > $@ +endif $(PRODUCT_BIN): $(PRODUCT_ELF) $(call SYM_TO_BIN,$<,$@) From 30d851284606476642c6cf8a54070bc08ab923c6 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 19 Nov 2014 12:31:04 +0100 Subject: [PATCH 047/652] 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 048/652] 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 049/652] 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 050/652] 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 051/652] 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 052/652] 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 053/652] 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 054/652] 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 055/652] 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 056/652] 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 057/652] 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 058/652] 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 059/652] 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 060/652] 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 061/652] 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 062/652] 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 063/652] 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 064/652] 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 065/652] 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 066/652] 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 067/652] 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 068/652] 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 069/652] 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 070/652] 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 071/652] 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 072/652] 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 073/652] 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 074/652] 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 075/652] 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 076/652] 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 077/652] 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 078/652] 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 079/652] 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 080/652] 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 081/652] 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 082/652] 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 083/652] 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 084/652] 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 085/652] 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 086/652] 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 087/652] 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 088/652] 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 089/652] 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 090/652] 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 091/652] 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 092/652] 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 093/652] 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 094/652] 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 095/652] 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 096/652] 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 097/652] 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 098/652] 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 099/652] 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 100/652] 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 101/652] 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 102/652] 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 103/652] 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 104/652] 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 105/652] 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 106/652] 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 107/652] 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 108/652] 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 109/652] 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 110/652] 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 111/652] 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 112/652] 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 113/652] 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 114/652] 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 115/652] 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 116/652] 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 117/652] 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 118/652] 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 119/652] 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 120/652] 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 121/652] 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 122/652] 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 123/652] 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 124/652] 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 125/652] 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 126/652] 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 127/652] 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 128/652] 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 129/652] 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 130/652] 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 131/652] 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 132/652] 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 133/652] 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 134/652] 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 135/652] 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 136/652] 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 137/652] 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 138/652] 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 139/652] 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 140/652] 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 141/652] 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 142/652] 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 143/652] 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 144/652] 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 145/652] 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 146/652] 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 147/652] 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 148/652] 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 149/652] 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 150/652] 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 151/652] 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 152/652] 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 153/652] 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 154/652] 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 155/652] 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 156/652] 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 e1bdc4a0fbb5e89dc5f3911dbf9a9a21ed5b459b Mon Sep 17 00:00:00 2001 From: "M.H.Kabir" Date: Mon, 15 Dec 2014 14:23:27 +0530 Subject: [PATCH 157/652] New timesync interface import. Not working yet. --- ROMFS/px4fmu_common/init.d/rc.usb | 2 +- src/modules/mavlink/mavlink_main.cpp | 2 + src/modules/mavlink/mavlink_messages.cpp | 796 +++++++++++------------ src/modules/mavlink/mavlink_receiver.cpp | 289 +++++--- src/modules/mavlink/mavlink_receiver.h | 14 + 5 files changed, 598 insertions(+), 505 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.usb b/ROMFS/px4fmu_common/init.d/rc.usb index ee040a706d..12aca0687b 100644 --- a/ROMFS/px4fmu_common/init.d/rc.usb +++ b/ROMFS/px4fmu_common/init.d/rc.usb @@ -3,7 +3,7 @@ # USB MAVLink start # -mavlink start -r 20000 -d /dev/ttyACM0 -x +mavlink start -r 20000 -d /dev/ttyACM0 -x -m onboard # Enable a number of interesting streams we want via USB mavlink stream -d /dev/ttyACM0 -s PARAM_VALUE -r 200 usleep 100000 diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 29b7ec7b74..76b2ad2176 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1416,6 +1416,8 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("ATTITUDE_TARGET", 10.0f); configure_stream("POSITION_TARGET_GLOBAL_INT", 10.0f); configure_stream("VFR_HUD", 10.0f); + configure_stream("SYSTEM_TIME", 1.0f); + configure_stream("TIMESYNC", 10.0f); break; default: diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 5908279d54..85643270f0 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -121,91 +121,93 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set switch (status->nav_state) { - case NAVIGATION_STATE_MANUAL: - *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED - | (status->is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0); - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL; - break; + case NAVIGATION_STATE_MANUAL: + *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED + | (status->is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0); + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL; + break; - case NAVIGATION_STATE_ACRO: - *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ACRO; - break; + case NAVIGATION_STATE_ACRO: + *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ACRO; + break; - case NAVIGATION_STATE_ALTCTL: - *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED - | MAV_MODE_FLAG_STABILIZE_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ALTCTL; - break; + case NAVIGATION_STATE_ALTCTL: + *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED + | MAV_MODE_FLAG_STABILIZE_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ALTCTL; + break; - case NAVIGATION_STATE_POSCTL: - *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED - | MAV_MODE_FLAG_STABILIZE_ENABLED - | MAV_MODE_FLAG_GUIDED_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_POSCTL; - break; + case NAVIGATION_STATE_POSCTL: + *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED + | MAV_MODE_FLAG_STABILIZE_ENABLED + | MAV_MODE_FLAG_GUIDED_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_POSCTL; + break; - case NAVIGATION_STATE_AUTO_MISSION: - *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED - | MAV_MODE_FLAG_STABILIZE_ENABLED - | MAV_MODE_FLAG_GUIDED_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; - custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION; - break; + case NAVIGATION_STATE_AUTO_MISSION: + *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED + | MAV_MODE_FLAG_STABILIZE_ENABLED + | MAV_MODE_FLAG_GUIDED_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION; + break; - case NAVIGATION_STATE_AUTO_LOITER: - *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED - | MAV_MODE_FLAG_STABILIZE_ENABLED - | MAV_MODE_FLAG_GUIDED_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; - custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER; - break; + case NAVIGATION_STATE_AUTO_LOITER: + *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED + | MAV_MODE_FLAG_STABILIZE_ENABLED + | MAV_MODE_FLAG_GUIDED_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER; + break; - case NAVIGATION_STATE_AUTO_RTL: - /* fallthrough */ - case NAVIGATION_STATE_AUTO_RCRECOVER: - *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED - | MAV_MODE_FLAG_STABILIZE_ENABLED - | MAV_MODE_FLAG_GUIDED_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; - custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL; - break; + case NAVIGATION_STATE_AUTO_RTL: - case NAVIGATION_STATE_LAND: - case NAVIGATION_STATE_AUTO_LANDENGFAIL: - case NAVIGATION_STATE_AUTO_LANDGPSFAIL: - /* fallthrough */ - case NAVIGATION_STATE_DESCEND: - *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED - | MAV_MODE_FLAG_STABILIZE_ENABLED - | MAV_MODE_FLAG_GUIDED_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; - custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND; - break; + /* fallthrough */ + case NAVIGATION_STATE_AUTO_RCRECOVER: + *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED + | MAV_MODE_FLAG_STABILIZE_ENABLED + | MAV_MODE_FLAG_GUIDED_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL; + break; - case NAVIGATION_STATE_AUTO_RTGS: - *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED - | MAV_MODE_FLAG_STABILIZE_ENABLED - | MAV_MODE_FLAG_GUIDED_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; - custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTGS; - break; + case NAVIGATION_STATE_LAND: + case NAVIGATION_STATE_AUTO_LANDENGFAIL: + case NAVIGATION_STATE_AUTO_LANDGPSFAIL: - case NAVIGATION_STATE_TERMINATION: - *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL; - break; + /* fallthrough */ + case NAVIGATION_STATE_DESCEND: + *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED + | MAV_MODE_FLAG_STABILIZE_ENABLED + | MAV_MODE_FLAG_GUIDED_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND; + break; - case NAVIGATION_STATE_OFFBOARD: - *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED - | MAV_MODE_FLAG_STABILIZE_ENABLED - | MAV_MODE_FLAG_GUIDED_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_OFFBOARD; - break; + case NAVIGATION_STATE_AUTO_RTGS: + *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED + | MAV_MODE_FLAG_STABILIZE_ENABLED + | MAV_MODE_FLAG_GUIDED_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTGS; + break; - case NAVIGATION_STATE_MAX: - /* this is an unused case, ignore */ - break; + case NAVIGATION_STATE_TERMINATION: + *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL; + break; + + case NAVIGATION_STATE_OFFBOARD: + *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED + | MAV_MODE_FLAG_STABILIZE_ENABLED + | MAV_MODE_FLAG_GUIDED_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_OFFBOARD; + break; + + case NAVIGATION_STATE_MAX: + /* this is an unused case, ignore */ + break; } @@ -238,28 +240,23 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set class MavlinkStreamHeartbeat : public MavlinkStream { public: - const char *get_name() const - { + const char *get_name() const { return MavlinkStreamHeartbeat::get_name_static(); } - static const char *get_name_static() - { + static const char *get_name_static() { return "HEARTBEAT"; } - uint8_t get_id() - { + uint8_t get_id() { return MAVLINK_MSG_ID_HEARTBEAT; } - static MavlinkStream *new_instance(Mavlink *mavlink) - { + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamHeartbeat(mavlink); } - unsigned get_size() - { + unsigned get_size() { return MAVLINK_MSG_ID_HEARTBEAT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } @@ -273,7 +270,7 @@ private: /* do not allow top copying this class */ MavlinkStreamHeartbeat(MavlinkStreamHeartbeat &); - MavlinkStreamHeartbeat& operator = (const MavlinkStreamHeartbeat &); + MavlinkStreamHeartbeat &operator = (const MavlinkStreamHeartbeat &); protected: explicit MavlinkStreamHeartbeat(Mavlink *mavlink) : MavlinkStream(mavlink), @@ -281,8 +278,7 @@ protected: _pos_sp_triplet_sub(_mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet))) {} - void send(const hrt_abstime t) - { + void send(const hrt_abstime t) { struct vehicle_status_s status; struct position_setpoint_triplet_s pos_sp_triplet; @@ -313,34 +309,31 @@ protected: class MavlinkStreamStatustext : public MavlinkStream { public: - const char *get_name() const - { + const char *get_name() const { return MavlinkStreamStatustext::get_name_static(); } - static const char *get_name_static() - { + static const char *get_name_static() { return "STATUSTEXT"; } - uint8_t get_id() - { + uint8_t get_id() { return MAVLINK_MSG_ID_STATUSTEXT; } - static MavlinkStream *new_instance(Mavlink *mavlink) - { + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamStatustext(mavlink); } unsigned get_size() { - return mavlink_logbuffer_is_empty(_mavlink->get_logbuffer()) ? 0 : (MAVLINK_MSG_ID_STATUSTEXT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES); + return mavlink_logbuffer_is_empty(_mavlink->get_logbuffer()) ? 0 : (MAVLINK_MSG_ID_STATUSTEXT_LEN + + MAVLINK_NUM_NON_PAYLOAD_BYTES); } private: /* do not allow top copying this class */ MavlinkStreamStatustext(MavlinkStreamStatustext &); - MavlinkStreamStatustext& operator = (const MavlinkStreamStatustext &); + MavlinkStreamStatustext &operator = (const MavlinkStreamStatustext &); FILE *fp = nullptr; protected: @@ -353,8 +346,7 @@ protected: } } - void send(const hrt_abstime t) - { + void send(const hrt_abstime t) { if (!mavlink_logbuffer_is_empty(_mavlink->get_logbuffer())) { struct mavlink_logmessage logmsg; int lb_ret = mavlink_logbuffer_read(_mavlink->get_logbuffer(), &logmsg); @@ -373,6 +365,7 @@ protected: fputs(msg.text, fp); fputs("\n", fp); fsync(fileno(fp)); + } else { /* string to hold the path to the log */ char log_file_name[32] = ""; @@ -399,23 +392,19 @@ protected: class MavlinkStreamCommandLong : public MavlinkStream { public: - const char *get_name() const - { + const char *get_name() const { return MavlinkStreamCommandLong::get_name_static(); } - static const char *get_name_static() - { + static const char *get_name_static() { return "COMMAND_LONG"; } - uint8_t get_id() - { + uint8_t get_id() { return MAVLINK_MSG_ID_COMMAND_LONG; } - static MavlinkStream *new_instance(Mavlink *mavlink) - { + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamCommandLong(mavlink); } @@ -429,7 +418,7 @@ private: /* do not allow top copying this class */ MavlinkStreamCommandLong(MavlinkStreamCommandLong &); - MavlinkStreamCommandLong& operator = (const MavlinkStreamCommandLong &); + MavlinkStreamCommandLong &operator = (const MavlinkStreamCommandLong &); protected: explicit MavlinkStreamCommandLong(Mavlink *mavlink) : MavlinkStream(mavlink), @@ -437,8 +426,7 @@ protected: _cmd_time(0) {} - void send(const hrt_abstime t) - { + void send(const hrt_abstime t) { struct vehicle_command_s cmd; if (_cmd_sub->update(&_cmd_time, &cmd)) { @@ -467,28 +455,23 @@ protected: class MavlinkStreamSysStatus : public MavlinkStream { public: - const char *get_name() const - { + const char *get_name() const { return MavlinkStreamSysStatus::get_name_static(); } - static const char *get_name_static() - { + static const char *get_name_static() { return "SYS_STATUS"; } - uint8_t get_id() - { + uint8_t get_id() { return MAVLINK_MSG_ID_SYS_STATUS; } - static MavlinkStream *new_instance(Mavlink *mavlink) - { + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamSysStatus(mavlink); } - unsigned get_size() - { + unsigned get_size() { return MAVLINK_MSG_ID_SYS_STATUS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } @@ -497,15 +480,14 @@ private: /* do not allow top copying this class */ MavlinkStreamSysStatus(MavlinkStreamSysStatus &); - MavlinkStreamSysStatus& operator = (const MavlinkStreamSysStatus &); + MavlinkStreamSysStatus &operator = (const MavlinkStreamSysStatus &); protected: explicit MavlinkStreamSysStatus(Mavlink *mavlink) : MavlinkStream(mavlink), _status_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_status))) {} - void send(const hrt_abstime t) - { + void send(const hrt_abstime t) { struct vehicle_status_s status; if (_status_sub->update(&status)) { @@ -534,28 +516,23 @@ protected: class MavlinkStreamHighresIMU : public MavlinkStream { public: - const char *get_name() const - { + const char *get_name() const { return MavlinkStreamHighresIMU::get_name_static(); } - static const char *get_name_static() - { + static const char *get_name_static() { return "HIGHRES_IMU"; } - uint8_t get_id() - { + uint8_t get_id() { return MAVLINK_MSG_ID_HIGHRES_IMU; } - static MavlinkStream *new_instance(Mavlink *mavlink) - { + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamHighresIMU(mavlink); } - unsigned get_size() - { + unsigned get_size() { return MAVLINK_MSG_ID_HIGHRES_IMU_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } @@ -570,7 +547,7 @@ private: /* do not allow top copying this class */ MavlinkStreamHighresIMU(MavlinkStreamHighresIMU &); - MavlinkStreamHighresIMU& operator = (const MavlinkStreamHighresIMU &); + MavlinkStreamHighresIMU &operator = (const MavlinkStreamHighresIMU &); protected: explicit MavlinkStreamHighresIMU(Mavlink *mavlink) : MavlinkStream(mavlink), @@ -582,8 +559,7 @@ protected: _baro_timestamp(0) {} - void send(const hrt_abstime t) - { + void send(const hrt_abstime t) { struct sensor_combined_s sensor; if (_sensor_sub->update(&_sensor_time, &sensor)) { @@ -640,28 +616,23 @@ protected: class MavlinkStreamAttitude : public MavlinkStream { public: - const char *get_name() const - { + const char *get_name() const { return MavlinkStreamAttitude::get_name_static(); } - static const char *get_name_static() - { + static const char *get_name_static() { return "ATTITUDE"; } - uint8_t get_id() - { + uint8_t get_id() { return MAVLINK_MSG_ID_ATTITUDE; } - static MavlinkStream *new_instance(Mavlink *mavlink) - { + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamAttitude(mavlink); } - unsigned get_size() - { + unsigned get_size() { return MAVLINK_MSG_ID_ATTITUDE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } @@ -671,7 +642,7 @@ private: /* do not allow top copying this class */ MavlinkStreamAttitude(MavlinkStreamAttitude &); - MavlinkStreamAttitude& operator = (const MavlinkStreamAttitude &); + MavlinkStreamAttitude &operator = (const MavlinkStreamAttitude &); protected: @@ -680,8 +651,7 @@ protected: _att_time(0) {} - void send(const hrt_abstime t) - { + void send(const hrt_abstime t) { struct vehicle_attitude_s att; if (_att_sub->update(&_att_time, &att)) { @@ -704,28 +674,23 @@ protected: class MavlinkStreamAttitudeQuaternion : public MavlinkStream { public: - const char *get_name() const - { + const char *get_name() const { return MavlinkStreamAttitudeQuaternion::get_name_static(); } - static const char *get_name_static() - { + static const char *get_name_static() { return "ATTITUDE_QUATERNION"; } - uint8_t get_id() - { + uint8_t get_id() { return MAVLINK_MSG_ID_ATTITUDE_QUATERNION; } - static MavlinkStream *new_instance(Mavlink *mavlink) - { + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamAttitudeQuaternion(mavlink); } - unsigned get_size() - { + unsigned get_size() { return MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } @@ -735,7 +700,7 @@ private: /* do not allow top copying this class */ MavlinkStreamAttitudeQuaternion(MavlinkStreamAttitudeQuaternion &); - MavlinkStreamAttitudeQuaternion& operator = (const MavlinkStreamAttitudeQuaternion &); + MavlinkStreamAttitudeQuaternion &operator = (const MavlinkStreamAttitudeQuaternion &); protected: explicit MavlinkStreamAttitudeQuaternion(Mavlink *mavlink) : MavlinkStream(mavlink), @@ -743,8 +708,7 @@ protected: _att_time(0) {} - void send(const hrt_abstime t) - { + void send(const hrt_abstime t) { struct vehicle_attitude_s att; if (_att_sub->update(&_att_time, &att)) { @@ -769,28 +733,23 @@ class MavlinkStreamVFRHUD : public MavlinkStream { public: - const char *get_name() const - { + const char *get_name() const { return MavlinkStreamVFRHUD::get_name_static(); } - static const char *get_name_static() - { + static const char *get_name_static() { return "VFR_HUD"; } - uint8_t get_id() - { + uint8_t get_id() { return MAVLINK_MSG_ID_VFR_HUD; } - static MavlinkStream *new_instance(Mavlink *mavlink) - { + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamVFRHUD(mavlink); } - unsigned get_size() - { + unsigned get_size() { return MAVLINK_MSG_ID_VFR_HUD_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } @@ -812,7 +771,7 @@ private: /* do not allow top copying this class */ MavlinkStreamVFRHUD(MavlinkStreamVFRHUD &); - MavlinkStreamVFRHUD& operator = (const MavlinkStreamVFRHUD &); + MavlinkStreamVFRHUD &operator = (const MavlinkStreamVFRHUD &); protected: explicit MavlinkStreamVFRHUD(Mavlink *mavlink) : MavlinkStream(mavlink), @@ -828,8 +787,7 @@ protected: _airspeed_time(0) {} - void send(const hrt_abstime t) - { + void send(const hrt_abstime t) { struct vehicle_attitude_s att; struct vehicle_global_position_s pos; struct actuator_armed_s armed; @@ -861,28 +819,23 @@ protected: class MavlinkStreamGPSRawInt : public MavlinkStream { public: - const char *get_name() const - { + const char *get_name() const { return MavlinkStreamGPSRawInt::get_name_static(); } - static const char *get_name_static() - { + static const char *get_name_static() { return "GPS_RAW_INT"; } - uint8_t get_id() - { + uint8_t get_id() { return MAVLINK_MSG_ID_GPS_RAW_INT; } - static MavlinkStream *new_instance(Mavlink *mavlink) - { + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamGPSRawInt(mavlink); } - unsigned get_size() - { + unsigned get_size() { return MAVLINK_MSG_ID_GPS_RAW_INT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } @@ -892,7 +845,7 @@ private: /* do not allow top copying this class */ MavlinkStreamGPSRawInt(MavlinkStreamGPSRawInt &); - MavlinkStreamGPSRawInt& operator = (const MavlinkStreamGPSRawInt &); + MavlinkStreamGPSRawInt &operator = (const MavlinkStreamGPSRawInt &); protected: explicit MavlinkStreamGPSRawInt(Mavlink *mavlink) : MavlinkStream(mavlink), @@ -900,8 +853,7 @@ protected: _gps_time(0) {} - void send(const hrt_abstime t) - { + void send(const hrt_abstime t) { struct vehicle_gps_position_s gps; if (_gps_sub->update(&_gps_time, &gps)) { @@ -915,40 +867,121 @@ protected: msg.eph = cm_uint16_from_m_float(gps.eph); msg.epv = cm_uint16_from_m_float(gps.epv); msg.vel = cm_uint16_from_m_float(gps.vel_m_s), - msg.cog = _wrap_2pi(gps.cog_rad) * M_RAD_TO_DEG_F * 1e2f, - msg.satellites_visible = gps.satellites_used; + msg.cog = _wrap_2pi(gps.cog_rad) * M_RAD_TO_DEG_F * 1e2f, + msg.satellites_visible = gps.satellites_used; _mavlink->send_message(MAVLINK_MSG_ID_GPS_RAW_INT, &msg); } } }; +class MavlinkStreamSystemTime : public MavlinkStream +{ +public: + const char *get_name() const { + return MavlinkStreamSystemTime::get_name_static(); + } + + static const char *get_name_static() { + return "SYSTEM_TIME"; + } + + uint8_t get_id() { + return MAVLINK_MSG_ID_SYSTEM_TIME; + } + + static MavlinkStream *new_instance(Mavlink *mavlink) { + return new MavlinkStreamSystemTime(mavlink); + } + + unsigned get_size() { + return MAVLINK_MSG_ID_SYSTEM_TIME_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + +private: + /* do not allow top copying this class */ + MavlinkStreamSystemTime(MavlinkStreamSystemTime &); + MavlinkStreamSystemTime &operator = (const MavlinkStreamSystemTime &); + +protected: + explicit MavlinkStreamSystemTime(Mavlink *mavlink) : MavlinkStream(mavlink) + {} + + void send(const hrt_abstime t) { + mavlink_system_time_t msg; + timespec tv; + + clock_gettime(CLOCK_REALTIME, &tv); + + msg.time_boot_ms = hrt_absolute_time() / 1000; + msg.time_unix_usec = (uint64_t)tv.tv_sec * 1000000 + tv.tv_nsec / 1000; + + _mavlink->send_message(MAVLINK_MSG_ID_SYSTEM_TIME, &msg); + } +}; + +class MavlinkStreamTimesync : public MavlinkStream +{ +public: + const char *get_name() const { + return MavlinkStreamTimesync::get_name_static(); + } + + static const char *get_name_static() { + return "TIMESYNC"; + } + + uint8_t get_id() { + return MAVLINK_MSG_ID_TIMESYNC; + } + + static MavlinkStream *new_instance(Mavlink *mavlink) { + return new MavlinkStreamTimesync(mavlink); + } + + unsigned get_size() { + return MAVLINK_MSG_ID_TIMESYNC_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + +private: + /* do not allow top copying this class */ + MavlinkStreamTimesync(MavlinkStreamTimesync &); + MavlinkStreamTimesync &operator = (const MavlinkStreamTimesync &); + +protected: + explicit MavlinkStreamTimesync(Mavlink *mavlink) : MavlinkStream(mavlink) + {} + + void send(const hrt_abstime t) { + mavlink_timesync_t msg; + + msg.tc1 = -1; + msg.ts1 = hrt_absolute_time() * 1000; // boot time in nanoseconds + + _mavlink->send_message(MAVLINK_MSG_ID_TIMESYNC, &msg); + } +}; class MavlinkStreamGlobalPositionInt : public MavlinkStream { public: - const char *get_name() const - { + const char *get_name() const { return MavlinkStreamGlobalPositionInt::get_name_static(); } - static const char *get_name_static() - { + static const char *get_name_static() { return "GLOBAL_POSITION_INT"; } - uint8_t get_id() - { + uint8_t get_id() { return MAVLINK_MSG_ID_GLOBAL_POSITION_INT; } - static MavlinkStream *new_instance(Mavlink *mavlink) - { + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamGlobalPositionInt(mavlink); } - unsigned get_size() - { + unsigned get_size() { return MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } @@ -961,7 +994,7 @@ private: /* do not allow top copying this class */ MavlinkStreamGlobalPositionInt(MavlinkStreamGlobalPositionInt &); - MavlinkStreamGlobalPositionInt& operator = (const MavlinkStreamGlobalPositionInt &); + MavlinkStreamGlobalPositionInt &operator = (const MavlinkStreamGlobalPositionInt &); protected: explicit MavlinkStreamGlobalPositionInt(Mavlink *mavlink) : MavlinkStream(mavlink), @@ -971,8 +1004,7 @@ protected: _home_time(0) {} - void send(const hrt_abstime t) - { + void send(const hrt_abstime t) { struct vehicle_global_position_s pos; struct home_position_s home; @@ -1001,28 +1033,23 @@ protected: class MavlinkStreamLocalPositionNED : public MavlinkStream { public: - const char *get_name() const - { + const char *get_name() const { return MavlinkStreamLocalPositionNED::get_name_static(); } - static const char *get_name_static() - { + static const char *get_name_static() { return "LOCAL_POSITION_NED"; } - uint8_t get_id() - { + uint8_t get_id() { return MAVLINK_MSG_ID_LOCAL_POSITION_NED; } - static MavlinkStream *new_instance(Mavlink *mavlink) - { + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamLocalPositionNED(mavlink); } - unsigned get_size() - { + unsigned get_size() { return MAVLINK_MSG_ID_LOCAL_POSITION_NED_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } @@ -1032,7 +1059,7 @@ private: /* do not allow top copying this class */ MavlinkStreamLocalPositionNED(MavlinkStreamLocalPositionNED &); - MavlinkStreamLocalPositionNED& operator = (const MavlinkStreamLocalPositionNED &); + MavlinkStreamLocalPositionNED &operator = (const MavlinkStreamLocalPositionNED &); protected: explicit MavlinkStreamLocalPositionNED(Mavlink *mavlink) : MavlinkStream(mavlink), @@ -1040,8 +1067,7 @@ protected: _pos_time(0) {} - void send(const hrt_abstime t) - { + void send(const hrt_abstime t) { struct vehicle_local_position_s pos; if (_pos_sub->update(&_pos_time, &pos)) { @@ -1064,28 +1090,23 @@ protected: class MavlinkStreamViconPositionEstimate : public MavlinkStream { public: - const char *get_name() const - { + const char *get_name() const { return MavlinkStreamViconPositionEstimate::get_name_static(); } - static const char *get_name_static() - { + static const char *get_name_static() { return "VICON_POSITION_ESTIMATE"; } - uint8_t get_id() - { + uint8_t get_id() { return MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE; } - static MavlinkStream *new_instance(Mavlink *mavlink) - { + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamViconPositionEstimate(mavlink); } - unsigned get_size() - { + unsigned get_size() { return MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } @@ -1095,7 +1116,7 @@ private: /* do not allow top copying this class */ MavlinkStreamViconPositionEstimate(MavlinkStreamViconPositionEstimate &); - MavlinkStreamViconPositionEstimate& operator = (const MavlinkStreamViconPositionEstimate &); + MavlinkStreamViconPositionEstimate &operator = (const MavlinkStreamViconPositionEstimate &); protected: explicit MavlinkStreamViconPositionEstimate(Mavlink *mavlink) : MavlinkStream(mavlink), @@ -1103,8 +1124,7 @@ protected: _pos_time(0) {} - void send(const hrt_abstime t) - { + void send(const hrt_abstime t) { struct vehicle_vicon_position_s pos; if (_pos_sub->update(&_pos_time, &pos)) { @@ -1127,28 +1147,23 @@ protected: class MavlinkStreamGPSGlobalOrigin : public MavlinkStream { public: - const char *get_name() const - { + const char *get_name() const { return MavlinkStreamGPSGlobalOrigin::get_name_static(); } - static const char *get_name_static() - { + static const char *get_name_static() { return "GPS_GLOBAL_ORIGIN"; } - uint8_t get_id() - { + uint8_t get_id() { return MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN; } - static MavlinkStream *new_instance(Mavlink *mavlink) - { + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamGPSGlobalOrigin(mavlink); } - unsigned get_size() - { + unsigned get_size() { return _home_sub->is_published() ? (MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; } @@ -1157,15 +1172,14 @@ private: /* do not allow top copying this class */ MavlinkStreamGPSGlobalOrigin(MavlinkStreamGPSGlobalOrigin &); - MavlinkStreamGPSGlobalOrigin& operator = (const MavlinkStreamGPSGlobalOrigin &); + MavlinkStreamGPSGlobalOrigin &operator = (const MavlinkStreamGPSGlobalOrigin &); protected: explicit MavlinkStreamGPSGlobalOrigin(Mavlink *mavlink) : MavlinkStream(mavlink), _home_sub(_mavlink->add_orb_subscription(ORB_ID(home_position))) {} - void send(const hrt_abstime t) - { + void send(const hrt_abstime t) { /* we're sending the GPS home periodically to ensure the * the GCS does pick it up at one point */ if (_home_sub->is_published()) { @@ -1189,40 +1203,35 @@ template class MavlinkStreamServoOutputRaw : public MavlinkStream { public: - const char *get_name() const - { + const char *get_name() const { return MavlinkStreamServoOutputRaw::get_name_static(); } - uint8_t get_id() - { + uint8_t get_id() { return MAVLINK_MSG_ID_SERVO_OUTPUT_RAW; } - static const char *get_name_static() - { + static const char *get_name_static() { switch (N) { - case 0: + case 0: return "SERVO_OUTPUT_RAW_0"; - case 1: + case 1: return "SERVO_OUTPUT_RAW_1"; - case 2: + case 2: return "SERVO_OUTPUT_RAW_2"; - case 3: + case 3: return "SERVO_OUTPUT_RAW_3"; } } - static MavlinkStream *new_instance(Mavlink *mavlink) - { + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamServoOutputRaw(mavlink); } - unsigned get_size() - { + unsigned get_size() { return MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } @@ -1232,13 +1241,12 @@ private: /* do not allow top copying this class */ MavlinkStreamServoOutputRaw(MavlinkStreamServoOutputRaw &); - MavlinkStreamServoOutputRaw& operator = (const MavlinkStreamServoOutputRaw &); + MavlinkStreamServoOutputRaw &operator = (const MavlinkStreamServoOutputRaw &); protected: explicit MavlinkStreamServoOutputRaw(Mavlink *mavlink) : MavlinkStream(mavlink), _act_sub(nullptr), - _act_time(0) - { + _act_time(0) { orb_id_t act_topics[] = { ORB_ID(actuator_outputs_0), ORB_ID(actuator_outputs_1), @@ -1249,8 +1257,7 @@ protected: _act_sub = _mavlink->add_orb_subscription(act_topics[N]); } - void send(const hrt_abstime t) - { + void send(const hrt_abstime t) { struct actuator_outputs_s act; if (_act_sub->update(&_act_time, &act)) { @@ -1276,28 +1283,23 @@ protected: class MavlinkStreamHILControls : public MavlinkStream { public: - const char *get_name() const - { + const char *get_name() const { return MavlinkStreamHILControls::get_name_static(); } - static const char *get_name_static() - { + static const char *get_name_static() { return "HIL_CONTROLS"; } - uint8_t get_id() - { + uint8_t get_id() { return MAVLINK_MSG_ID_HIL_CONTROLS; } - static MavlinkStream *new_instance(Mavlink *mavlink) - { + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamHILControls(mavlink); } - unsigned get_size() - { + unsigned get_size() { return MAVLINK_MSG_ID_HIL_CONTROLS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } @@ -1313,7 +1315,7 @@ private: /* do not allow top copying this class */ MavlinkStreamHILControls(MavlinkStreamHILControls &); - MavlinkStreamHILControls& operator = (const MavlinkStreamHILControls &); + MavlinkStreamHILControls &operator = (const MavlinkStreamHILControls &); protected: explicit MavlinkStreamHILControls(Mavlink *mavlink) : MavlinkStream(mavlink), @@ -1325,8 +1327,7 @@ protected: _act_time(0) {} - void send(const hrt_abstime t) - { + void send(const hrt_abstime t) { struct vehicle_status_s status; struct position_setpoint_triplet_s pos_sp_triplet; struct actuator_outputs_s act; @@ -1350,8 +1351,8 @@ 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_HEXAROTOR || + system_type == MAV_TYPE_OCTOROTOR) { /* multirotors: set number of rotor outputs depending on type */ unsigned n; @@ -1431,28 +1432,23 @@ protected: class MavlinkStreamPositionTargetGlobalInt : public MavlinkStream { public: - const char *get_name() const - { + const char *get_name() const { return MavlinkStreamPositionTargetGlobalInt::get_name_static(); } - static const char *get_name_static() - { + static const char *get_name_static() { return "POSITION_TARGET_GLOBAL_INT"; } - uint8_t get_id() - { + uint8_t get_id() { return MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT; } - static MavlinkStream *new_instance(Mavlink *mavlink) - { + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamPositionTargetGlobalInt(mavlink); } - unsigned get_size() - { + unsigned get_size() { return MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } @@ -1461,19 +1457,18 @@ private: /* do not allow top copying this class */ MavlinkStreamPositionTargetGlobalInt(MavlinkStreamPositionTargetGlobalInt &); - MavlinkStreamPositionTargetGlobalInt& operator = (const MavlinkStreamPositionTargetGlobalInt &); + MavlinkStreamPositionTargetGlobalInt &operator = (const MavlinkStreamPositionTargetGlobalInt &); protected: explicit MavlinkStreamPositionTargetGlobalInt(Mavlink *mavlink) : MavlinkStream(mavlink), _pos_sp_triplet_sub(_mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet))) {} - void send(const hrt_abstime t) - { + void send(const hrt_abstime t) { struct position_setpoint_triplet_s pos_sp_triplet; if (_pos_sp_triplet_sub->update(&pos_sp_triplet)) { - mavlink_position_target_global_int_t msg{}; + mavlink_position_target_global_int_t msg {}; msg.coordinate_frame = MAV_FRAME_GLOBAL; msg.lat_int = pos_sp_triplet.current.lat * 1e7; @@ -1489,28 +1484,23 @@ protected: class MavlinkStreamLocalPositionSetpoint : public MavlinkStream { public: - const char *get_name() const - { + const char *get_name() const { return MavlinkStreamLocalPositionSetpoint::get_name_static(); } - static const char *get_name_static() - { + static const char *get_name_static() { return "POSITION_TARGET_LOCAL_NED"; } - uint8_t get_id() - { + uint8_t get_id() { return MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED; } - static MavlinkStream *new_instance(Mavlink *mavlink) - { + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamLocalPositionSetpoint(mavlink); } - unsigned get_size() - { + unsigned get_size() { return MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } @@ -1520,7 +1510,7 @@ private: /* do not allow top copying this class */ MavlinkStreamLocalPositionSetpoint(MavlinkStreamLocalPositionSetpoint &); - MavlinkStreamLocalPositionSetpoint& operator = (const MavlinkStreamLocalPositionSetpoint &); + MavlinkStreamLocalPositionSetpoint &operator = (const MavlinkStreamLocalPositionSetpoint &); protected: explicit MavlinkStreamLocalPositionSetpoint(Mavlink *mavlink) : MavlinkStream(mavlink), @@ -1528,12 +1518,11 @@ protected: _pos_sp_time(0) {} - void send(const hrt_abstime t) - { + void send(const hrt_abstime t) { struct vehicle_local_position_setpoint_s pos_sp; if (_pos_sp_sub->update(&_pos_sp_time, &pos_sp)) { - mavlink_position_target_local_ned_t msg{}; + mavlink_position_target_local_ned_t msg {}; msg.time_boot_ms = pos_sp.timestamp / 1000; msg.coordinate_frame = MAV_FRAME_LOCAL_NED; @@ -1550,28 +1539,23 @@ protected: class MavlinkStreamAttitudeTarget : public MavlinkStream { public: - const char *get_name() const - { + const char *get_name() const { return MavlinkStreamAttitudeTarget::get_name_static(); } - static const char *get_name_static() - { + static const char *get_name_static() { return "ATTITUDE_TARGET"; } - uint8_t get_id() - { + uint8_t get_id() { return MAVLINK_MSG_ID_ATTITUDE_TARGET; } - static MavlinkStream *new_instance(Mavlink *mavlink) - { + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamAttitudeTarget(mavlink); } - unsigned get_size() - { + unsigned get_size() { return MAVLINK_MSG_ID_ATTITUDE_TARGET_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } @@ -1583,7 +1567,7 @@ private: /* do not allow top copying this class */ MavlinkStreamAttitudeTarget(MavlinkStreamAttitudeTarget &); - MavlinkStreamAttitudeTarget& operator = (const MavlinkStreamAttitudeTarget &); + MavlinkStreamAttitudeTarget &operator = (const MavlinkStreamAttitudeTarget &); protected: explicit MavlinkStreamAttitudeTarget(Mavlink *mavlink) : MavlinkStream(mavlink), @@ -1593,8 +1577,7 @@ protected: _att_rates_sp_time(0) {} - void send(const hrt_abstime t) - { + void send(const hrt_abstime t) { struct vehicle_attitude_setpoint_s att_sp; if (_att_sp_sub->update(&_att_sp_time, &att_sp)) { @@ -1602,7 +1585,7 @@ protected: struct vehicle_rates_setpoint_s att_rates_sp; (void)_att_rates_sp_sub->update(&_att_rates_sp_time, &att_rates_sp); - mavlink_attitude_target_t msg{}; + mavlink_attitude_target_t msg {}; msg.time_boot_ms = att_sp.timestamp / 1000; mavlink_euler_to_quaternion(att_sp.roll_body, att_sp.pitch_body, att_sp.yaw_body, msg.q); @@ -1622,30 +1605,26 @@ protected: class MavlinkStreamRCChannelsRaw : public MavlinkStream { public: - const char *get_name() const - { + const char *get_name() const { return MavlinkStreamRCChannelsRaw::get_name_static(); } - static const char *get_name_static() - { + static const char *get_name_static() { return "RC_CHANNELS_RAW"; } - uint8_t get_id() - { + uint8_t get_id() { return MAVLINK_MSG_ID_RC_CHANNELS_RAW; } - static MavlinkStream *new_instance(Mavlink *mavlink) - { + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamRCChannelsRaw(mavlink); } - unsigned get_size() - { - return _rc_sub->is_published() ? ((RC_INPUT_MAX_CHANNELS / 8) * (MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) + - MAVLINK_MSG_ID_RC_CHANNELS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; + unsigned get_size() { + return _rc_sub->is_published() ? ((RC_INPUT_MAX_CHANNELS / 8) * (MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN + + MAVLINK_NUM_NON_PAYLOAD_BYTES) + + MAVLINK_MSG_ID_RC_CHANNELS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; } private: @@ -1654,7 +1633,7 @@ private: /* do not allow top copying this class */ MavlinkStreamRCChannelsRaw(MavlinkStreamRCChannelsRaw &); - MavlinkStreamRCChannelsRaw& operator = (const MavlinkStreamRCChannelsRaw &); + MavlinkStreamRCChannelsRaw &operator = (const MavlinkStreamRCChannelsRaw &); protected: explicit MavlinkStreamRCChannelsRaw(Mavlink *mavlink) : MavlinkStream(mavlink), @@ -1662,8 +1641,7 @@ protected: _rc_time(0) {} - void send(const hrt_abstime t) - { + void send(const hrt_abstime t) { struct rc_input_values rc; if (_rc_sub->update(&_rc_time, &rc)) { @@ -1739,20 +1717,24 @@ protected: msg.rssi |= (rc.rssi <= 100) ? ((rc.rssi / 7) + 1) : 15; switch (rc.input_source) { - case RC_INPUT_SOURCE_PX4FMU_PPM: + case RC_INPUT_SOURCE_PX4FMU_PPM: + /* fallthrough */ - case RC_INPUT_SOURCE_PX4IO_PPM: - msg.rssi |= (0 << 4); - break; - case RC_INPUT_SOURCE_PX4IO_SPEKTRUM: - msg.rssi |= (1 << 4); - break; - case RC_INPUT_SOURCE_PX4IO_SBUS: - msg.rssi |= (2 << 4); - break; - case RC_INPUT_SOURCE_PX4IO_ST24: - msg.rssi |= (3 << 4); - break; + case RC_INPUT_SOURCE_PX4IO_PPM: + msg.rssi |= (0 << 4); + break; + + case RC_INPUT_SOURCE_PX4IO_SPEKTRUM: + msg.rssi |= (1 << 4); + break; + + case RC_INPUT_SOURCE_PX4IO_SBUS: + msg.rssi |= (2 << 4); + break; + + case RC_INPUT_SOURCE_PX4IO_ST24: + msg.rssi |= (3 << 4); + break; } if (rc.rc_lost) { @@ -1769,28 +1751,23 @@ protected: class MavlinkStreamManualControl : public MavlinkStream { public: - const char *get_name() const - { + const char *get_name() const { return MavlinkStreamManualControl::get_name_static(); } - static const char *get_name_static() - { + static const char *get_name_static() { return "MANUAL_CONTROL"; } - uint8_t get_id() - { + uint8_t get_id() { return MAVLINK_MSG_ID_MANUAL_CONTROL; } - static MavlinkStream *new_instance(Mavlink *mavlink) - { + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamManualControl(mavlink); } - unsigned get_size() - { + unsigned get_size() { return _manual_sub->is_published() ? (MAVLINK_MSG_ID_MANUAL_CONTROL_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; } @@ -1800,7 +1777,7 @@ private: /* do not allow top copying this class */ MavlinkStreamManualControl(MavlinkStreamManualControl &); - MavlinkStreamManualControl& operator = (const MavlinkStreamManualControl &); + MavlinkStreamManualControl &operator = (const MavlinkStreamManualControl &); protected: explicit MavlinkStreamManualControl(Mavlink *mavlink) : MavlinkStream(mavlink), @@ -1808,8 +1785,7 @@ protected: _manual_time(0) {} - void send(const hrt_abstime t) - { + void send(const hrt_abstime t) { struct manual_control_setpoint_s manual; if (_manual_sub->update(&_manual_time, &manual)) { @@ -1830,28 +1806,23 @@ protected: class MavlinkStreamOpticalFlowRad : public MavlinkStream { public: - const char *get_name() const - { + const char *get_name() const { return MavlinkStreamOpticalFlowRad::get_name_static(); } - static const char *get_name_static() - { + static const char *get_name_static() { return "OPTICAL_FLOW_RAD"; } - uint8_t get_id() - { + uint8_t get_id() { return MAVLINK_MSG_ID_OPTICAL_FLOW_RAD; } - static MavlinkStream *new_instance(Mavlink *mavlink) - { + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamOpticalFlowRad(mavlink); } - unsigned get_size() - { + unsigned get_size() { return _flow_sub->is_published() ? (MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; } @@ -1861,7 +1832,7 @@ private: /* do not allow top copying this class */ MavlinkStreamOpticalFlowRad(MavlinkStreamOpticalFlowRad &); - MavlinkStreamOpticalFlowRad& operator = (const MavlinkStreamOpticalFlowRad &); + MavlinkStreamOpticalFlowRad &operator = (const MavlinkStreamOpticalFlowRad &); protected: explicit MavlinkStreamOpticalFlowRad(Mavlink *mavlink) : MavlinkStream(mavlink), @@ -1869,8 +1840,7 @@ protected: _flow_time(0) {} - void send(const hrt_abstime t) - { + void send(const hrt_abstime t) { struct optical_flow_s flow; if (_flow_sub->update(&_flow_time, &flow)) { @@ -1898,28 +1868,23 @@ protected: class MavlinkStreamAttitudeControls : public MavlinkStream { public: - const char *get_name() const - { + const char *get_name() const { return MavlinkStreamAttitudeControls::get_name_static(); } - static const char *get_name_static() - { + static const char *get_name_static() { return "ATTITUDE_CONTROLS"; } - uint8_t get_id() - { + uint8_t get_id() { return 0; } - static MavlinkStream *new_instance(Mavlink *mavlink) - { + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamAttitudeControls(mavlink); } - unsigned get_size() - { + unsigned get_size() { return 4 * (MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES); } @@ -1929,7 +1894,7 @@ private: /* do not allow top copying this class */ MavlinkStreamAttitudeControls(MavlinkStreamAttitudeControls &); - MavlinkStreamAttitudeControls& operator = (const MavlinkStreamAttitudeControls &); + MavlinkStreamAttitudeControls &operator = (const MavlinkStreamAttitudeControls &); protected: explicit MavlinkStreamAttitudeControls(Mavlink *mavlink) : MavlinkStream(mavlink), @@ -1937,8 +1902,7 @@ protected: _att_ctrl_time(0) {} - void send(const hrt_abstime t) - { + void send(const hrt_abstime t) { struct actuator_controls_s att_ctrl; if (_att_ctrl_sub->update(&_att_ctrl_time, &att_ctrl)) { @@ -1974,28 +1938,23 @@ protected: class MavlinkStreamNamedValueFloat : public MavlinkStream { public: - const char *get_name() const - { + const char *get_name() const { return MavlinkStreamNamedValueFloat::get_name_static(); } - static const char *get_name_static() - { + static const char *get_name_static() { return "NAMED_VALUE_FLOAT"; } - uint8_t get_id() - { + uint8_t get_id() { return MAVLINK_MSG_ID_NAMED_VALUE_FLOAT; } - static MavlinkStream *new_instance(Mavlink *mavlink) - { + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamNamedValueFloat(mavlink); } - unsigned get_size() - { + unsigned get_size() { return MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } @@ -2005,7 +1964,7 @@ private: /* do not allow top copying this class */ MavlinkStreamNamedValueFloat(MavlinkStreamNamedValueFloat &); - MavlinkStreamNamedValueFloat& operator = (const MavlinkStreamNamedValueFloat &); + MavlinkStreamNamedValueFloat &operator = (const MavlinkStreamNamedValueFloat &); protected: explicit MavlinkStreamNamedValueFloat(Mavlink *mavlink) : MavlinkStream(mavlink), @@ -2013,8 +1972,7 @@ protected: _debug_time(0) {} - void send(const hrt_abstime t) - { + void send(const hrt_abstime t) { struct debug_key_value_s debug; if (_debug_sub->update(&_debug_time, &debug)) { @@ -2035,28 +1993,23 @@ protected: class MavlinkStreamCameraCapture : public MavlinkStream { public: - const char *get_name() const - { + const char *get_name() const { return MavlinkStreamCameraCapture::get_name_static(); } - static const char *get_name_static() - { + static const char *get_name_static() { return "CAMERA_CAPTURE"; } - uint8_t get_id() - { + uint8_t get_id() { return 0; } - static MavlinkStream *new_instance(Mavlink *mavlink) - { + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamCameraCapture(mavlink); } - unsigned get_size() - { + unsigned get_size() { return MAVLINK_MSG_ID_COMMAND_LONG_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } @@ -2065,15 +2018,14 @@ private: /* do not allow top copying this class */ MavlinkStreamCameraCapture(MavlinkStreamCameraCapture &); - MavlinkStreamCameraCapture& operator = (const MavlinkStreamCameraCapture &); + MavlinkStreamCameraCapture &operator = (const MavlinkStreamCameraCapture &); protected: explicit MavlinkStreamCameraCapture(Mavlink *mavlink) : MavlinkStream(mavlink), _status_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_status))) {} - void send(const hrt_abstime t) - { + void send(const hrt_abstime t) { struct vehicle_status_s status; (void)_status_sub->update(&status); @@ -2100,28 +2052,23 @@ protected: class MavlinkStreamDistanceSensor : public MavlinkStream { public: - const char *get_name() const - { + const char *get_name() const { return MavlinkStreamDistanceSensor::get_name_static(); } - static const char *get_name_static() - { + static const char *get_name_static() { return "DISTANCE_SENSOR"; } - uint8_t get_id() - { + uint8_t get_id() { return MAVLINK_MSG_ID_DISTANCE_SENSOR; } - static MavlinkStream *new_instance(Mavlink *mavlink) - { + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamDistanceSensor(mavlink); } - unsigned get_size() - { + unsigned get_size() { return _range_sub->is_published() ? (MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; } @@ -2131,7 +2078,7 @@ private: /* do not allow top copying this class */ MavlinkStreamDistanceSensor(MavlinkStreamDistanceSensor &); - MavlinkStreamDistanceSensor& operator = (const MavlinkStreamDistanceSensor &); + MavlinkStreamDistanceSensor &operator = (const MavlinkStreamDistanceSensor &); protected: explicit MavlinkStreamDistanceSensor(Mavlink *mavlink) : MavlinkStream(mavlink), @@ -2139,8 +2086,7 @@ protected: _range_time(0) {} - void send(const hrt_abstime t) - { + void send(const hrt_abstime t) { struct range_finder_report range; if (_range_sub->update(&_range_time, &range)) { @@ -2182,6 +2128,8 @@ StreamListItem *streams_list[] = { new StreamListItem(&MavlinkStreamAttitudeQuaternion::new_instance, &MavlinkStreamAttitudeQuaternion::get_name_static), new StreamListItem(&MavlinkStreamVFRHUD::new_instance, &MavlinkStreamVFRHUD::get_name_static), new StreamListItem(&MavlinkStreamGPSRawInt::new_instance, &MavlinkStreamGPSRawInt::get_name_static), + new StreamListItem(&MavlinkStreamSystemTime::new_instance, &MavlinkStreamSystemTime::get_name_static), + new StreamListItem(&MavlinkStreamTimesync::new_instance, &MavlinkStreamTimesync::get_name_static), new StreamListItem(&MavlinkStreamGlobalPositionInt::new_instance, &MavlinkStreamGlobalPositionInt::get_name_static), new StreamListItem(&MavlinkStreamLocalPositionNED::new_instance, &MavlinkStreamLocalPositionNED::get_name_static), new StreamListItem(&MavlinkStreamViconPositionEstimate::new_instance, &MavlinkStreamViconPositionEstimate::get_name_static), diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index e98d72afe7..13e17ab64c 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -88,40 +88,42 @@ static const float mg2ms2 = CONSTANTS_ONE_G / 1000.0f; MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _mavlink(parent), - status{}, - hil_local_pos{}, - _control_mode{}, - _global_pos_pub(-1), - _local_pos_pub(-1), - _attitude_pub(-1), - _gps_pub(-1), - _sensors_pub(-1), - _gyro_pub(-1), - _accel_pub(-1), - _mag_pub(-1), - _baro_pub(-1), - _airspeed_pub(-1), - _battery_pub(-1), - _cmd_pub(-1), - _flow_pub(-1), - _range_pub(-1), - _offboard_control_sp_pub(-1), - _global_vel_sp_pub(-1), - _att_sp_pub(-1), - _rates_sp_pub(-1), - _force_sp_pub(-1), - _pos_sp_triplet_pub(-1), - _vicon_position_pub(-1), - _vision_position_pub(-1), - _telemetry_status_pub(-1), - _rc_pub(-1), - _manual_pub(-1), - _control_mode_sub(orb_subscribe(ORB_ID(vehicle_control_mode))), - _hil_frames(0), - _old_timestamp(0), - _hil_local_proj_inited(0), - _hil_local_alt0(0.0f), - _hil_local_proj_ref{} + status {}, + hil_local_pos {}, + _control_mode {}, + _global_pos_pub(-1), + _local_pos_pub(-1), + _attitude_pub(-1), + _gps_pub(-1), + _sensors_pub(-1), + _gyro_pub(-1), + _accel_pub(-1), + _mag_pub(-1), + _baro_pub(-1), + _airspeed_pub(-1), + _battery_pub(-1), + _cmd_pub(-1), + _flow_pub(-1), + _range_pub(-1), + _offboard_control_sp_pub(-1), + _global_vel_sp_pub(-1), + _att_sp_pub(-1), + _rates_sp_pub(-1), + _force_sp_pub(-1), + _pos_sp_triplet_pub(-1), + _vicon_position_pub(-1), + _vision_position_pub(-1), + _telemetry_status_pub(-1), + _rc_pub(-1), + _manual_pub(-1), + _control_mode_sub(orb_subscribe(ORB_ID(vehicle_control_mode))), + _hil_frames(0), + _old_timestamp(0), + _hil_local_proj_inited(0), + _hil_local_alt0(0.0f), + _hil_local_proj_ref {}, + _time_offset_avg_alpha(0.8f), + _time_offset(0) { // make sure the FTP server is started @@ -188,6 +190,14 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) MavlinkFTP::get_server()->handle_message(_mavlink, msg); break; + case MAVLINK_MSG_ID_SYSTEM_TIME: + handle_message_system_time(msg); + break; + + case MAVLINK_MSG_ID_TIMESYNC: + handle_message_timesync(msg); + break; + default: break; } @@ -222,7 +232,7 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) } - if (_mavlink->get_hil_enabled() || (_mavlink->get_use_hil_gps() && msg->sysid == mavlink_system.sysid)) { + if (_mavlink->get_hil_enabled() || (_mavlink->get_use_hil_gps() && msg->sysid == mavlink_system.sysid)) { switch (msg->msgid) { case MAVLINK_MSG_ID_HIL_GPS: handle_message_hil_gps(msg); @@ -234,7 +244,7 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) } - /* If we've received a valid message, mark the flag indicating so. + /* If we've received a valid message, mark the flag indicating so. This is used in the '-w' command-line flag. */ _mavlink->set_has_received_messages(true); } @@ -267,22 +277,35 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg) } struct vehicle_command_s vcmd; + memset(&vcmd, 0, sizeof(vcmd)); /* Copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */ vcmd.param1 = cmd_mavlink.param1; + vcmd.param2 = cmd_mavlink.param2; + vcmd.param3 = cmd_mavlink.param3; + vcmd.param4 = cmd_mavlink.param4; + vcmd.param5 = cmd_mavlink.param5; + vcmd.param6 = cmd_mavlink.param6; + vcmd.param7 = cmd_mavlink.param7; + // XXX do proper translation vcmd.command = (enum VEHICLE_CMD)cmd_mavlink.command; + vcmd.target_system = cmd_mavlink.target_system; + vcmd.target_component = cmd_mavlink.target_component; + vcmd.source_system = msg->sysid; + vcmd.source_component = msg->compid; + vcmd.confirmation = cmd_mavlink.confirmation; if (_cmd_pub < 0) { @@ -323,22 +346,34 @@ MavlinkReceiver::handle_message_command_int(mavlink_message_t *msg) } struct vehicle_command_s vcmd; + memset(&vcmd, 0, sizeof(vcmd)); /* Copy the content of mavlink_command_int_t cmd_mavlink into command_t cmd */ vcmd.param1 = cmd_mavlink.param1; + vcmd.param2 = cmd_mavlink.param2; + vcmd.param3 = cmd_mavlink.param3; + vcmd.param4 = cmd_mavlink.param4; + /* these are coordinates as 1e7 scaled integers to work around the 32 bit floating point limits */ vcmd.param5 = ((double)cmd_mavlink.x) / 1e7; + vcmd.param6 = ((double)cmd_mavlink.y) / 1e7; + vcmd.param7 = cmd_mavlink.z; + // XXX do proper translation vcmd.command = (enum VEHICLE_CMD)cmd_mavlink.command; + vcmd.target_system = cmd_mavlink.target_system; + vcmd.target_component = cmd_mavlink.target_component; + vcmd.source_system = msg->sysid; + vcmd.source_component = msg->compid; if (_cmd_pub < 0) { @@ -426,6 +461,7 @@ MavlinkReceiver::handle_message_hil_optical_flow(mavlink_message_t *msg) if (_range_pub < 0) { _range_pub = orb_advertise(ORB_ID(sensor_range_finder), &r); + } else { orb_publish(ORB_ID(sensor_range_finder), _range_pub, &r); } @@ -501,28 +537,33 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t /* Only accept messages which are intended for this system */ if ((mavlink_system.sysid == set_position_target_local_ned.target_system || - set_position_target_local_ned.target_system == 0) && - (mavlink_system.compid == set_position_target_local_ned.target_component || - set_position_target_local_ned.target_component == 0)) { + set_position_target_local_ned.target_system == 0) && + (mavlink_system.compid == set_position_target_local_ned.target_component || + set_position_target_local_ned.target_component == 0)) { /* convert mavlink type (local, NED) to uORB offboard control struct */ switch (set_position_target_local_ned.coordinate_frame) { - case MAV_FRAME_LOCAL_NED: - offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED; - break; - case MAV_FRAME_LOCAL_OFFSET_NED: - offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_OFFSET_NED; - break; - case MAV_FRAME_BODY_NED: - offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_BODY_NED; - break; - case MAV_FRAME_BODY_OFFSET_NED: - offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_BODY_OFFSET_NED; - break; - default: - /* invalid setpoint, avoid publishing */ - return; + case MAV_FRAME_LOCAL_NED: + offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED; + break; + + case MAV_FRAME_LOCAL_OFFSET_NED: + offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_OFFSET_NED; + break; + + case MAV_FRAME_BODY_NED: + offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_BODY_NED; + break; + + case MAV_FRAME_BODY_OFFSET_NED: + offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_BODY_OFFSET_NED; + break; + + default: + /* invalid setpoint, avoid publishing */ + return; } + offboard_control_sp.position[0] = set_position_target_local_ned.x; offboard_control_sp.position[1] = set_position_target_local_ned.y; offboard_control_sp.position[2] = set_position_target_local_ned.z; @@ -544,17 +585,19 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t /* set ignore flags */ for (int i = 0; i < 9; i++) { offboard_control_sp.ignore &= ~(1 << i); - offboard_control_sp.ignore |= (set_position_target_local_ned.type_mask & (1 << i)); + offboard_control_sp.ignore |= (set_position_target_local_ned.type_mask & (1 << i)); } offboard_control_sp.ignore &= ~(1 << OFB_IGN_BIT_YAW); + if (set_position_target_local_ned.type_mask & (1 << 10)) { - offboard_control_sp.ignore |= (1 << OFB_IGN_BIT_YAW); + offboard_control_sp.ignore |= (1 << OFB_IGN_BIT_YAW); } offboard_control_sp.ignore &= ~(1 << OFB_IGN_BIT_YAWRATE); + if (set_position_target_local_ned.type_mask & (1 << 11)) { - offboard_control_sp.ignore |= (1 << OFB_IGN_BIT_YAWRATE); + offboard_control_sp.ignore |= (1 << OFB_IGN_BIT_YAWRATE); } offboard_control_sp.timestamp = hrt_absolute_time(); @@ -571,25 +614,30 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t if (_mavlink->get_forward_externalsp()) { bool updated; orb_check(_control_mode_sub, &updated); + if (updated) { orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode); } + if (_control_mode.flag_control_offboard_enabled) { if (offboard_control_sp.isForceSetpoint && - offboard_control_sp_ignore_position_all(offboard_control_sp) && - offboard_control_sp_ignore_velocity_all(offboard_control_sp)) { + offboard_control_sp_ignore_position_all(offboard_control_sp) && + offboard_control_sp_ignore_velocity_all(offboard_control_sp)) { /* The offboard setpoint is a force setpoint only, directly writing to the force * setpoint topic and not publishing the setpoint triplet topic */ struct vehicle_force_setpoint_s force_sp; force_sp.x = offboard_control_sp.acceleration[0]; force_sp.y = offboard_control_sp.acceleration[1]; force_sp.z = offboard_control_sp.acceleration[2]; + //XXX: yaw if (_force_sp_pub < 0) { _force_sp_pub = orb_advertise(ORB_ID(vehicle_force_setpoint), &force_sp); + } else { orb_publish(ORB_ID(vehicle_force_setpoint), _force_sp_pub, &force_sp); } + } else { /* It's not a pure force setpoint: publish to setpoint triplet topic */ struct position_setpoint_triplet_s pos_sp_triplet; @@ -601,11 +649,12 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t /* set the local pos values if the setpoint type is 'local pos' and none * of the local pos fields is set to 'ignore' */ if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED && - !offboard_control_sp_ignore_position_some(offboard_control_sp)) { - pos_sp_triplet.current.position_valid = true; - pos_sp_triplet.current.x = offboard_control_sp.position[0]; - pos_sp_triplet.current.y = offboard_control_sp.position[1]; - pos_sp_triplet.current.z = offboard_control_sp.position[2]; + !offboard_control_sp_ignore_position_some(offboard_control_sp)) { + pos_sp_triplet.current.position_valid = true; + pos_sp_triplet.current.x = offboard_control_sp.position[0]; + pos_sp_triplet.current.y = offboard_control_sp.position[1]; + pos_sp_triplet.current.z = offboard_control_sp.position[2]; + } else { pos_sp_triplet.current.position_valid = false; } @@ -613,11 +662,12 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t /* set the local vel values if the setpoint type is 'local pos' and none * of the local vel fields is set to 'ignore' */ if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED && - !offboard_control_sp_ignore_velocity_some(offboard_control_sp)) { + !offboard_control_sp_ignore_velocity_some(offboard_control_sp)) { pos_sp_triplet.current.velocity_valid = true; pos_sp_triplet.current.vx = offboard_control_sp.velocity[0]; pos_sp_triplet.current.vy = offboard_control_sp.velocity[1]; pos_sp_triplet.current.vz = offboard_control_sp.velocity[2]; + } else { pos_sp_triplet.current.velocity_valid = false; } @@ -625,13 +675,13 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t /* set the local acceleration values if the setpoint type is 'local pos' and none * of the accelerations fields is set to 'ignore' */ if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED && - !offboard_control_sp_ignore_acceleration_some(offboard_control_sp)) { + !offboard_control_sp_ignore_acceleration_some(offboard_control_sp)) { pos_sp_triplet.current.acceleration_valid = true; pos_sp_triplet.current.a_x = offboard_control_sp.acceleration[0]; pos_sp_triplet.current.a_y = offboard_control_sp.acceleration[1]; pos_sp_triplet.current.a_z = offboard_control_sp.acceleration[2]; pos_sp_triplet.current.acceleration_is_force = - offboard_control_sp.isForceSetpoint; + offboard_control_sp.isForceSetpoint; } else { pos_sp_triplet.current.acceleration_valid = false; @@ -640,7 +690,7 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t /* set the yaw sp value if the setpoint type is 'local pos' and the yaw * field is not set to 'ignore' */ if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED && - !offboard_control_sp_ignore_yaw(offboard_control_sp)) { + !offboard_control_sp_ignore_yaw(offboard_control_sp)) { pos_sp_triplet.current.yaw_valid = true; pos_sp_triplet.current.yaw = offboard_control_sp.yaw; @@ -651,22 +701,24 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t /* set the yawrate sp value if the setpoint type is 'local pos' and the yawrate * field is not set to 'ignore' */ if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED && - !offboard_control_sp_ignore_yawrate(offboard_control_sp)) { + !offboard_control_sp_ignore_yawrate(offboard_control_sp)) { pos_sp_triplet.current.yawspeed_valid = true; pos_sp_triplet.current.yawspeed = offboard_control_sp.yaw_rate; } else { pos_sp_triplet.current.yawspeed_valid = false; } + //XXX handle global pos setpoints (different MAV frames) if (_pos_sp_triplet_pub < 0) { _pos_sp_triplet_pub = orb_advertise(ORB_ID(position_setpoint_triplet), - &pos_sp_triplet); + &pos_sp_triplet); + } else { orb_publish(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_pub, - &pos_sp_triplet); + &pos_sp_triplet); } } @@ -727,12 +779,13 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) /* Only accept messages which are intended for this system */ if ((mavlink_system.sysid == set_attitude_target.target_system || - set_attitude_target.target_system == 0) && - (mavlink_system.compid == set_attitude_target.target_component || - set_attitude_target.target_component == 0)) { + set_attitude_target.target_system == 0) && + (mavlink_system.compid == set_attitude_target.target_component || + set_attitude_target.target_component == 0)) { for (int i = 0; i < 4; i++) { offboard_control_sp.attitude[i] = set_attitude_target.q[i]; } + offboard_control_sp.attitude_rate[0] = set_attitude_target.body_roll_rate; offboard_control_sp.attitude_rate[1] = set_attitude_target.body_pitch_rate; offboard_control_sp.attitude_rate[2] = set_attitude_target.body_yaw_rate; @@ -740,8 +793,9 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) /* set correct ignore flags for body rate fields: copy from mavlink message */ for (int i = 0; i < 3; i++) { offboard_control_sp.ignore &= ~(1 << (i + OFB_IGN_BIT_BODYRATE_X)); - offboard_control_sp.ignore |= (set_attitude_target.type_mask & (1 << i)) << OFB_IGN_BIT_BODYRATE_X; + offboard_control_sp.ignore |= (set_attitude_target.type_mask & (1 << i)) << OFB_IGN_BIT_BODYRATE_X; } + /* set correct ignore flags for thrust field: copy from mavlink message */ offboard_control_sp.ignore &= ~(1 << OFB_IGN_BIT_THRUST); offboard_control_sp.ignore |= ((set_attitude_target.type_mask & (1 << 6)) << OFB_IGN_BIT_THRUST); @@ -751,7 +805,7 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) offboard_control_sp.timestamp = hrt_absolute_time(); - offboard_control_sp.mode =OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE; //XXX handle rate control mode + offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE; //XXX handle rate control mode if (_offboard_control_sp_pub < 0) { _offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp); @@ -765,6 +819,7 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) if (_mavlink->get_forward_externalsp()) { bool updated; orb_check(_control_mode_sub, &updated); + if (updated) { orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode); } @@ -773,18 +828,20 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) /* Publish attitude setpoint if attitude and thrust ignore bits are not set */ if (!(offboard_control_sp_ignore_attitude(offboard_control_sp) || - offboard_control_sp_ignore_thrust(offboard_control_sp))) { + offboard_control_sp_ignore_thrust(offboard_control_sp))) { struct vehicle_attitude_setpoint_s att_sp; att_sp.timestamp = hrt_absolute_time(); mavlink_quaternion_to_euler(set_attitude_target.q, - &att_sp.roll_body, &att_sp.pitch_body, &att_sp.yaw_body); + &att_sp.roll_body, &att_sp.pitch_body, &att_sp.yaw_body); mavlink_quaternion_to_dcm(set_attitude_target.q, att_sp.R_body); att_sp.R_valid = true; att_sp.thrust = set_attitude_target.thrust; memcpy(att_sp.q_d, set_attitude_target.q, sizeof(att_sp.q_d)); att_sp.q_d_valid = true; + if (_att_sp_pub < 0) { _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); + } else { orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &att_sp); } @@ -793,7 +850,7 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) /* Publish attitude rate setpoint if bodyrate and thrust ignore bits are not set */ ///XXX add support for ignoring individual axes if (!(offboard_control_sp_ignore_bodyrates_some(offboard_control_sp) || - offboard_control_sp_ignore_thrust(offboard_control_sp))) { + offboard_control_sp_ignore_thrust(offboard_control_sp))) { struct vehicle_rates_setpoint_s rates_sp; rates_sp.timestamp = hrt_absolute_time(); rates_sp.roll = set_attitude_target.body_roll_rate; @@ -803,6 +860,7 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) if (_att_sp_pub < 0) { _rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp); + } else { orb_publish(ORB_ID(vehicle_rates_setpoint), _rates_sp_pub, &rates_sp); } @@ -861,7 +919,8 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg) manual.r = man.r / 1000.0f; manual.z = man.z / 1000.0f; - warnx("pitch: %.2f, roll: %.2f, yaw: %.2f, throttle: %.2f", (double)manual.x, (double)manual.y, (double)manual.r, (double)manual.z); + warnx("pitch: %.2f, roll: %.2f, yaw: %.2f, throttle: %.2f", (double)manual.x, (double)manual.y, (double)manual.r, + (double)manual.z); if (_manual_pub < 0) { _manual_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual); @@ -918,6 +977,61 @@ MavlinkReceiver::handle_message_request_data_stream(mavlink_message_t *msg) } } +void +MavlinkReceiver::handle_message_system_time(mavlink_message_t *msg) +{ + mavlink_system_time_t time; + mavlink_msg_system_time_decode(msg, &time); + + timespec tv; + clock_gettime(CLOCK_REALTIME, &tv); + + // date -d @1234567890: Sat Feb 14 02:31:30 MSK 2009 + bool onb_unix_valid = tv.tv_sec > 1234567890L; + bool ofb_unix_valid = time.time_unix_usec > 1234567890L * 1000; + + if (!onb_unix_valid && ofb_unix_valid) { + tv.tv_sec = time.time_unix_usec / 1000000; + tv.tv_nsec = (time.time_unix_usec % 1000000) * 1000; + clock_settime(CLOCK_REALTIME, &tv); + warnx("[timesync] Set system time from SYSTEM_TIME message"); + } + +} + +void +MavlinkReceiver::handle_message_timesync(mavlink_message_t *msg) +{ + mavlink_timesync_t tsync; + mavlink_msg_timesync_decode(msg, &tsync); + + uint64_t now_ns = hrt_absolute_time() * 1000 ; + + if (tsync.tc1 == -1) { + + mavlink_timesync_t rsync; // return sync message + + rsync.tc1 = now_ns; + rsync.ts1 = tsync.ts1; + + _mavlink->send_message(MAVLINK_MSG_ID_TIMESYNC, &rsync); + + } else if (tsync.tc1 > -1) { + + int64_t offset_ns = ((tsync.ts1 + now_ns) - (tsync.tc1 * 2)) / 2; + int64_t dt = _time_offset - offset_ns; + + if (dt > 1000000 || dt < -100000) { // 1 millisecond skew XXX Make this + _time_offset = offset_ns; // hard-set it. + warnx("[timesync] Timesync offset is off. Hard-setting offset"); + + } else { + average_time_offset(offset_ns); + } + } + +} + void MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) { @@ -1385,6 +1499,21 @@ void MavlinkReceiver::print_status() } +uint64_t MavlinkReceiver::to_hrt(uint64_t usec) +{ + return usec - (_time_offset / 1000) ; +} + +void MavlinkReceiver::average_time_offset(uint64_t offset_ns) +{ + /* alpha = 0.8 fixed for now. The closer alpha is to 1.0, + * the faster the moving average updates in response to + * new offset samples. + */ + + _time_offset = (_time_offset_avg_alpha * offset_ns) + (1.0 - _time_offset_avg_alpha) * _time_offset; +} + void *MavlinkReceiver::start_helper(void *context) { MavlinkReceiver *rcv = new MavlinkReceiver((Mavlink *)context); diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index a057074a78..71273786de 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -124,12 +124,24 @@ private: void handle_message_manual_control(mavlink_message_t *msg); void handle_message_heartbeat(mavlink_message_t *msg); void handle_message_request_data_stream(mavlink_message_t *msg); + void handle_message_system_time(mavlink_message_t *msg); + void handle_message_timesync(mavlink_message_t *msg); void handle_message_hil_sensor(mavlink_message_t *msg); void handle_message_hil_gps(mavlink_message_t *msg); void handle_message_hil_state_quaternion(mavlink_message_t *msg); void *receive_thread(void *arg); + /** + * Convert remote nsec timestamp to local hrt time (usec) + */ + uint64_t to_hrt(uint64_t nsec); + + /** + * Exponential moving average filter to smooth time offset + */ + void average_time_offset(uint64_t offset_ns); + mavlink_status_t status; struct vehicle_local_position_s hil_local_pos; struct vehicle_control_mode_s _control_mode; @@ -164,6 +176,8 @@ private: bool _hil_local_proj_inited; float _hil_local_alt0; struct map_projection_reference_s _hil_local_proj_ref; + double _time_offset_avg_alpha; + uint64_t _time_offset; /* do not allow copying this class */ MavlinkReceiver(const MavlinkReceiver&); From aa40c69853be0dc7e79bc3084472b77f9667c1f1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 15 Dec 2014 15:34:19 +0100 Subject: [PATCH 158/652] 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 159/652] 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 160/652] 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 8c0d7047b209c23cb14b3a09bf20599af0f8cc6e Mon Sep 17 00:00:00 2001 From: "M.H.Kabir" Date: Tue, 16 Dec 2014 13:43:48 +0530 Subject: [PATCH 161/652] Working now. --- src/modules/mavlink/mavlink_messages.cpp | 2 +- src/modules/mavlink/mavlink_receiver.cpp | 16 +++++++++------- 2 files changed, 10 insertions(+), 8 deletions(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 85643270f0..dcf9aeb64f 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -955,7 +955,7 @@ protected: void send(const hrt_abstime t) { mavlink_timesync_t msg; - msg.tc1 = -1; + msg.tc1 = 0; msg.ts1 = hrt_absolute_time() * 1000; // boot time in nanoseconds _mavlink->send_message(MAVLINK_MSG_ID_TIMESYNC, &msg); diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 13e17ab64c..cd2ef1ffd8 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -122,7 +122,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _hil_local_proj_inited(0), _hil_local_alt0(0.0f), _hil_local_proj_ref {}, - _time_offset_avg_alpha(0.8f), + _time_offset_avg_alpha(0.75), _time_offset(0) { @@ -1007,22 +1007,24 @@ MavlinkReceiver::handle_message_timesync(mavlink_message_t *msg) uint64_t now_ns = hrt_absolute_time() * 1000 ; - if (tsync.tc1 == -1) { + if (tsync.tc1 == 0) { - mavlink_timesync_t rsync; // return sync message + mavlink_timesync_t rsync; // return timestamped sync message rsync.tc1 = now_ns; rsync.ts1 = tsync.ts1; _mavlink->send_message(MAVLINK_MSG_ID_TIMESYNC, &rsync); - } else if (tsync.tc1 > -1) { + return; + + } else if (tsync.tc1 > 0) { int64_t offset_ns = ((tsync.ts1 + now_ns) - (tsync.tc1 * 2)) / 2; int64_t dt = _time_offset - offset_ns; - if (dt > 1000000 || dt < -100000) { // 1 millisecond skew XXX Make this - _time_offset = offset_ns; // hard-set it. + if (dt > 10000000 || dt < -1000000) { // 10 millisecond skew + _time_offset = offset_ns; warnx("[timesync] Timesync offset is off. Hard-setting offset"); } else { @@ -1506,7 +1508,7 @@ uint64_t MavlinkReceiver::to_hrt(uint64_t usec) void MavlinkReceiver::average_time_offset(uint64_t offset_ns) { - /* alpha = 0.8 fixed for now. The closer alpha is to 1.0, + /* alpha = 0.75 fixed for now. The closer alpha is to 1.0, * the faster the moving average updates in response to * new offset samples. */ From f7da65f819db2e5d6602cd494119d8dccfd5ed56 Mon Sep 17 00:00:00 2001 From: "M.H.Kabir" Date: Tue, 16 Dec 2014 14:08:28 +0530 Subject: [PATCH 162/652] Fix formatting --- src/modules/mavlink/mavlink_messages.cpp | 704 ++++++++++++++--------- src/modules/mavlink/mavlink_receiver.cpp | 211 +++---- 2 files changed, 502 insertions(+), 413 deletions(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index bad788ad0e..f674d6501e 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -121,93 +121,91 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set switch (status->nav_state) { - case NAVIGATION_STATE_MANUAL: - *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED - | (status->is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0); - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL; - break; + case NAVIGATION_STATE_MANUAL: + *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED + | (status->is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0); + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL; + break; - case NAVIGATION_STATE_ACRO: - *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ACRO; - break; + case NAVIGATION_STATE_ACRO: + *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ACRO; + break; - case NAVIGATION_STATE_ALTCTL: - *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED - | MAV_MODE_FLAG_STABILIZE_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ALTCTL; - break; + case NAVIGATION_STATE_ALTCTL: + *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED + | MAV_MODE_FLAG_STABILIZE_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ALTCTL; + break; - case NAVIGATION_STATE_POSCTL: - *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED - | MAV_MODE_FLAG_STABILIZE_ENABLED - | MAV_MODE_FLAG_GUIDED_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_POSCTL; - break; + case NAVIGATION_STATE_POSCTL: + *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED + | MAV_MODE_FLAG_STABILIZE_ENABLED + | MAV_MODE_FLAG_GUIDED_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_POSCTL; + break; - case NAVIGATION_STATE_AUTO_MISSION: - *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED - | MAV_MODE_FLAG_STABILIZE_ENABLED - | MAV_MODE_FLAG_GUIDED_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; - custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION; - break; + case NAVIGATION_STATE_AUTO_MISSION: + *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED + | MAV_MODE_FLAG_STABILIZE_ENABLED + | MAV_MODE_FLAG_GUIDED_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION; + break; - case NAVIGATION_STATE_AUTO_LOITER: - *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED - | MAV_MODE_FLAG_STABILIZE_ENABLED - | MAV_MODE_FLAG_GUIDED_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; - custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER; - break; + case NAVIGATION_STATE_AUTO_LOITER: + *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED + | MAV_MODE_FLAG_STABILIZE_ENABLED + | MAV_MODE_FLAG_GUIDED_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER; + break; - case NAVIGATION_STATE_AUTO_RTL: + case NAVIGATION_STATE_AUTO_RTL: + /* fallthrough */ + case NAVIGATION_STATE_AUTO_RCRECOVER: + *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED + | MAV_MODE_FLAG_STABILIZE_ENABLED + | MAV_MODE_FLAG_GUIDED_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL; + break; - /* fallthrough */ - case NAVIGATION_STATE_AUTO_RCRECOVER: - *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED - | MAV_MODE_FLAG_STABILIZE_ENABLED - | MAV_MODE_FLAG_GUIDED_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; - custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL; - break; + case NAVIGATION_STATE_LAND: + case NAVIGATION_STATE_AUTO_LANDENGFAIL: + case NAVIGATION_STATE_AUTO_LANDGPSFAIL: + /* fallthrough */ + case NAVIGATION_STATE_DESCEND: + *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED + | MAV_MODE_FLAG_STABILIZE_ENABLED + | MAV_MODE_FLAG_GUIDED_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND; + break; - case NAVIGATION_STATE_LAND: - case NAVIGATION_STATE_AUTO_LANDENGFAIL: - case NAVIGATION_STATE_AUTO_LANDGPSFAIL: + case NAVIGATION_STATE_AUTO_RTGS: + *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED + | MAV_MODE_FLAG_STABILIZE_ENABLED + | MAV_MODE_FLAG_GUIDED_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTGS; + break; - /* fallthrough */ - case NAVIGATION_STATE_DESCEND: - *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED - | MAV_MODE_FLAG_STABILIZE_ENABLED - | MAV_MODE_FLAG_GUIDED_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; - custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND; - break; + case NAVIGATION_STATE_TERMINATION: + *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL; + break; - case NAVIGATION_STATE_AUTO_RTGS: - *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED - | MAV_MODE_FLAG_STABILIZE_ENABLED - | MAV_MODE_FLAG_GUIDED_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; - custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTGS; - break; + case NAVIGATION_STATE_OFFBOARD: + *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED + | MAV_MODE_FLAG_STABILIZE_ENABLED + | MAV_MODE_FLAG_GUIDED_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_OFFBOARD; + break; - case NAVIGATION_STATE_TERMINATION: - *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL; - break; - - case NAVIGATION_STATE_OFFBOARD: - *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED - | MAV_MODE_FLAG_STABILIZE_ENABLED - | MAV_MODE_FLAG_GUIDED_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_OFFBOARD; - break; - - case NAVIGATION_STATE_MAX: - /* this is an unused case, ignore */ - break; + case NAVIGATION_STATE_MAX: + /* this is an unused case, ignore */ + break; } @@ -240,23 +238,28 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set class MavlinkStreamHeartbeat : public MavlinkStream { public: - const char *get_name() const { + const char *get_name() const + { return MavlinkStreamHeartbeat::get_name_static(); } - static const char *get_name_static() { + static const char *get_name_static() + { return "HEARTBEAT"; } - uint8_t get_id() { + uint8_t get_id() + { return MAVLINK_MSG_ID_HEARTBEAT; } - static MavlinkStream *new_instance(Mavlink *mavlink) { + static MavlinkStream *new_instance(Mavlink *mavlink) + { return new MavlinkStreamHeartbeat(mavlink); } - unsigned get_size() { + unsigned get_size() + { return MAVLINK_MSG_ID_HEARTBEAT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } @@ -270,7 +273,7 @@ private: /* do not allow top copying this class */ MavlinkStreamHeartbeat(MavlinkStreamHeartbeat &); - MavlinkStreamHeartbeat &operator = (const MavlinkStreamHeartbeat &); + MavlinkStreamHeartbeat& operator = (const MavlinkStreamHeartbeat &); protected: explicit MavlinkStreamHeartbeat(Mavlink *mavlink) : MavlinkStream(mavlink), @@ -278,7 +281,8 @@ protected: _pos_sp_triplet_sub(_mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet))) {} - void send(const hrt_abstime t) { + void send(const hrt_abstime t) + { struct vehicle_status_s status; struct position_setpoint_triplet_s pos_sp_triplet; @@ -309,31 +313,34 @@ protected: class MavlinkStreamStatustext : public MavlinkStream { public: - const char *get_name() const { + const char *get_name() const + { return MavlinkStreamStatustext::get_name_static(); } - static const char *get_name_static() { + static const char *get_name_static() + { return "STATUSTEXT"; } - uint8_t get_id() { + uint8_t get_id() + { return MAVLINK_MSG_ID_STATUSTEXT; } - static MavlinkStream *new_instance(Mavlink *mavlink) { + static MavlinkStream *new_instance(Mavlink *mavlink) + { return new MavlinkStreamStatustext(mavlink); } unsigned get_size() { - return mavlink_logbuffer_is_empty(_mavlink->get_logbuffer()) ? 0 : (MAVLINK_MSG_ID_STATUSTEXT_LEN + - MAVLINK_NUM_NON_PAYLOAD_BYTES); + return mavlink_logbuffer_is_empty(_mavlink->get_logbuffer()) ? 0 : (MAVLINK_MSG_ID_STATUSTEXT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES); } private: /* do not allow top copying this class */ MavlinkStreamStatustext(MavlinkStreamStatustext &); - MavlinkStreamStatustext &operator = (const MavlinkStreamStatustext &); + MavlinkStreamStatustext& operator = (const MavlinkStreamStatustext &); FILE *fp = nullptr; protected: @@ -346,7 +353,8 @@ protected: } } - void send(const hrt_abstime t) { + void send(const hrt_abstime t) + { if (!mavlink_logbuffer_is_empty(_mavlink->get_logbuffer())) { struct mavlink_logmessage logmsg; int lb_ret = mavlink_logbuffer_read(_mavlink->get_logbuffer(), &logmsg); @@ -365,7 +373,6 @@ protected: fputs(msg.text, fp); fputs("\n", fp); fsync(fileno(fp)); - } else { /* string to hold the path to the log */ char log_file_name[32] = ""; @@ -392,19 +399,23 @@ protected: class MavlinkStreamCommandLong : public MavlinkStream { public: - const char *get_name() const { + const char *get_name() const + { return MavlinkStreamCommandLong::get_name_static(); } - static const char *get_name_static() { + static const char *get_name_static() + { return "COMMAND_LONG"; } - uint8_t get_id() { + uint8_t get_id() + { return MAVLINK_MSG_ID_COMMAND_LONG; } - static MavlinkStream *new_instance(Mavlink *mavlink) { + static MavlinkStream *new_instance(Mavlink *mavlink) + { return new MavlinkStreamCommandLong(mavlink); } @@ -418,7 +429,7 @@ private: /* do not allow top copying this class */ MavlinkStreamCommandLong(MavlinkStreamCommandLong &); - MavlinkStreamCommandLong &operator = (const MavlinkStreamCommandLong &); + MavlinkStreamCommandLong& operator = (const MavlinkStreamCommandLong &); protected: explicit MavlinkStreamCommandLong(Mavlink *mavlink) : MavlinkStream(mavlink), @@ -426,7 +437,8 @@ protected: _cmd_time(0) {} - void send(const hrt_abstime t) { + void send(const hrt_abstime t) + { struct vehicle_command_s cmd; if (_cmd_sub->update(&_cmd_time, &cmd)) { @@ -455,23 +467,28 @@ protected: class MavlinkStreamSysStatus : public MavlinkStream { public: - const char *get_name() const { + const char *get_name() const + { return MavlinkStreamSysStatus::get_name_static(); } - static const char *get_name_static() { + static const char *get_name_static() + { return "SYS_STATUS"; } - uint8_t get_id() { + uint8_t get_id() + { return MAVLINK_MSG_ID_SYS_STATUS; } - static MavlinkStream *new_instance(Mavlink *mavlink) { + static MavlinkStream *new_instance(Mavlink *mavlink) + { return new MavlinkStreamSysStatus(mavlink); } - unsigned get_size() { + unsigned get_size() + { return MAVLINK_MSG_ID_SYS_STATUS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } @@ -480,14 +497,15 @@ private: /* do not allow top copying this class */ MavlinkStreamSysStatus(MavlinkStreamSysStatus &); - MavlinkStreamSysStatus &operator = (const MavlinkStreamSysStatus &); + MavlinkStreamSysStatus& operator = (const MavlinkStreamSysStatus &); protected: explicit MavlinkStreamSysStatus(Mavlink *mavlink) : MavlinkStream(mavlink), _status_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_status))) {} - void send(const hrt_abstime t) { + void send(const hrt_abstime t) + { struct vehicle_status_s status; if (_status_sub->update(&status)) { @@ -516,23 +534,28 @@ protected: class MavlinkStreamHighresIMU : public MavlinkStream { public: - const char *get_name() const { + const char *get_name() const + { return MavlinkStreamHighresIMU::get_name_static(); } - static const char *get_name_static() { + static const char *get_name_static() + { return "HIGHRES_IMU"; } - uint8_t get_id() { + uint8_t get_id() + { return MAVLINK_MSG_ID_HIGHRES_IMU; } - static MavlinkStream *new_instance(Mavlink *mavlink) { + static MavlinkStream *new_instance(Mavlink *mavlink) + { return new MavlinkStreamHighresIMU(mavlink); } - unsigned get_size() { + unsigned get_size() + { return MAVLINK_MSG_ID_HIGHRES_IMU_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } @@ -547,7 +570,7 @@ private: /* do not allow top copying this class */ MavlinkStreamHighresIMU(MavlinkStreamHighresIMU &); - MavlinkStreamHighresIMU &operator = (const MavlinkStreamHighresIMU &); + MavlinkStreamHighresIMU& operator = (const MavlinkStreamHighresIMU &); protected: explicit MavlinkStreamHighresIMU(Mavlink *mavlink) : MavlinkStream(mavlink), @@ -559,7 +582,8 @@ protected: _baro_timestamp(0) {} - void send(const hrt_abstime t) { + void send(const hrt_abstime t) + { struct sensor_combined_s sensor; if (_sensor_sub->update(&_sensor_time, &sensor)) { @@ -616,23 +640,28 @@ protected: class MavlinkStreamAttitude : public MavlinkStream { public: - const char *get_name() const { + const char *get_name() const + { return MavlinkStreamAttitude::get_name_static(); } - static const char *get_name_static() { + static const char *get_name_static() + { return "ATTITUDE"; } - uint8_t get_id() { + uint8_t get_id() + { return MAVLINK_MSG_ID_ATTITUDE; } - static MavlinkStream *new_instance(Mavlink *mavlink) { + static MavlinkStream *new_instance(Mavlink *mavlink) + { return new MavlinkStreamAttitude(mavlink); } - unsigned get_size() { + unsigned get_size() + { return MAVLINK_MSG_ID_ATTITUDE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } @@ -642,7 +671,7 @@ private: /* do not allow top copying this class */ MavlinkStreamAttitude(MavlinkStreamAttitude &); - MavlinkStreamAttitude &operator = (const MavlinkStreamAttitude &); + MavlinkStreamAttitude& operator = (const MavlinkStreamAttitude &); protected: @@ -651,7 +680,8 @@ protected: _att_time(0) {} - void send(const hrt_abstime t) { + void send(const hrt_abstime t) + { struct vehicle_attitude_s att; if (_att_sub->update(&_att_time, &att)) { @@ -674,23 +704,28 @@ protected: class MavlinkStreamAttitudeQuaternion : public MavlinkStream { public: - const char *get_name() const { + const char *get_name() const + { return MavlinkStreamAttitudeQuaternion::get_name_static(); } - static const char *get_name_static() { + static const char *get_name_static() + { return "ATTITUDE_QUATERNION"; } - uint8_t get_id() { + uint8_t get_id() + { return MAVLINK_MSG_ID_ATTITUDE_QUATERNION; } - static MavlinkStream *new_instance(Mavlink *mavlink) { + static MavlinkStream *new_instance(Mavlink *mavlink) + { return new MavlinkStreamAttitudeQuaternion(mavlink); } - unsigned get_size() { + unsigned get_size() + { return MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } @@ -700,7 +735,7 @@ private: /* do not allow top copying this class */ MavlinkStreamAttitudeQuaternion(MavlinkStreamAttitudeQuaternion &); - MavlinkStreamAttitudeQuaternion &operator = (const MavlinkStreamAttitudeQuaternion &); + MavlinkStreamAttitudeQuaternion& operator = (const MavlinkStreamAttitudeQuaternion &); protected: explicit MavlinkStreamAttitudeQuaternion(Mavlink *mavlink) : MavlinkStream(mavlink), @@ -708,7 +743,8 @@ protected: _att_time(0) {} - void send(const hrt_abstime t) { + void send(const hrt_abstime t) + { struct vehicle_attitude_s att; if (_att_sub->update(&_att_time, &att)) { @@ -733,23 +769,28 @@ class MavlinkStreamVFRHUD : public MavlinkStream { public: - const char *get_name() const { + const char *get_name() const + { return MavlinkStreamVFRHUD::get_name_static(); } - static const char *get_name_static() { + static const char *get_name_static() + { return "VFR_HUD"; } - uint8_t get_id() { + uint8_t get_id() + { return MAVLINK_MSG_ID_VFR_HUD; } - static MavlinkStream *new_instance(Mavlink *mavlink) { + static MavlinkStream *new_instance(Mavlink *mavlink) + { return new MavlinkStreamVFRHUD(mavlink); } - unsigned get_size() { + unsigned get_size() + { return MAVLINK_MSG_ID_VFR_HUD_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } @@ -771,7 +812,7 @@ private: /* do not allow top copying this class */ MavlinkStreamVFRHUD(MavlinkStreamVFRHUD &); - MavlinkStreamVFRHUD &operator = (const MavlinkStreamVFRHUD &); + MavlinkStreamVFRHUD& operator = (const MavlinkStreamVFRHUD &); protected: explicit MavlinkStreamVFRHUD(Mavlink *mavlink) : MavlinkStream(mavlink), @@ -787,7 +828,8 @@ protected: _airspeed_time(0) {} - void send(const hrt_abstime t) { + void send(const hrt_abstime t) + { struct vehicle_attitude_s att; struct vehicle_global_position_s pos; struct actuator_armed_s armed; @@ -819,23 +861,28 @@ protected: class MavlinkStreamGPSRawInt : public MavlinkStream { public: - const char *get_name() const { + const char *get_name() const + { return MavlinkStreamGPSRawInt::get_name_static(); } - static const char *get_name_static() { + static const char *get_name_static() + { return "GPS_RAW_INT"; } - uint8_t get_id() { + uint8_t get_id() + { return MAVLINK_MSG_ID_GPS_RAW_INT; } - static MavlinkStream *new_instance(Mavlink *mavlink) { + static MavlinkStream *new_instance(Mavlink *mavlink) + { return new MavlinkStreamGPSRawInt(mavlink); } - unsigned get_size() { + unsigned get_size() + { return MAVLINK_MSG_ID_GPS_RAW_INT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } @@ -845,7 +892,7 @@ private: /* do not allow top copying this class */ MavlinkStreamGPSRawInt(MavlinkStreamGPSRawInt &); - MavlinkStreamGPSRawInt &operator = (const MavlinkStreamGPSRawInt &); + MavlinkStreamGPSRawInt& operator = (const MavlinkStreamGPSRawInt &); protected: explicit MavlinkStreamGPSRawInt(Mavlink *mavlink) : MavlinkStream(mavlink), @@ -853,7 +900,8 @@ protected: _gps_time(0) {} - void send(const hrt_abstime t) { + void send(const hrt_abstime t) + { struct vehicle_gps_position_s gps; if (_gps_sub->update(&_gps_time, &gps)) { @@ -867,8 +915,8 @@ protected: msg.eph = cm_uint16_from_m_float(gps.eph); msg.epv = cm_uint16_from_m_float(gps.epv); msg.vel = cm_uint16_from_m_float(gps.vel_m_s), - msg.cog = _wrap_2pi(gps.cog_rad) * M_RAD_TO_DEG_F * 1e2f, - msg.satellites_visible = gps.satellites_used; + msg.cog = _wrap_2pi(gps.cog_rad) * M_RAD_TO_DEG_F * 1e2f, + msg.satellites_visible = gps.satellites_used; _mavlink->send_message(MAVLINK_MSG_ID_GPS_RAW_INT, &msg); } @@ -965,23 +1013,28 @@ protected: class MavlinkStreamGlobalPositionInt : public MavlinkStream { public: - const char *get_name() const { + const char *get_name() const + { return MavlinkStreamGlobalPositionInt::get_name_static(); } - static const char *get_name_static() { + static const char *get_name_static() + { return "GLOBAL_POSITION_INT"; } - uint8_t get_id() { + uint8_t get_id() + { return MAVLINK_MSG_ID_GLOBAL_POSITION_INT; } - static MavlinkStream *new_instance(Mavlink *mavlink) { + static MavlinkStream *new_instance(Mavlink *mavlink) + { return new MavlinkStreamGlobalPositionInt(mavlink); } - unsigned get_size() { + unsigned get_size() + { return MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } @@ -994,7 +1047,7 @@ private: /* do not allow top copying this class */ MavlinkStreamGlobalPositionInt(MavlinkStreamGlobalPositionInt &); - MavlinkStreamGlobalPositionInt &operator = (const MavlinkStreamGlobalPositionInt &); + MavlinkStreamGlobalPositionInt& operator = (const MavlinkStreamGlobalPositionInt &); protected: explicit MavlinkStreamGlobalPositionInt(Mavlink *mavlink) : MavlinkStream(mavlink), @@ -1004,7 +1057,8 @@ protected: _home_time(0) {} - void send(const hrt_abstime t) { + void send(const hrt_abstime t) + { struct vehicle_global_position_s pos; struct home_position_s home; @@ -1033,23 +1087,28 @@ protected: class MavlinkStreamLocalPositionNED : public MavlinkStream { public: - const char *get_name() const { + const char *get_name() const + { return MavlinkStreamLocalPositionNED::get_name_static(); } - static const char *get_name_static() { + static const char *get_name_static() + { return "LOCAL_POSITION_NED"; } - uint8_t get_id() { + uint8_t get_id() + { return MAVLINK_MSG_ID_LOCAL_POSITION_NED; } - static MavlinkStream *new_instance(Mavlink *mavlink) { + static MavlinkStream *new_instance(Mavlink *mavlink) + { return new MavlinkStreamLocalPositionNED(mavlink); } - unsigned get_size() { + unsigned get_size() + { return MAVLINK_MSG_ID_LOCAL_POSITION_NED_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } @@ -1059,7 +1118,7 @@ private: /* do not allow top copying this class */ MavlinkStreamLocalPositionNED(MavlinkStreamLocalPositionNED &); - MavlinkStreamLocalPositionNED &operator = (const MavlinkStreamLocalPositionNED &); + MavlinkStreamLocalPositionNED& operator = (const MavlinkStreamLocalPositionNED &); protected: explicit MavlinkStreamLocalPositionNED(Mavlink *mavlink) : MavlinkStream(mavlink), @@ -1067,7 +1126,8 @@ protected: _pos_time(0) {} - void send(const hrt_abstime t) { + void send(const hrt_abstime t) + { struct vehicle_local_position_s pos; if (_pos_sub->update(&_pos_time, &pos)) { @@ -1090,23 +1150,28 @@ protected: class MavlinkStreamViconPositionEstimate : public MavlinkStream { public: - const char *get_name() const { + const char *get_name() const + { return MavlinkStreamViconPositionEstimate::get_name_static(); } - static const char *get_name_static() { + static const char *get_name_static() + { return "VICON_POSITION_ESTIMATE"; } - uint8_t get_id() { + uint8_t get_id() + { return MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE; } - static MavlinkStream *new_instance(Mavlink *mavlink) { + static MavlinkStream *new_instance(Mavlink *mavlink) + { return new MavlinkStreamViconPositionEstimate(mavlink); } - unsigned get_size() { + unsigned get_size() + { return MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } @@ -1116,7 +1181,7 @@ private: /* do not allow top copying this class */ MavlinkStreamViconPositionEstimate(MavlinkStreamViconPositionEstimate &); - MavlinkStreamViconPositionEstimate &operator = (const MavlinkStreamViconPositionEstimate &); + MavlinkStreamViconPositionEstimate& operator = (const MavlinkStreamViconPositionEstimate &); protected: explicit MavlinkStreamViconPositionEstimate(Mavlink *mavlink) : MavlinkStream(mavlink), @@ -1124,7 +1189,8 @@ protected: _pos_time(0) {} - void send(const hrt_abstime t) { + void send(const hrt_abstime t) + { struct vehicle_vicon_position_s pos; if (_pos_sub->update(&_pos_time, &pos)) { @@ -1147,23 +1213,28 @@ protected: class MavlinkStreamGPSGlobalOrigin : public MavlinkStream { public: - const char *get_name() const { + const char *get_name() const + { return MavlinkStreamGPSGlobalOrigin::get_name_static(); } - static const char *get_name_static() { + static const char *get_name_static() + { return "GPS_GLOBAL_ORIGIN"; } - uint8_t get_id() { + uint8_t get_id() + { return MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN; } - static MavlinkStream *new_instance(Mavlink *mavlink) { + static MavlinkStream *new_instance(Mavlink *mavlink) + { return new MavlinkStreamGPSGlobalOrigin(mavlink); } - unsigned get_size() { + unsigned get_size() + { return _home_sub->is_published() ? (MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; } @@ -1172,14 +1243,15 @@ private: /* do not allow top copying this class */ MavlinkStreamGPSGlobalOrigin(MavlinkStreamGPSGlobalOrigin &); - MavlinkStreamGPSGlobalOrigin &operator = (const MavlinkStreamGPSGlobalOrigin &); + MavlinkStreamGPSGlobalOrigin& operator = (const MavlinkStreamGPSGlobalOrigin &); protected: explicit MavlinkStreamGPSGlobalOrigin(Mavlink *mavlink) : MavlinkStream(mavlink), _home_sub(_mavlink->add_orb_subscription(ORB_ID(home_position))) {} - void send(const hrt_abstime t) { + void send(const hrt_abstime t) + { /* we're sending the GPS home periodically to ensure the * the GCS does pick it up at one point */ if (_home_sub->is_published()) { @@ -1203,35 +1275,40 @@ template class MavlinkStreamServoOutputRaw : public MavlinkStream { public: - const char *get_name() const { + const char *get_name() const + { return MavlinkStreamServoOutputRaw::get_name_static(); } - uint8_t get_id() { + uint8_t get_id() + { return MAVLINK_MSG_ID_SERVO_OUTPUT_RAW; } - static const char *get_name_static() { + static const char *get_name_static() + { switch (N) { - case 0: + case 0: return "SERVO_OUTPUT_RAW_0"; - case 1: + case 1: return "SERVO_OUTPUT_RAW_1"; - case 2: + case 2: return "SERVO_OUTPUT_RAW_2"; - case 3: + case 3: return "SERVO_OUTPUT_RAW_3"; } } - static MavlinkStream *new_instance(Mavlink *mavlink) { + static MavlinkStream *new_instance(Mavlink *mavlink) + { return new MavlinkStreamServoOutputRaw(mavlink); } - unsigned get_size() { + unsigned get_size() + { return MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } @@ -1241,12 +1318,13 @@ private: /* do not allow top copying this class */ MavlinkStreamServoOutputRaw(MavlinkStreamServoOutputRaw &); - MavlinkStreamServoOutputRaw &operator = (const MavlinkStreamServoOutputRaw &); + MavlinkStreamServoOutputRaw& operator = (const MavlinkStreamServoOutputRaw &); protected: explicit MavlinkStreamServoOutputRaw(Mavlink *mavlink) : MavlinkStream(mavlink), _act_sub(nullptr), - _act_time(0) { + _act_time(0) + { orb_id_t act_topics[] = { ORB_ID(actuator_outputs_0), ORB_ID(actuator_outputs_1), @@ -1257,7 +1335,8 @@ protected: _act_sub = _mavlink->add_orb_subscription(act_topics[N]); } - void send(const hrt_abstime t) { + void send(const hrt_abstime t) + { struct actuator_outputs_s act; if (_act_sub->update(&_act_time, &act)) { @@ -1283,23 +1362,28 @@ protected: class MavlinkStreamHILControls : public MavlinkStream { public: - const char *get_name() const { + const char *get_name() const + { return MavlinkStreamHILControls::get_name_static(); } - static const char *get_name_static() { + static const char *get_name_static() + { return "HIL_CONTROLS"; } - uint8_t get_id() { + uint8_t get_id() + { return MAVLINK_MSG_ID_HIL_CONTROLS; } - static MavlinkStream *new_instance(Mavlink *mavlink) { + static MavlinkStream *new_instance(Mavlink *mavlink) + { return new MavlinkStreamHILControls(mavlink); } - unsigned get_size() { + unsigned get_size() + { return MAVLINK_MSG_ID_HIL_CONTROLS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } @@ -1315,7 +1399,7 @@ private: /* do not allow top copying this class */ MavlinkStreamHILControls(MavlinkStreamHILControls &); - MavlinkStreamHILControls &operator = (const MavlinkStreamHILControls &); + MavlinkStreamHILControls& operator = (const MavlinkStreamHILControls &); protected: explicit MavlinkStreamHILControls(Mavlink *mavlink) : MavlinkStream(mavlink), @@ -1327,7 +1411,8 @@ protected: _act_time(0) {} - void send(const hrt_abstime t) { + void send(const hrt_abstime t) + { struct vehicle_status_s status; struct position_setpoint_triplet_s pos_sp_triplet; struct actuator_outputs_s act; @@ -1443,23 +1528,28 @@ protected: class MavlinkStreamPositionTargetGlobalInt : public MavlinkStream { public: - const char *get_name() const { + const char *get_name() const + { return MavlinkStreamPositionTargetGlobalInt::get_name_static(); } - static const char *get_name_static() { + static const char *get_name_static() + { return "POSITION_TARGET_GLOBAL_INT"; } - uint8_t get_id() { + uint8_t get_id() + { return MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT; } - static MavlinkStream *new_instance(Mavlink *mavlink) { + static MavlinkStream *new_instance(Mavlink *mavlink) + { return new MavlinkStreamPositionTargetGlobalInt(mavlink); } - unsigned get_size() { + unsigned get_size() + { return MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } @@ -1468,18 +1558,19 @@ private: /* do not allow top copying this class */ MavlinkStreamPositionTargetGlobalInt(MavlinkStreamPositionTargetGlobalInt &); - MavlinkStreamPositionTargetGlobalInt &operator = (const MavlinkStreamPositionTargetGlobalInt &); + MavlinkStreamPositionTargetGlobalInt& operator = (const MavlinkStreamPositionTargetGlobalInt &); protected: explicit MavlinkStreamPositionTargetGlobalInt(Mavlink *mavlink) : MavlinkStream(mavlink), _pos_sp_triplet_sub(_mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet))) {} - void send(const hrt_abstime t) { + void send(const hrt_abstime t) + { struct position_setpoint_triplet_s pos_sp_triplet; if (_pos_sp_triplet_sub->update(&pos_sp_triplet)) { - mavlink_position_target_global_int_t msg {}; + mavlink_position_target_global_int_t msg{}; msg.coordinate_frame = MAV_FRAME_GLOBAL; msg.lat_int = pos_sp_triplet.current.lat * 1e7; @@ -1495,23 +1586,28 @@ protected: class MavlinkStreamLocalPositionSetpoint : public MavlinkStream { public: - const char *get_name() const { + const char *get_name() const + { return MavlinkStreamLocalPositionSetpoint::get_name_static(); } - static const char *get_name_static() { + static const char *get_name_static() + { return "POSITION_TARGET_LOCAL_NED"; } - uint8_t get_id() { + uint8_t get_id() + { return MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED; } - static MavlinkStream *new_instance(Mavlink *mavlink) { + static MavlinkStream *new_instance(Mavlink *mavlink) + { return new MavlinkStreamLocalPositionSetpoint(mavlink); } - unsigned get_size() { + unsigned get_size() + { return MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } @@ -1521,7 +1617,7 @@ private: /* do not allow top copying this class */ MavlinkStreamLocalPositionSetpoint(MavlinkStreamLocalPositionSetpoint &); - MavlinkStreamLocalPositionSetpoint &operator = (const MavlinkStreamLocalPositionSetpoint &); + MavlinkStreamLocalPositionSetpoint& operator = (const MavlinkStreamLocalPositionSetpoint &); protected: explicit MavlinkStreamLocalPositionSetpoint(Mavlink *mavlink) : MavlinkStream(mavlink), @@ -1529,11 +1625,12 @@ protected: _pos_sp_time(0) {} - void send(const hrt_abstime t) { + void send(const hrt_abstime t) + { struct vehicle_local_position_setpoint_s pos_sp; if (_pos_sp_sub->update(&_pos_sp_time, &pos_sp)) { - mavlink_position_target_local_ned_t msg {}; + mavlink_position_target_local_ned_t msg{}; msg.time_boot_ms = pos_sp.timestamp / 1000; msg.coordinate_frame = MAV_FRAME_LOCAL_NED; @@ -1550,23 +1647,28 @@ protected: class MavlinkStreamAttitudeTarget : public MavlinkStream { public: - const char *get_name() const { + const char *get_name() const + { return MavlinkStreamAttitudeTarget::get_name_static(); } - static const char *get_name_static() { + static const char *get_name_static() + { return "ATTITUDE_TARGET"; } - uint8_t get_id() { + uint8_t get_id() + { return MAVLINK_MSG_ID_ATTITUDE_TARGET; } - static MavlinkStream *new_instance(Mavlink *mavlink) { + static MavlinkStream *new_instance(Mavlink *mavlink) + { return new MavlinkStreamAttitudeTarget(mavlink); } - unsigned get_size() { + unsigned get_size() + { return MAVLINK_MSG_ID_ATTITUDE_TARGET_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } @@ -1578,7 +1680,7 @@ private: /* do not allow top copying this class */ MavlinkStreamAttitudeTarget(MavlinkStreamAttitudeTarget &); - MavlinkStreamAttitudeTarget &operator = (const MavlinkStreamAttitudeTarget &); + MavlinkStreamAttitudeTarget& operator = (const MavlinkStreamAttitudeTarget &); protected: explicit MavlinkStreamAttitudeTarget(Mavlink *mavlink) : MavlinkStream(mavlink), @@ -1588,7 +1690,8 @@ protected: _att_rates_sp_time(0) {} - void send(const hrt_abstime t) { + void send(const hrt_abstime t) + { struct vehicle_attitude_setpoint_s att_sp; if (_att_sp_sub->update(&_att_sp_time, &att_sp)) { @@ -1596,7 +1699,7 @@ protected: struct vehicle_rates_setpoint_s att_rates_sp; (void)_att_rates_sp_sub->update(&_att_rates_sp_time, &att_rates_sp); - mavlink_attitude_target_t msg {}; + mavlink_attitude_target_t msg{}; msg.time_boot_ms = att_sp.timestamp / 1000; mavlink_euler_to_quaternion(att_sp.roll_body, att_sp.pitch_body, att_sp.yaw_body, msg.q); @@ -1616,26 +1719,30 @@ protected: class MavlinkStreamRCChannelsRaw : public MavlinkStream { public: - const char *get_name() const { + const char *get_name() const + { return MavlinkStreamRCChannelsRaw::get_name_static(); } - static const char *get_name_static() { + static const char *get_name_static() + { return "RC_CHANNELS_RAW"; } - uint8_t get_id() { + uint8_t get_id() + { return MAVLINK_MSG_ID_RC_CHANNELS_RAW; } - static MavlinkStream *new_instance(Mavlink *mavlink) { + static MavlinkStream *new_instance(Mavlink *mavlink) + { return new MavlinkStreamRCChannelsRaw(mavlink); } - unsigned get_size() { - return _rc_sub->is_published() ? ((RC_INPUT_MAX_CHANNELS / 8) * (MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN + - MAVLINK_NUM_NON_PAYLOAD_BYTES) + - MAVLINK_MSG_ID_RC_CHANNELS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; + unsigned get_size() + { + return _rc_sub->is_published() ? ((RC_INPUT_MAX_CHANNELS / 8) * (MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) + + MAVLINK_MSG_ID_RC_CHANNELS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; } private: @@ -1644,7 +1751,7 @@ private: /* do not allow top copying this class */ MavlinkStreamRCChannelsRaw(MavlinkStreamRCChannelsRaw &); - MavlinkStreamRCChannelsRaw &operator = (const MavlinkStreamRCChannelsRaw &); + MavlinkStreamRCChannelsRaw& operator = (const MavlinkStreamRCChannelsRaw &); protected: explicit MavlinkStreamRCChannelsRaw(Mavlink *mavlink) : MavlinkStream(mavlink), @@ -1652,7 +1759,8 @@ protected: _rc_time(0) {} - void send(const hrt_abstime t) { + void send(const hrt_abstime t) + { struct rc_input_values rc; if (_rc_sub->update(&_rc_time, &rc)) { @@ -1728,24 +1836,20 @@ protected: msg.rssi |= (rc.rssi <= 100) ? ((rc.rssi / 7) + 1) : 15; switch (rc.input_source) { - case RC_INPUT_SOURCE_PX4FMU_PPM: - + case RC_INPUT_SOURCE_PX4FMU_PPM: /* fallthrough */ - case RC_INPUT_SOURCE_PX4IO_PPM: - msg.rssi |= (0 << 4); - break; - - case RC_INPUT_SOURCE_PX4IO_SPEKTRUM: - msg.rssi |= (1 << 4); - break; - - case RC_INPUT_SOURCE_PX4IO_SBUS: - msg.rssi |= (2 << 4); - break; - - case RC_INPUT_SOURCE_PX4IO_ST24: - msg.rssi |= (3 << 4); - break; + case RC_INPUT_SOURCE_PX4IO_PPM: + msg.rssi |= (0 << 4); + break; + case RC_INPUT_SOURCE_PX4IO_SPEKTRUM: + msg.rssi |= (1 << 4); + break; + case RC_INPUT_SOURCE_PX4IO_SBUS: + msg.rssi |= (2 << 4); + break; + case RC_INPUT_SOURCE_PX4IO_ST24: + msg.rssi |= (3 << 4); + break; } if (rc.rc_lost) { @@ -1762,23 +1866,28 @@ protected: class MavlinkStreamManualControl : public MavlinkStream { public: - const char *get_name() const { + const char *get_name() const + { return MavlinkStreamManualControl::get_name_static(); } - static const char *get_name_static() { + static const char *get_name_static() + { return "MANUAL_CONTROL"; } - uint8_t get_id() { + uint8_t get_id() + { return MAVLINK_MSG_ID_MANUAL_CONTROL; } - static MavlinkStream *new_instance(Mavlink *mavlink) { + static MavlinkStream *new_instance(Mavlink *mavlink) + { return new MavlinkStreamManualControl(mavlink); } - unsigned get_size() { + unsigned get_size() + { return _manual_sub->is_published() ? (MAVLINK_MSG_ID_MANUAL_CONTROL_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; } @@ -1788,7 +1897,7 @@ private: /* do not allow top copying this class */ MavlinkStreamManualControl(MavlinkStreamManualControl &); - MavlinkStreamManualControl &operator = (const MavlinkStreamManualControl &); + MavlinkStreamManualControl& operator = (const MavlinkStreamManualControl &); protected: explicit MavlinkStreamManualControl(Mavlink *mavlink) : MavlinkStream(mavlink), @@ -1796,7 +1905,8 @@ protected: _manual_time(0) {} - void send(const hrt_abstime t) { + void send(const hrt_abstime t) + { struct manual_control_setpoint_s manual; if (_manual_sub->update(&_manual_time, &manual)) { @@ -1817,23 +1927,28 @@ protected: class MavlinkStreamOpticalFlowRad : public MavlinkStream { public: - const char *get_name() const { + const char *get_name() const + { return MavlinkStreamOpticalFlowRad::get_name_static(); } - static const char *get_name_static() { + static const char *get_name_static() + { return "OPTICAL_FLOW_RAD"; } - uint8_t get_id() { + uint8_t get_id() + { return MAVLINK_MSG_ID_OPTICAL_FLOW_RAD; } - static MavlinkStream *new_instance(Mavlink *mavlink) { + static MavlinkStream *new_instance(Mavlink *mavlink) + { return new MavlinkStreamOpticalFlowRad(mavlink); } - unsigned get_size() { + unsigned get_size() + { return _flow_sub->is_published() ? (MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; } @@ -1843,7 +1958,7 @@ private: /* do not allow top copying this class */ MavlinkStreamOpticalFlowRad(MavlinkStreamOpticalFlowRad &); - MavlinkStreamOpticalFlowRad &operator = (const MavlinkStreamOpticalFlowRad &); + MavlinkStreamOpticalFlowRad& operator = (const MavlinkStreamOpticalFlowRad &); protected: explicit MavlinkStreamOpticalFlowRad(Mavlink *mavlink) : MavlinkStream(mavlink), @@ -1851,7 +1966,8 @@ protected: _flow_time(0) {} - void send(const hrt_abstime t) { + void send(const hrt_abstime t) + { struct optical_flow_s flow; if (_flow_sub->update(&_flow_time, &flow)) { @@ -1879,23 +1995,28 @@ protected: class MavlinkStreamAttitudeControls : public MavlinkStream { public: - const char *get_name() const { + const char *get_name() const + { return MavlinkStreamAttitudeControls::get_name_static(); } - static const char *get_name_static() { + static const char *get_name_static() + { return "ATTITUDE_CONTROLS"; } - uint8_t get_id() { + uint8_t get_id() + { return 0; } - static MavlinkStream *new_instance(Mavlink *mavlink) { + static MavlinkStream *new_instance(Mavlink *mavlink) + { return new MavlinkStreamAttitudeControls(mavlink); } - unsigned get_size() { + unsigned get_size() + { return 4 * (MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES); } @@ -1905,7 +2026,7 @@ private: /* do not allow top copying this class */ MavlinkStreamAttitudeControls(MavlinkStreamAttitudeControls &); - MavlinkStreamAttitudeControls &operator = (const MavlinkStreamAttitudeControls &); + MavlinkStreamAttitudeControls& operator = (const MavlinkStreamAttitudeControls &); protected: explicit MavlinkStreamAttitudeControls(Mavlink *mavlink) : MavlinkStream(mavlink), @@ -1913,7 +2034,8 @@ protected: _att_ctrl_time(0) {} - void send(const hrt_abstime t) { + void send(const hrt_abstime t) + { struct actuator_controls_s att_ctrl; if (_att_ctrl_sub->update(&_att_ctrl_time, &att_ctrl)) { @@ -1949,23 +2071,28 @@ protected: class MavlinkStreamNamedValueFloat : public MavlinkStream { public: - const char *get_name() const { + const char *get_name() const + { return MavlinkStreamNamedValueFloat::get_name_static(); } - static const char *get_name_static() { + static const char *get_name_static() + { return "NAMED_VALUE_FLOAT"; } - uint8_t get_id() { + uint8_t get_id() + { return MAVLINK_MSG_ID_NAMED_VALUE_FLOAT; } - static MavlinkStream *new_instance(Mavlink *mavlink) { + static MavlinkStream *new_instance(Mavlink *mavlink) + { return new MavlinkStreamNamedValueFloat(mavlink); } - unsigned get_size() { + unsigned get_size() + { return MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } @@ -1975,7 +2102,7 @@ private: /* do not allow top copying this class */ MavlinkStreamNamedValueFloat(MavlinkStreamNamedValueFloat &); - MavlinkStreamNamedValueFloat &operator = (const MavlinkStreamNamedValueFloat &); + MavlinkStreamNamedValueFloat& operator = (const MavlinkStreamNamedValueFloat &); protected: explicit MavlinkStreamNamedValueFloat(Mavlink *mavlink) : MavlinkStream(mavlink), @@ -1983,7 +2110,8 @@ protected: _debug_time(0) {} - void send(const hrt_abstime t) { + void send(const hrt_abstime t) + { struct debug_key_value_s debug; if (_debug_sub->update(&_debug_time, &debug)) { @@ -2004,23 +2132,28 @@ protected: class MavlinkStreamCameraCapture : public MavlinkStream { public: - const char *get_name() const { + const char *get_name() const + { return MavlinkStreamCameraCapture::get_name_static(); } - static const char *get_name_static() { + static const char *get_name_static() + { return "CAMERA_CAPTURE"; } - uint8_t get_id() { + uint8_t get_id() + { return 0; } - static MavlinkStream *new_instance(Mavlink *mavlink) { + static MavlinkStream *new_instance(Mavlink *mavlink) + { return new MavlinkStreamCameraCapture(mavlink); } - unsigned get_size() { + unsigned get_size() + { return MAVLINK_MSG_ID_COMMAND_LONG_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } @@ -2029,14 +2162,15 @@ private: /* do not allow top copying this class */ MavlinkStreamCameraCapture(MavlinkStreamCameraCapture &); - MavlinkStreamCameraCapture &operator = (const MavlinkStreamCameraCapture &); + MavlinkStreamCameraCapture& operator = (const MavlinkStreamCameraCapture &); protected: explicit MavlinkStreamCameraCapture(Mavlink *mavlink) : MavlinkStream(mavlink), _status_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_status))) {} - void send(const hrt_abstime t) { + void send(const hrt_abstime t) + { struct vehicle_status_s status; (void)_status_sub->update(&status); @@ -2063,23 +2197,28 @@ protected: class MavlinkStreamDistanceSensor : public MavlinkStream { public: - const char *get_name() const { + const char *get_name() const + { return MavlinkStreamDistanceSensor::get_name_static(); } - static const char *get_name_static() { + static const char *get_name_static() + { return "DISTANCE_SENSOR"; } - uint8_t get_id() { + uint8_t get_id() + { return MAVLINK_MSG_ID_DISTANCE_SENSOR; } - static MavlinkStream *new_instance(Mavlink *mavlink) { + static MavlinkStream *new_instance(Mavlink *mavlink) + { return new MavlinkStreamDistanceSensor(mavlink); } - unsigned get_size() { + unsigned get_size() + { return _range_sub->is_published() ? (MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; } @@ -2089,7 +2228,7 @@ private: /* do not allow top copying this class */ MavlinkStreamDistanceSensor(MavlinkStreamDistanceSensor &); - MavlinkStreamDistanceSensor &operator = (const MavlinkStreamDistanceSensor &); + MavlinkStreamDistanceSensor& operator = (const MavlinkStreamDistanceSensor &); protected: explicit MavlinkStreamDistanceSensor(Mavlink *mavlink) : MavlinkStream(mavlink), @@ -2097,7 +2236,8 @@ protected: _range_time(0) {} - void send(const hrt_abstime t) { + void send(const hrt_abstime t) + { struct range_finder_report range; if (_range_sub->update(&_range_time, &range)) { diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index cd2ef1ffd8..72c0d47bd5 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -88,42 +88,40 @@ static const float mg2ms2 = CONSTANTS_ONE_G / 1000.0f; MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _mavlink(parent), - status {}, - hil_local_pos {}, - _control_mode {}, - _global_pos_pub(-1), - _local_pos_pub(-1), - _attitude_pub(-1), - _gps_pub(-1), - _sensors_pub(-1), - _gyro_pub(-1), - _accel_pub(-1), - _mag_pub(-1), - _baro_pub(-1), - _airspeed_pub(-1), - _battery_pub(-1), - _cmd_pub(-1), - _flow_pub(-1), - _range_pub(-1), - _offboard_control_sp_pub(-1), - _global_vel_sp_pub(-1), - _att_sp_pub(-1), - _rates_sp_pub(-1), - _force_sp_pub(-1), - _pos_sp_triplet_pub(-1), - _vicon_position_pub(-1), - _vision_position_pub(-1), - _telemetry_status_pub(-1), - _rc_pub(-1), - _manual_pub(-1), - _control_mode_sub(orb_subscribe(ORB_ID(vehicle_control_mode))), - _hil_frames(0), - _old_timestamp(0), - _hil_local_proj_inited(0), - _hil_local_alt0(0.0f), - _hil_local_proj_ref {}, - _time_offset_avg_alpha(0.75), - _time_offset(0) + status{}, + hil_local_pos{}, + _control_mode{}, + _global_pos_pub(-1), + _local_pos_pub(-1), + _attitude_pub(-1), + _gps_pub(-1), + _sensors_pub(-1), + _gyro_pub(-1), + _accel_pub(-1), + _mag_pub(-1), + _baro_pub(-1), + _airspeed_pub(-1), + _battery_pub(-1), + _cmd_pub(-1), + _flow_pub(-1), + _range_pub(-1), + _offboard_control_sp_pub(-1), + _global_vel_sp_pub(-1), + _att_sp_pub(-1), + _rates_sp_pub(-1), + _force_sp_pub(-1), + _pos_sp_triplet_pub(-1), + _vicon_position_pub(-1), + _vision_position_pub(-1), + _telemetry_status_pub(-1), + _rc_pub(-1), + _manual_pub(-1), + _control_mode_sub(orb_subscribe(ORB_ID(vehicle_control_mode))), + _hil_frames(0), + _old_timestamp(0), + _hil_local_proj_inited(0), + _hil_local_alt0(0.0f), + _hil_local_proj_ref{} { // make sure the FTP server is started @@ -232,7 +230,7 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) } - if (_mavlink->get_hil_enabled() || (_mavlink->get_use_hil_gps() && msg->sysid == mavlink_system.sysid)) { + if (_mavlink->get_hil_enabled() || (_mavlink->get_use_hil_gps() && msg->sysid == mavlink_system.sysid)) { switch (msg->msgid) { case MAVLINK_MSG_ID_HIL_GPS: handle_message_hil_gps(msg); @@ -244,7 +242,7 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) } - /* If we've received a valid message, mark the flag indicating so. + /* If we've received a valid message, mark the flag indicating so. This is used in the '-w' command-line flag. */ _mavlink->set_has_received_messages(true); } @@ -277,35 +275,22 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg) } struct vehicle_command_s vcmd; - memset(&vcmd, 0, sizeof(vcmd)); /* Copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */ vcmd.param1 = cmd_mavlink.param1; - vcmd.param2 = cmd_mavlink.param2; - vcmd.param3 = cmd_mavlink.param3; - vcmd.param4 = cmd_mavlink.param4; - vcmd.param5 = cmd_mavlink.param5; - vcmd.param6 = cmd_mavlink.param6; - vcmd.param7 = cmd_mavlink.param7; - // XXX do proper translation vcmd.command = (enum VEHICLE_CMD)cmd_mavlink.command; - vcmd.target_system = cmd_mavlink.target_system; - vcmd.target_component = cmd_mavlink.target_component; - vcmd.source_system = msg->sysid; - vcmd.source_component = msg->compid; - vcmd.confirmation = cmd_mavlink.confirmation; if (_cmd_pub < 0) { @@ -346,34 +331,22 @@ MavlinkReceiver::handle_message_command_int(mavlink_message_t *msg) } struct vehicle_command_s vcmd; - memset(&vcmd, 0, sizeof(vcmd)); /* Copy the content of mavlink_command_int_t cmd_mavlink into command_t cmd */ vcmd.param1 = cmd_mavlink.param1; - vcmd.param2 = cmd_mavlink.param2; - vcmd.param3 = cmd_mavlink.param3; - vcmd.param4 = cmd_mavlink.param4; - /* these are coordinates as 1e7 scaled integers to work around the 32 bit floating point limits */ vcmd.param5 = ((double)cmd_mavlink.x) / 1e7; - vcmd.param6 = ((double)cmd_mavlink.y) / 1e7; - vcmd.param7 = cmd_mavlink.z; - // XXX do proper translation vcmd.command = (enum VEHICLE_CMD)cmd_mavlink.command; - vcmd.target_system = cmd_mavlink.target_system; - vcmd.target_component = cmd_mavlink.target_component; - vcmd.source_system = msg->sysid; - vcmd.source_component = msg->compid; if (_cmd_pub < 0) { @@ -461,7 +434,6 @@ MavlinkReceiver::handle_message_hil_optical_flow(mavlink_message_t *msg) if (_range_pub < 0) { _range_pub = orb_advertise(ORB_ID(sensor_range_finder), &r); - } else { orb_publish(ORB_ID(sensor_range_finder), _range_pub, &r); } @@ -537,33 +509,28 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t /* Only accept messages which are intended for this system */ if ((mavlink_system.sysid == set_position_target_local_ned.target_system || - set_position_target_local_ned.target_system == 0) && - (mavlink_system.compid == set_position_target_local_ned.target_component || - set_position_target_local_ned.target_component == 0)) { + set_position_target_local_ned.target_system == 0) && + (mavlink_system.compid == set_position_target_local_ned.target_component || + set_position_target_local_ned.target_component == 0)) { /* convert mavlink type (local, NED) to uORB offboard control struct */ switch (set_position_target_local_ned.coordinate_frame) { - case MAV_FRAME_LOCAL_NED: - offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED; - break; - - case MAV_FRAME_LOCAL_OFFSET_NED: - offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_OFFSET_NED; - break; - - case MAV_FRAME_BODY_NED: - offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_BODY_NED; - break; - - case MAV_FRAME_BODY_OFFSET_NED: - offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_BODY_OFFSET_NED; - break; - - default: - /* invalid setpoint, avoid publishing */ - return; + case MAV_FRAME_LOCAL_NED: + offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED; + break; + case MAV_FRAME_LOCAL_OFFSET_NED: + offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_OFFSET_NED; + break; + case MAV_FRAME_BODY_NED: + offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_BODY_NED; + break; + case MAV_FRAME_BODY_OFFSET_NED: + offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_BODY_OFFSET_NED; + break; + default: + /* invalid setpoint, avoid publishing */ + return; } - offboard_control_sp.position[0] = set_position_target_local_ned.x; offboard_control_sp.position[1] = set_position_target_local_ned.y; offboard_control_sp.position[2] = set_position_target_local_ned.z; @@ -585,19 +552,17 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t /* set ignore flags */ for (int i = 0; i < 9; i++) { offboard_control_sp.ignore &= ~(1 << i); - offboard_control_sp.ignore |= (set_position_target_local_ned.type_mask & (1 << i)); + offboard_control_sp.ignore |= (set_position_target_local_ned.type_mask & (1 << i)); } offboard_control_sp.ignore &= ~(1 << OFB_IGN_BIT_YAW); - if (set_position_target_local_ned.type_mask & (1 << 10)) { - offboard_control_sp.ignore |= (1 << OFB_IGN_BIT_YAW); + offboard_control_sp.ignore |= (1 << OFB_IGN_BIT_YAW); } offboard_control_sp.ignore &= ~(1 << OFB_IGN_BIT_YAWRATE); - if (set_position_target_local_ned.type_mask & (1 << 11)) { - offboard_control_sp.ignore |= (1 << OFB_IGN_BIT_YAWRATE); + offboard_control_sp.ignore |= (1 << OFB_IGN_BIT_YAWRATE); } offboard_control_sp.timestamp = hrt_absolute_time(); @@ -614,30 +579,25 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t if (_mavlink->get_forward_externalsp()) { bool updated; orb_check(_control_mode_sub, &updated); - if (updated) { orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode); } - if (_control_mode.flag_control_offboard_enabled) { if (offboard_control_sp.isForceSetpoint && - offboard_control_sp_ignore_position_all(offboard_control_sp) && - offboard_control_sp_ignore_velocity_all(offboard_control_sp)) { + offboard_control_sp_ignore_position_all(offboard_control_sp) && + offboard_control_sp_ignore_velocity_all(offboard_control_sp)) { /* The offboard setpoint is a force setpoint only, directly writing to the force * setpoint topic and not publishing the setpoint triplet topic */ struct vehicle_force_setpoint_s force_sp; force_sp.x = offboard_control_sp.acceleration[0]; force_sp.y = offboard_control_sp.acceleration[1]; force_sp.z = offboard_control_sp.acceleration[2]; - //XXX: yaw if (_force_sp_pub < 0) { _force_sp_pub = orb_advertise(ORB_ID(vehicle_force_setpoint), &force_sp); - } else { orb_publish(ORB_ID(vehicle_force_setpoint), _force_sp_pub, &force_sp); } - } else { /* It's not a pure force setpoint: publish to setpoint triplet topic */ struct position_setpoint_triplet_s pos_sp_triplet; @@ -649,12 +609,11 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t /* set the local pos values if the setpoint type is 'local pos' and none * of the local pos fields is set to 'ignore' */ if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED && - !offboard_control_sp_ignore_position_some(offboard_control_sp)) { - pos_sp_triplet.current.position_valid = true; - pos_sp_triplet.current.x = offboard_control_sp.position[0]; - pos_sp_triplet.current.y = offboard_control_sp.position[1]; - pos_sp_triplet.current.z = offboard_control_sp.position[2]; - + !offboard_control_sp_ignore_position_some(offboard_control_sp)) { + pos_sp_triplet.current.position_valid = true; + pos_sp_triplet.current.x = offboard_control_sp.position[0]; + pos_sp_triplet.current.y = offboard_control_sp.position[1]; + pos_sp_triplet.current.z = offboard_control_sp.position[2]; } else { pos_sp_triplet.current.position_valid = false; } @@ -662,12 +621,11 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t /* set the local vel values if the setpoint type is 'local pos' and none * of the local vel fields is set to 'ignore' */ if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED && - !offboard_control_sp_ignore_velocity_some(offboard_control_sp)) { + !offboard_control_sp_ignore_velocity_some(offboard_control_sp)) { pos_sp_triplet.current.velocity_valid = true; pos_sp_triplet.current.vx = offboard_control_sp.velocity[0]; pos_sp_triplet.current.vy = offboard_control_sp.velocity[1]; pos_sp_triplet.current.vz = offboard_control_sp.velocity[2]; - } else { pos_sp_triplet.current.velocity_valid = false; } @@ -675,13 +633,13 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t /* set the local acceleration values if the setpoint type is 'local pos' and none * of the accelerations fields is set to 'ignore' */ if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED && - !offboard_control_sp_ignore_acceleration_some(offboard_control_sp)) { + !offboard_control_sp_ignore_acceleration_some(offboard_control_sp)) { pos_sp_triplet.current.acceleration_valid = true; pos_sp_triplet.current.a_x = offboard_control_sp.acceleration[0]; pos_sp_triplet.current.a_y = offboard_control_sp.acceleration[1]; pos_sp_triplet.current.a_z = offboard_control_sp.acceleration[2]; pos_sp_triplet.current.acceleration_is_force = - offboard_control_sp.isForceSetpoint; + offboard_control_sp.isForceSetpoint; } else { pos_sp_triplet.current.acceleration_valid = false; @@ -690,7 +648,7 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t /* set the yaw sp value if the setpoint type is 'local pos' and the yaw * field is not set to 'ignore' */ if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED && - !offboard_control_sp_ignore_yaw(offboard_control_sp)) { + !offboard_control_sp_ignore_yaw(offboard_control_sp)) { pos_sp_triplet.current.yaw_valid = true; pos_sp_triplet.current.yaw = offboard_control_sp.yaw; @@ -701,24 +659,22 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t /* set the yawrate sp value if the setpoint type is 'local pos' and the yawrate * field is not set to 'ignore' */ if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED && - !offboard_control_sp_ignore_yawrate(offboard_control_sp)) { + !offboard_control_sp_ignore_yawrate(offboard_control_sp)) { pos_sp_triplet.current.yawspeed_valid = true; pos_sp_triplet.current.yawspeed = offboard_control_sp.yaw_rate; } else { pos_sp_triplet.current.yawspeed_valid = false; } - //XXX handle global pos setpoints (different MAV frames) if (_pos_sp_triplet_pub < 0) { _pos_sp_triplet_pub = orb_advertise(ORB_ID(position_setpoint_triplet), - &pos_sp_triplet); - + &pos_sp_triplet); } else { orb_publish(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_pub, - &pos_sp_triplet); + &pos_sp_triplet); } } @@ -779,13 +735,12 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) /* Only accept messages which are intended for this system */ if ((mavlink_system.sysid == set_attitude_target.target_system || - set_attitude_target.target_system == 0) && - (mavlink_system.compid == set_attitude_target.target_component || - set_attitude_target.target_component == 0)) { + set_attitude_target.target_system == 0) && + (mavlink_system.compid == set_attitude_target.target_component || + set_attitude_target.target_component == 0)) { for (int i = 0; i < 4; i++) { offboard_control_sp.attitude[i] = set_attitude_target.q[i]; } - offboard_control_sp.attitude_rate[0] = set_attitude_target.body_roll_rate; offboard_control_sp.attitude_rate[1] = set_attitude_target.body_pitch_rate; offboard_control_sp.attitude_rate[2] = set_attitude_target.body_yaw_rate; @@ -793,9 +748,8 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) /* set correct ignore flags for body rate fields: copy from mavlink message */ for (int i = 0; i < 3; i++) { offboard_control_sp.ignore &= ~(1 << (i + OFB_IGN_BIT_BODYRATE_X)); - offboard_control_sp.ignore |= (set_attitude_target.type_mask & (1 << i)) << OFB_IGN_BIT_BODYRATE_X; + offboard_control_sp.ignore |= (set_attitude_target.type_mask & (1 << i)) << OFB_IGN_BIT_BODYRATE_X; } - /* set correct ignore flags for thrust field: copy from mavlink message */ offboard_control_sp.ignore &= ~(1 << OFB_IGN_BIT_THRUST); offboard_control_sp.ignore |= ((set_attitude_target.type_mask & (1 << 6)) << OFB_IGN_BIT_THRUST); @@ -805,7 +759,7 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) offboard_control_sp.timestamp = hrt_absolute_time(); - offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE; //XXX handle rate control mode + offboard_control_sp.mode =OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE; //XXX handle rate control mode if (_offboard_control_sp_pub < 0) { _offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp); @@ -819,7 +773,6 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) if (_mavlink->get_forward_externalsp()) { bool updated; orb_check(_control_mode_sub, &updated); - if (updated) { orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode); } @@ -828,20 +781,18 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) /* Publish attitude setpoint if attitude and thrust ignore bits are not set */ if (!(offboard_control_sp_ignore_attitude(offboard_control_sp) || - offboard_control_sp_ignore_thrust(offboard_control_sp))) { + offboard_control_sp_ignore_thrust(offboard_control_sp))) { struct vehicle_attitude_setpoint_s att_sp; att_sp.timestamp = hrt_absolute_time(); mavlink_quaternion_to_euler(set_attitude_target.q, - &att_sp.roll_body, &att_sp.pitch_body, &att_sp.yaw_body); + &att_sp.roll_body, &att_sp.pitch_body, &att_sp.yaw_body); mavlink_quaternion_to_dcm(set_attitude_target.q, att_sp.R_body); att_sp.R_valid = true; att_sp.thrust = set_attitude_target.thrust; memcpy(att_sp.q_d, set_attitude_target.q, sizeof(att_sp.q_d)); att_sp.q_d_valid = true; - if (_att_sp_pub < 0) { _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); - } else { orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &att_sp); } @@ -850,7 +801,7 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) /* Publish attitude rate setpoint if bodyrate and thrust ignore bits are not set */ ///XXX add support for ignoring individual axes if (!(offboard_control_sp_ignore_bodyrates_some(offboard_control_sp) || - offboard_control_sp_ignore_thrust(offboard_control_sp))) { + offboard_control_sp_ignore_thrust(offboard_control_sp))) { struct vehicle_rates_setpoint_s rates_sp; rates_sp.timestamp = hrt_absolute_time(); rates_sp.roll = set_attitude_target.body_roll_rate; @@ -860,7 +811,6 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) if (_att_sp_pub < 0) { _rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp); - } else { orb_publish(ORB_ID(vehicle_rates_setpoint), _rates_sp_pub, &rates_sp); } @@ -919,8 +869,7 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg) manual.r = man.r / 1000.0f; manual.z = man.z / 1000.0f; - warnx("pitch: %.2f, roll: %.2f, yaw: %.2f, throttle: %.2f", (double)manual.x, (double)manual.y, (double)manual.r, - (double)manual.z); + warnx("pitch: %.2f, roll: %.2f, yaw: %.2f, throttle: %.2f", (double)manual.x, (double)manual.y, (double)manual.r, (double)manual.z); if (_manual_pub < 0) { _manual_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual); From b4a611e95dc9aa4fd14c9f649bb86538908b22fc Mon Sep 17 00:00:00 2001 From: Mohammed Kabir Date: Tue, 16 Dec 2014 14:11:58 +0530 Subject: [PATCH 163/652] Remove debugging noise --- ROMFS/px4fmu_common/init.d/rc.usb | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.usb b/ROMFS/px4fmu_common/init.d/rc.usb index 12aca0687b..ee040a706d 100644 --- a/ROMFS/px4fmu_common/init.d/rc.usb +++ b/ROMFS/px4fmu_common/init.d/rc.usb @@ -3,7 +3,7 @@ # USB MAVLink start # -mavlink start -r 20000 -d /dev/ttyACM0 -x -m onboard +mavlink start -r 20000 -d /dev/ttyACM0 -x # Enable a number of interesting streams we want via USB mavlink stream -d /dev/ttyACM0 -s PARAM_VALUE -r 200 usleep 100000 From 5c6155c94d3a27cdc426f7d3e18a679f0bc589e7 Mon Sep 17 00:00:00 2001 From: "M.H.Kabir" Date: Tue, 16 Dec 2014 14:22:35 +0530 Subject: [PATCH 164/652] minor fix --- src/modules/mavlink/mavlink_receiver.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 72c0d47bd5..88d12e7750 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -121,7 +121,9 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _old_timestamp(0), _hil_local_proj_inited(0), _hil_local_alt0(0.0f), - _hil_local_proj_ref{} + _hil_local_proj_ref{}, + _time_offset_avg_alpha(0.75), + _time_offset(0) { // make sure the FTP server is started From 0e41624f7902b13bbf830742481174e08e0f97c4 Mon Sep 17 00:00:00 2001 From: "M.H.Kabir" Date: Tue, 16 Dec 2014 20:39:28 +0530 Subject: [PATCH 165/652] removed exponential filter. simple weighted average works fine. --- src/modules/mavlink/mavlink_receiver.cpp | 19 ++++--------------- src/modules/mavlink/mavlink_receiver.h | 6 ------ 2 files changed, 4 insertions(+), 21 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 88d12e7750..cd385ffac9 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -122,7 +122,6 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _hil_local_proj_inited(0), _hil_local_alt0(0.0f), _hil_local_proj_ref{}, - _time_offset_avg_alpha(0.75), _time_offset(0) { @@ -971,15 +970,15 @@ MavlinkReceiver::handle_message_timesync(mavlink_message_t *msg) } else if (tsync.tc1 > 0) { - int64_t offset_ns = ((tsync.ts1 + now_ns) - (tsync.tc1 * 2)) / 2; + int64_t offset_ns = (9*_time_offset + (tsync.ts1 + now_ns - tsync.tc1*2)/2 )/10; // average offset int64_t dt = _time_offset - offset_ns; if (dt > 10000000 || dt < -1000000) { // 10 millisecond skew - _time_offset = offset_ns; - warnx("[timesync] Timesync offset is off. Hard-setting offset"); + _time_offset = (tsync.ts1 + now_ns - tsync.tc1*2)/2; + warnx("[timesync] Companion clock offset is skewed. Hard-setting offset"); } else { - average_time_offset(offset_ns); + _time_offset = offset_ns; } } @@ -1457,16 +1456,6 @@ uint64_t MavlinkReceiver::to_hrt(uint64_t usec) return usec - (_time_offset / 1000) ; } -void MavlinkReceiver::average_time_offset(uint64_t offset_ns) -{ - /* alpha = 0.75 fixed for now. The closer alpha is to 1.0, - * the faster the moving average updates in response to - * new offset samples. - */ - - _time_offset = (_time_offset_avg_alpha * offset_ns) + (1.0 - _time_offset_avg_alpha) * _time_offset; -} - void *MavlinkReceiver::start_helper(void *context) { MavlinkReceiver *rcv = new MavlinkReceiver((Mavlink *)context); diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 71273786de..7c79f661bd 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -137,11 +137,6 @@ private: */ uint64_t to_hrt(uint64_t nsec); - /** - * Exponential moving average filter to smooth time offset - */ - void average_time_offset(uint64_t offset_ns); - mavlink_status_t status; struct vehicle_local_position_s hil_local_pos; struct vehicle_control_mode_s _control_mode; @@ -176,7 +171,6 @@ private: bool _hil_local_proj_inited; float _hil_local_alt0; struct map_projection_reference_s _hil_local_proj_ref; - double _time_offset_avg_alpha; uint64_t _time_offset; /* do not allow copying this class */ From 4a42f6ca6a95c9b05ed2c1e66cc251ea0e001c59 Mon Sep 17 00:00:00 2001 From: "M.H.Kabir" Date: Wed, 17 Dec 2014 12:12:09 +0530 Subject: [PATCH 166/652] Restore EMA. Works better for low rates --- src/modules/mavlink/mavlink_receiver.cpp | 15 ++++++++++++++- src/modules/mavlink/mavlink_receiver.h | 7 ++++++- 2 files changed, 20 insertions(+), 2 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index cd385ffac9..5aa41e5d8e 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -122,6 +122,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _hil_local_proj_inited(0), _hil_local_alt0(0.0f), _hil_local_proj_ref{}, + _time_offset_avg_alpha(0.6), _time_offset(0) { @@ -978,7 +979,7 @@ MavlinkReceiver::handle_message_timesync(mavlink_message_t *msg) warnx("[timesync] Companion clock offset is skewed. Hard-setting offset"); } else { - _time_offset = offset_ns; + smooth_time_offset(offset_ns); } } @@ -1456,6 +1457,18 @@ uint64_t MavlinkReceiver::to_hrt(uint64_t usec) return usec - (_time_offset / 1000) ; } + +void MavlinkReceiver::smooth_time_offset(uint64_t offset_ns) +{ + /* alpha = 0.75 fixed for now. The closer alpha is to 1.0, + * the faster the moving average updates in response to + * new offset samples. + */ + + _time_offset = (_time_offset_avg_alpha * offset_ns) + (1.0 - _time_offset_avg_alpha) * _time_offset; +} + + void *MavlinkReceiver::start_helper(void *context) { MavlinkReceiver *rcv = new MavlinkReceiver((Mavlink *)context); diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 7c79f661bd..a677e75cd9 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -136,7 +136,11 @@ private: * Convert remote nsec timestamp to local hrt time (usec) */ uint64_t to_hrt(uint64_t nsec); - + /** + * Exponential moving average filter to smooth time offset + */ + void smooth_time_offset(uint64_t offset_ns); + mavlink_status_t status; struct vehicle_local_position_s hil_local_pos; struct vehicle_control_mode_s _control_mode; @@ -171,6 +175,7 @@ private: bool _hil_local_proj_inited; float _hil_local_alt0; struct map_projection_reference_s _hil_local_proj_ref; + double _time_offset_avg_alpha; uint64_t _time_offset; /* do not allow copying this class */ From c59f20706f93908ff9137aa315e8c77cb470e298 Mon Sep 17 00:00:00 2001 From: "M.H.Kabir" Date: Wed, 17 Dec 2014 12:16:00 +0530 Subject: [PATCH 167/652] Add use example --- src/modules/mavlink/mavlink_receiver.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 5aa41e5d8e..3a7fcd0e20 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -699,7 +699,7 @@ MavlinkReceiver::handle_message_vision_position_estimate(mavlink_message_t *msg) // Use the component ID to identify the vision sensor vision_position.id = msg->compid; - vision_position.timestamp_boot = hrt_absolute_time(); + vision_position.timestamp_boot = to_hrt(pos.usec); // Synced time vision_position.timestamp_computer = pos.usec; vision_position.x = pos.x; vision_position.y = pos.y; From a65151d1c05dfae8ea2b7361340a04f3bb02d1d2 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Wed, 17 Dec 2014 14:50:11 +0100 Subject: [PATCH 168/652] 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 169/652] 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 170/652] 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 171/652] 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 172/652] 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 173/652] 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 174/652] 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 e18065081af0dbbbf50aaba30cc0b14621c7b29b Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 18 Dec 2014 14:46:31 +0100 Subject: [PATCH 175/652] is_vtol flag in vehicle_status Getting rid of the autostart based checks if the system is a vtol Fixes #1503 --- src/modules/commander/commander.cpp | 18 ++++++----- .../fw_att_control/fw_att_control_main.cpp | 32 ++++++++++--------- .../mc_att_control/mc_att_control_main.cpp | 19 +++++------ src/modules/uORB/topics/vehicle_status.h | 4 ++- 4 files changed, 40 insertions(+), 33 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 744323c011..8d2e62e32c 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1092,6 +1092,11 @@ int commander_thread_main(int argc, char *argv[]) status.is_rotary_wing = false; } + /* set vehicle_status.is_vtol flag */ + status.is_vtol = (status.system_type == VEHICLE_TYPE_VTOL_DUOROTOR) || + (status.system_type == VEHICLE_TYPE_VTOL_QUADROTOR); + + /* check and update system / component ID */ param_get(_param_system_id, &(status.system_id)); param_get(_param_component_id, &(status.component_id)); @@ -1113,6 +1118,8 @@ int commander_thread_main(int argc, char *argv[]) /* navigation parameters */ param_get(_param_takeoff_alt, &takeoff_alt); + + /* Safety parameters */ param_get(_param_enable_parachute, ¶chute_enabled); param_get(_param_enable_datalink_loss, &datalink_loss_enabled); param_get(_param_datalink_loss_timeout, &datalink_loss_timeout); @@ -1121,6 +1128,8 @@ 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); + + /* Autostart id */ param_get(_param_autostart_id, &autostart_id); } @@ -2217,14 +2226,7 @@ set_control_mode() { /* set vehicle_control_mode according to set_navigation_state */ control_mode.flag_armed = armed.armed; - /* TODO: check this */ - 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_external_manual_override_ok = !status.is_rotary_wing && !status.is_vtol; control_mode.flag_system_hil_enabled = status.hil_state == HIL_STATE_ON; control_mode.flag_control_offboard_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 8d18ae68cb..72d9a5452f 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -338,6 +338,9 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _actuators_0_pub(-1), _actuators_2_pub(-1), + _rates_sp_id(ORB_ID(vehicle_rates_setpoint)), + _actuators_id(ORB_ID(actuator_controls_0)), + /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "fw att control")), _nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control nonfinite input")), @@ -400,15 +403,6 @@ 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() @@ -600,6 +594,14 @@ FixedwingAttitudeControl::vehicle_status_poll() if (vehicle_status_updated) { orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status); + /* set correct uORB ID, depending on if vehicle is VTOL or not */ + if (_vehicle_status.is_vtol) { + _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); + } } } @@ -700,11 +702,11 @@ 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 !!! - + if (_vehicle_status.is_vtol) { + /* 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 @@ -849,7 +851,7 @@ FixedwingAttitudeControl::task_main() _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) 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 0702e63783..d864feb254 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -285,6 +285,8 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _att_sp_pub(-1), _v_rates_sp_pub(-1), _actuators_0_pub(-1), + _rates_sp_id(ORB_ID(vehicle_rates_setpoint)), + _actuators_id(ORB_ID(actuator_controls_0)), _actuators_0_circuit_breaker_enabled(false), @@ -348,15 +350,6 @@ 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() @@ -531,6 +524,14 @@ MulticopterAttitudeControl::vehicle_status_poll() if (vehicle_status_updated) { orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status); + /* set correct uORB ID, depending on if vehicle is VTOL or not */ + if (_vehicle_status.is_vtol) { + _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); + } } } diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 749d00d75b..b4d5c7b884 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -185,7 +185,9 @@ struct vehicle_status_s { int32_t system_id; /**< system id, inspired by MAVLink's system ID field */ int32_t component_id; /**< subsystem / component id, inspired by MAVLink's component ID field */ - bool is_rotary_wing; + bool is_rotary_wing; /**< True if system is in rotary wing configuration, so for a VTOL + this is only true while flying as a multicopter */ + bool is_vtol; /**< True if the system is VTOL capable */ bool condition_battery_voltage_valid; bool condition_system_in_air_restore; /**< true if we can restore in mid air */ From 32675ff1c150529bbb662fde87c0940ee90acd6e Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 18 Dec 2014 14:59:52 +0100 Subject: [PATCH 176/652] fix flag_external_manual_override_ok The flag should not depend on the vtol state anymore. The intended functionality of this is by now handled via the is_rotary_wing flag --- src/modules/commander/commander.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 8d2e62e32c..5779183abf 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1096,7 +1096,6 @@ int commander_thread_main(int argc, char *argv[]) status.is_vtol = (status.system_type == VEHICLE_TYPE_VTOL_DUOROTOR) || (status.system_type == VEHICLE_TYPE_VTOL_QUADROTOR); - /* check and update system / component ID */ param_get(_param_system_id, &(status.system_id)); param_get(_param_component_id, &(status.component_id)); @@ -2226,7 +2225,7 @@ set_control_mode() { /* set vehicle_control_mode according to set_navigation_state */ control_mode.flag_armed = armed.armed; - control_mode.flag_external_manual_override_ok = !status.is_rotary_wing && !status.is_vtol; + control_mode.flag_external_manual_override_ok = !status.is_rotary_wing; control_mode.flag_system_hil_enabled = status.hil_state == HIL_STATE_ON; control_mode.flag_control_offboard_enabled = false; From ecde80ad3596dd4ceae6be399467510e4a05e3d7 Mon Sep 17 00:00:00 2001 From: The Gitter Badger Date: Thu, 18 Dec 2014 18:13:22 +0000 Subject: [PATCH 177/652] 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 178/652] 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 4d0d6f09ce9aef8875e33ee80aefcc8ed00d4769 Mon Sep 17 00:00:00 2001 From: Rowland O'Flaherty Date: Thu, 18 Dec 2014 16:56:28 -0800 Subject: [PATCH 179/652] Fixed loop limit errors in Matrix.h In few of the overloaded operators the loop limits (i.e. M and N) were swapped. --- src/lib/mathlib/math/Matrix.hpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/lib/mathlib/math/Matrix.hpp b/src/lib/mathlib/math/Matrix.hpp index ca931e2daf..ac1f1538f7 100644 --- a/src/lib/mathlib/math/Matrix.hpp +++ b/src/lib/mathlib/math/Matrix.hpp @@ -180,8 +180,8 @@ public: Matrix operator -(void) const { Matrix res; - for (unsigned int i = 0; i < N; i++) - for (unsigned int j = 0; j < M; j++) + for (unsigned int i = 0; i < M; i++) + for (unsigned int j = 0; j < N; j++) res.data[i][j] = -data[i][j]; return res; @@ -193,16 +193,16 @@ public: Matrix operator +(const Matrix &m) const { Matrix res; - for (unsigned int i = 0; i < N; i++) - for (unsigned int j = 0; j < M; j++) + for (unsigned int i = 0; i < M; i++) + for (unsigned int j = 0; j < N; j++) res.data[i][j] = data[i][j] + m.data[i][j]; return res; } Matrix &operator +=(const Matrix &m) { - for (unsigned int i = 0; i < N; i++) - for (unsigned int j = 0; j < M; j++) + for (unsigned int i = 0; i < M; i++) + for (unsigned int j = 0; j < N; j++) data[i][j] += m.data[i][j]; return *static_cast*>(this); @@ -222,8 +222,8 @@ public: } Matrix &operator -=(const Matrix &m) { - for (unsigned int i = 0; i < N; i++) - for (unsigned int j = 0; j < M; j++) + for (unsigned int i = 0; i < M; i++) + for (unsigned int j = 0; j < N; j++) data[i][j] -= m.data[i][j]; return *static_cast*>(this); From 2a09473e5fc73bf2f59f0d27b0b5c38b2c91812f Mon Sep 17 00:00:00 2001 From: Ban Siesta Date: Fri, 19 Dec 2014 22:26:31 +0000 Subject: [PATCH 180/652] 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 181/652] 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 182/652] 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 183/652] 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 184/652] 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 185/652] 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 186/652] 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 187/652] 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 188/652] 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 189/652] 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 190/652] 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 191/652] 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 192/652] 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 193/652] 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 194/652] 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 195/652] 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 196/652] 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 197/652] 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 198/652] 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 199/652] 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 200/652] 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 201/652] 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 202/652] 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 203/652] 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 204/652] 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 205/652] 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 206/652] 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 207/652] 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 208/652] 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 209/652] 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 210/652] 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 211/652] 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 212/652] 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 213/652] 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 214/652] 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 215/652] 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 216/652] 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 217/652] 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 218/652] 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 219/652] 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 220/652] 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 221/652] 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 222/652] 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 223/652] 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 224/652] 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 225/652] 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 226/652] 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 227/652] 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 228/652] 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 229/652] 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 230/652] 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 231/652] 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 232/652] 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 6f83f77bf422c4dd66f63aa2ac83e82dc2283b94 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 22 Dec 2014 15:18:09 +0100 Subject: [PATCH 233/652] fw att ctl: remove autostart id param --- src/modules/fw_att_control/fw_att_control_main.cpp | 7 ------- 1 file changed, 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 72d9a5452f..598408c9fa 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -194,8 +194,6 @@ 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 { @@ -236,7 +234,6 @@ private: 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 */ @@ -399,8 +396,6 @@ 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(); } @@ -477,8 +472,6 @@ 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); _pitch_ctrl.set_k_p(_parameters.p_p); From 6dd98cb3118d7d08bbf01b58737dcdc53b429e1b Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 22 Dec 2014 15:18:20 +0100 Subject: [PATCH 234/652] mc att ctl: remove autostart id param --- src/modules/mc_att_control/mc_att_control_main.cpp | 8 -------- 1 file changed, 8 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 d864feb254..25b085b7b2 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -175,7 +175,6 @@ private: 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 { @@ -191,7 +190,6 @@ private: float man_yaw_max; math::Vector<3> acro_rate_max; /**< max attitude rates in acro mode */ - param_t autostart_id; } _params; /** @@ -315,8 +313,6 @@ 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(); @@ -346,8 +342,6 @@ 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(); } @@ -433,8 +427,6 @@ 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; } From d511e39ea7a3f1e0cb672b14598e18a7df18156a Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 22 Dec 2014 17:09:43 -0500 Subject: [PATCH 235/652] 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 236/652] 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 237/652] 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 238/652] 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 239/652] 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 240/652] 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 d8eefa30538331fde7e5ce79fef4e04dc62664bc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 23 Dec 2014 21:50:16 +0100 Subject: [PATCH 241/652] VTOL: Do not allow manual override --- 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 5779183abf..de3e69373f 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -2225,7 +2225,7 @@ set_control_mode() { /* set vehicle_control_mode according to set_navigation_state */ control_mode.flag_armed = armed.armed; - control_mode.flag_external_manual_override_ok = !status.is_rotary_wing; + control_mode.flag_external_manual_override_ok = (!status.is_rotary_wing && !status.is_vtol); control_mode.flag_system_hil_enabled = status.hil_state == HIL_STATE_ON; control_mode.flag_control_offboard_enabled = false; From 1240912f1c50d2f2f9993e5d32a5ebb46626accb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 24 Dec 2014 09:26:21 +0100 Subject: [PATCH 242/652] FMUv1 / FMUv2: Up number of file descriptors slightly - no known issues, but some tasks are getting close. --- nuttx-configs/px4fmu-v1/nsh/defconfig | 2 +- nuttx-configs/px4fmu-v2/nsh/defconfig | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig index edf49b26e8..fade1f0c69 100644 --- a/nuttx-configs/px4fmu-v1/nsh/defconfig +++ b/nuttx-configs/px4fmu-v1/nsh/defconfig @@ -404,7 +404,7 @@ CONFIG_SIG_SIGWORK=4 CONFIG_MAX_TASKS=32 CONFIG_MAX_TASK_ARGS=10 CONFIG_NPTHREAD_KEYS=4 -CONFIG_NFILE_DESCRIPTORS=38 +CONFIG_NFILE_DESCRIPTORS=42 CONFIG_NFILE_STREAMS=8 CONFIG_NAME_MAX=32 CONFIG_PREALLOC_MQ_MSGS=4 diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index e31f40cd2a..90dde62946 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -438,7 +438,7 @@ CONFIG_SIG_SIGWORK=4 CONFIG_MAX_TASKS=32 CONFIG_MAX_TASK_ARGS=10 CONFIG_NPTHREAD_KEYS=4 -CONFIG_NFILE_DESCRIPTORS=38 +CONFIG_NFILE_DESCRIPTORS=42 CONFIG_NFILE_STREAMS=8 CONFIG_NAME_MAX=32 CONFIG_PREALLOC_MQ_MSGS=4 From d64c223416858b0024d70f871f46356434dc8145 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 24 Dec 2014 09:44:52 +0100 Subject: [PATCH 243/652] 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 244/652] 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 245/652] 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 246/652] 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 247/652] 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 248/652] 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 249/652] 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); From ed493918f70560067e213847b5546166398d494e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 26 Dec 2014 17:36:10 +0100 Subject: [PATCH 250/652] Attitude estimator EKF: fix warnings --- .../attitude_estimator_ekf/attitude_estimator_ekf_main.cpp | 4 ++-- src/modules/attitude_estimator_ekf/module.mk | 2 +- 2 files changed, 3 insertions(+), 3 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 1942fdc629..702894d600 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -462,11 +462,11 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds if (vision.timestamp_boot > 0 && (hrt_elapsed_time(&vision.timestamp_boot) < 500000)) { math::Quaternion q(vision.q); - math::Matrix<3, 3> R = q.to_dcm(); + math::Matrix<3, 3> Rvis = q.to_dcm(); math::Vector<3> v(1.0f, 0.0f, 0.4f); - math::Vector<3> vn = R * v; + math::Vector<3> vn = Rvis * v; z_k[6] = vn(0); z_k[7] = vn(1); diff --git a/src/modules/attitude_estimator_ekf/module.mk b/src/modules/attitude_estimator_ekf/module.mk index 3a3e1cc698..1158536e1d 100644 --- a/src/modules/attitude_estimator_ekf/module.mk +++ b/src/modules/attitude_estimator_ekf/module.mk @@ -45,4 +45,4 @@ MODULE_STACKSIZE = 1200 EXTRACFLAGS = -Wno-float-equal -Wframe-larger-than=3600 -EXTRACXXFLAGS = -Wframe-larger-than=2200 +EXTRACXXFLAGS = -Wframe-larger-than=2400 From 2f6767480b5c88c16ec181b47d194bd9dabdc959 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 26 Dec 2014 17:36:34 +0100 Subject: [PATCH 251/652] EKF att/pos estimator: Fix warnings (cross-checked stack size already) --- src/modules/ekf_att_pos_estimator/module.mk | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/ekf_att_pos_estimator/module.mk b/src/modules/ekf_att_pos_estimator/module.mk index 4311fb3f9d..843dde9789 100644 --- a/src/modules/ekf_att_pos_estimator/module.mk +++ b/src/modules/ekf_att_pos_estimator/module.mk @@ -42,5 +42,5 @@ SRCS = ekf_att_pos_estimator_main.cpp \ estimator_23states.cpp \ estimator_utilities.cpp -EXTRACXXFLAGS = -Weffc++ -Wframe-larger-than=3000 +EXTRACXXFLAGS = -Weffc++ -Wframe-larger-than=6100 From d04f1fde50b62938f08cc64f3d915f38917c46b7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 26 Dec 2014 17:37:36 +0100 Subject: [PATCH 252/652] Suppress nothing to do messages from make for clean targets --- Makefile | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/Makefile b/Makefile index 0f68e7b4be..905a60a3b6 100644 --- a/Makefile +++ b/Makefile @@ -245,14 +245,16 @@ tests: # .PHONY: clean clean: - $(Q) $(RMDIR) $(BUILD_DIR)*.build > /dev/null - $(Q) $(REMOVE) $(IMAGE_DIR)*.px4 > /dev/null + @echo > /dev/null + $(Q) $(RMDIR) $(BUILD_DIR)*.build + $(Q) $(REMOVE) $(IMAGE_DIR)*.px4 .PHONY: distclean distclean: clean - $(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 + @echo > /dev/null + $(Q) $(REMOVE) $(ARCHIVE_DIR)*.export + $(Q) $(MAKE) -C $(NUTTX_SRC) -r $(MQUIET) distclean + $(Q) (cd $(NUTTX_SRC)/configs && $(FIND) . -maxdepth 1 -type l -delete) # # Print some help text From f1c5fcd8da98e16588ac17e4da9ae8ef69fb461b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 26 Dec 2014 17:38:11 +0100 Subject: [PATCH 253/652] NuttX configs / build options: Do not warn about sign compare, as there are a bunch of non-standard signedness assumptions --- nuttx-configs/aerocore/nsh/Make.defs | 1 + nuttx-configs/px4fmu-v1/nsh/Make.defs | 1 + nuttx-configs/px4fmu-v2/nsh/Make.defs | 1 + nuttx-configs/px4io-v1/nsh/Make.defs | 1 + nuttx-configs/px4io-v2/nsh/Make.defs | 1 + 5 files changed, 5 insertions(+) diff --git a/nuttx-configs/aerocore/nsh/Make.defs b/nuttx-configs/aerocore/nsh/Make.defs index c7a1b71bbd..c1f5a8ac42 100644 --- a/nuttx-configs/aerocore/nsh/Make.defs +++ b/nuttx-configs/aerocore/nsh/Make.defs @@ -119,6 +119,7 @@ endif ARCHCFLAGS = -std=gnu99 ARCHCXXFLAGS = -fno-exceptions -fno-rtti -std=gnu++0x ARCHWARNINGS = -Wall \ + -Wno-sign-compare \ -Wextra \ -Wdouble-promotion \ -Wshadow \ diff --git a/nuttx-configs/px4fmu-v1/nsh/Make.defs b/nuttx-configs/px4fmu-v1/nsh/Make.defs index f055cfddf8..5e28f2473d 100644 --- a/nuttx-configs/px4fmu-v1/nsh/Make.defs +++ b/nuttx-configs/px4fmu-v1/nsh/Make.defs @@ -119,6 +119,7 @@ endif ARCHCFLAGS = -std=gnu99 ARCHCXXFLAGS = -fno-exceptions -fno-rtti -std=gnu++0x ARCHWARNINGS = -Wall \ + -Wno-sign-compare \ -Wextra \ -Wdouble-promotion \ -Wshadow \ diff --git a/nuttx-configs/px4fmu-v2/nsh/Make.defs b/nuttx-configs/px4fmu-v2/nsh/Make.defs index f660deeca7..99f3b3140d 100644 --- a/nuttx-configs/px4fmu-v2/nsh/Make.defs +++ b/nuttx-configs/px4fmu-v2/nsh/Make.defs @@ -119,6 +119,7 @@ endif ARCHCFLAGS = -std=gnu99 ARCHCXXFLAGS = -fno-exceptions -fno-rtti -std=gnu++0x ARCHWARNINGS = -Wall \ + -Wno-sign-compare \ -Wextra \ -Wdouble-promotion \ -Wshadow \ diff --git a/nuttx-configs/px4io-v1/nsh/Make.defs b/nuttx-configs/px4io-v1/nsh/Make.defs index e96be6a1f2..b4f5577aed 100644 --- a/nuttx-configs/px4io-v1/nsh/Make.defs +++ b/nuttx-configs/px4io-v1/nsh/Make.defs @@ -108,6 +108,7 @@ endif ARCHCFLAGS = -std=gnu99 ARCHCXXFLAGS = -fno-exceptions -fno-rtti -std=gnu++0x ARCHWARNINGS = -Wall \ + -Wno-sign-compare \ -Wextra \ -Wdouble-promotion \ -Wshadow \ diff --git a/nuttx-configs/px4io-v2/nsh/Make.defs b/nuttx-configs/px4io-v2/nsh/Make.defs index 4a8df27388..51420eb23d 100644 --- a/nuttx-configs/px4io-v2/nsh/Make.defs +++ b/nuttx-configs/px4io-v2/nsh/Make.defs @@ -108,6 +108,7 @@ endif ARCHCFLAGS = -std=gnu99 ARCHCXXFLAGS = -fno-exceptions -fno-rtti -std=gnu++0x ARCHWARNINGS = -Wall \ + -Wno-sign-compare \ -Wextra \ -Wdouble-promotion \ -Wshadow \ From b097b13ba687412b97d2bcd005bc0f11be33de7b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 26 Dec 2014 18:01:52 +0100 Subject: [PATCH 254/652] Companion computer interface: Add SYS_COMPANION parameter. Setting it to 921600 enables the companion interface. --- ROMFS/px4fmu_common/init.d/rcS | 5 ++++- src/modules/systemlib/system_params.c | 14 ++++++++++++++ 2 files changed, 18 insertions(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index b0e0c26748..26b729aad1 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -434,7 +434,10 @@ then # if ver hwcmp PX4FMU_V2 then - mavlink start -d /dev/ttyS2 -b 230400 -m onboard + if param compare SYS_COMPANION 921600 + then + mavlink start -d /dev/ttyS2 -b 921600 -m onboard + fi fi # UAVCAN diff --git a/src/modules/systemlib/system_params.c b/src/modules/systemlib/system_params.c index 702e435ac9..a0988035c9 100644 --- a/src/modules/systemlib/system_params.c +++ b/src/modules/systemlib/system_params.c @@ -82,3 +82,17 @@ PARAM_DEFINE_INT32(SYS_USE_IO, 1); * @group System */ PARAM_DEFINE_INT32(SYS_RESTART_TYPE, 2); + +/** +* Companion computer interface +* +* Configures the baud rate of the companion computer interface. +* Set to zero to disable, set to 921600 to enable. +* CURRENTLY ONLY SUPPORTS 921600 BAUD! Use extras.txt for +* other baud rates. +* +* @min 0 +* @max 921600 +* @group System +*/ +PARAM_DEFINE_INT32(SYS_COMPANION, 0); From 3df44a27ec72bf29b7bff6d043384c164d743b0e Mon Sep 17 00:00:00 2001 From: Mohammed Kabir Date: Fri, 26 Dec 2014 22:55:27 +0530 Subject: [PATCH 255/652] Add streams to onboard mode --- src/modules/mavlink/mavlink_main.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 29b7ec7b74..5d22dab6a3 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1412,9 +1412,13 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("SYS_STATUS", 1.0f); configure_stream("ATTITUDE", 50.0f); configure_stream("GLOBAL_POSITION_INT", 50.0f); + configure_stream("LOCAL_POSITION_NED", 30.0f); configure_stream("CAMERA_CAPTURE", 2.0f); configure_stream("ATTITUDE_TARGET", 10.0f); configure_stream("POSITION_TARGET_GLOBAL_INT", 10.0f); + configure_stream("POSITION_TARGET_LOCAL_NED", 10.0f); + configure_stream("DISTANCE_SENSOR", 10.0f); + configure_stream("OPTICAL_FLOW_RAD", 10.0f); configure_stream("VFR_HUD", 10.0f); break; From da977bb39bf6157247cfd31a3da0f0f80b497bf0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 10 Oct 2014 00:04:39 +0200 Subject: [PATCH 256/652] Added HMC chip select support --- src/drivers/boards/px4fmu-v2/board_config.h | 2 ++ src/drivers/boards/px4fmu-v2/px4fmu_spi.c | 15 +++++++++++++++ 2 files changed, 17 insertions(+) diff --git a/src/drivers/boards/px4fmu-v2/board_config.h b/src/drivers/boards/px4fmu-v2/board_config.h index ee365e47c0..2fd8bc724a 100644 --- a/src/drivers/boards/px4fmu-v2/board_config.h +++ b/src/drivers/boards/px4fmu-v2/board_config.h @@ -105,6 +105,7 @@ __BEGIN_DECLS #define GPIO_SPI_CS_ACCEL_MAG (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN15) #define GPIO_SPI_CS_BARO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN7) #define GPIO_SPI_CS_FRAM (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN10) +#define GPIO_SPI_CS_HMC (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN1) #define GPIO_SPI_CS_MPU (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN2) #define GPIO_SPI_CS_EXT0 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN4) #define GPIO_SPI_CS_EXT1 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN14) @@ -119,6 +120,7 @@ __BEGIN_DECLS #define PX4_SPIDEV_ACCEL_MAG 2 #define PX4_SPIDEV_BARO 3 #define PX4_SPIDEV_MPU 4 +#define PX4_SPIDEV_HMC 5 /* External bus */ #define PX4_SPIDEV_EXT0 1 diff --git a/src/drivers/boards/px4fmu-v2/px4fmu_spi.c b/src/drivers/boards/px4fmu-v2/px4fmu_spi.c index 8c37d31a73..27f193513b 100644 --- a/src/drivers/boards/px4fmu-v2/px4fmu_spi.c +++ b/src/drivers/boards/px4fmu-v2/px4fmu_spi.c @@ -73,6 +73,7 @@ __EXPORT void weak_function stm32_spiinitialize(void) stm32_configgpio(GPIO_SPI_CS_GYRO); stm32_configgpio(GPIO_SPI_CS_ACCEL_MAG); stm32_configgpio(GPIO_SPI_CS_BARO); + stm32_configgpio(GPIO_SPI_CS_HMC); stm32_configgpio(GPIO_SPI_CS_MPU); /* De-activate all peripherals, @@ -82,6 +83,7 @@ __EXPORT void weak_function stm32_spiinitialize(void) stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1); stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); stm32_gpiowrite(GPIO_SPI_CS_BARO, 1); + stm32_gpiowrite(GPIO_SPI_CS_HMC, 1); stm32_gpiowrite(GPIO_SPI_CS_MPU, 1); stm32_configgpio(GPIO_EXTI_GYRO_DRDY); @@ -117,6 +119,7 @@ __EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, stm32_gpiowrite(GPIO_SPI_CS_GYRO, !selected); stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); stm32_gpiowrite(GPIO_SPI_CS_BARO, 1); + stm32_gpiowrite(GPIO_SPI_CS_HMC, 1); stm32_gpiowrite(GPIO_SPI_CS_MPU, 1); break; @@ -125,6 +128,7 @@ __EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1); stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, !selected); stm32_gpiowrite(GPIO_SPI_CS_BARO, 1); + stm32_gpiowrite(GPIO_SPI_CS_HMC, 1); stm32_gpiowrite(GPIO_SPI_CS_MPU, 1); break; @@ -133,6 +137,16 @@ __EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1); stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); stm32_gpiowrite(GPIO_SPI_CS_BARO, !selected); + stm32_gpiowrite(GPIO_SPI_CS_HMC, 1); + stm32_gpiowrite(GPIO_SPI_CS_MPU, 1); + break; + + case PX4_SPIDEV_HMC: + /* Making sure the other peripherals are not selected */ + stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1); + stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); + stm32_gpiowrite(GPIO_SPI_CS_BARO, 1); + stm32_gpiowrite(GPIO_SPI_CS_HMC, !selected); stm32_gpiowrite(GPIO_SPI_CS_MPU, 1); break; @@ -141,6 +155,7 @@ __EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1); stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); stm32_gpiowrite(GPIO_SPI_CS_BARO, 1); + stm32_gpiowrite(GPIO_SPI_CS_HMC, 1); stm32_gpiowrite(GPIO_SPI_CS_MPU, !selected); break; From 0e84f60826a5f640233a3e265da2079f89e422c0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 11 Oct 2014 00:25:10 +0200 Subject: [PATCH 257/652] Add macro for multi topic support --- src/modules/uORB/uORB.h | 27 +++++++++++++++++++++++++++ 1 file changed, 27 insertions(+) diff --git a/src/modules/uORB/uORB.h b/src/modules/uORB/uORB.h index 82ff46ad26..beb23f61df 100644 --- a/src/modules/uORB/uORB.h +++ b/src/modules/uORB/uORB.h @@ -67,6 +67,33 @@ typedef const struct orb_metadata *orb_id_t; */ #define ORB_ID(_name) &__orb_##_name +/** + * Generates a pointer to the uORB metadata structure for + * a given topic. + * + * The topic must have been declared previously in scope + * with ORB_DECLARE(). + * + * @param _name The name of the topic. + * @param _count The class ID of the topic + */ +#define ORB_ID_DOUBLE(_name, _count) ((_count == CLASS_DEVICE_PRIMARY) ? &__orb_##_name##0 : &__orb_##_name##1) + +/** + * Generates a pointer to the uORB metadata structure for + * a given topic. + * + * The topic must have been declared previously in scope + * with ORB_DECLARE(). + * + * @param _name The name of the topic. + * @param _count The class ID of the topic + */ +#define ORB_ID_TRIPLE(_name, _count) \ + ((_count == CLASS_DEVICE_PRIMARY) ? &__orb_##_name##0 : \ + ((_count == CLASS_DEVICE_SECONDARY) ? &__orb_##_name##1 : \ + (((_count == CLASS_DEVICE_TERTIARY) ? &__orb_##_name##2 : 0)))) + /** * Declare (prototype) the uORB metadata for a topic. * From e952324fbf08405fe555be7169ef0a53a12487f8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 26 Dec 2014 20:25:31 +0100 Subject: [PATCH 258/652] Rework HoTT into a proper library, which fixes parallel build breakage --- makefiles/config_px4fmu-v2_default.mk | 1 + src/drivers/hott/comms.h | 2 +- src/drivers/hott/hott_sensors/module.mk | 4 +-- src/drivers/hott/hott_telemetry/module.mk | 4 +-- src/drivers/hott/messages.h | 18 +++++----- src/drivers/hott/module.mk | 41 +++++++++++++++++++++++ 6 files changed, 54 insertions(+), 16 deletions(-) create mode 100644 src/drivers/hott/module.mk diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index 335055cb17..fa5dff93c2 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -32,6 +32,7 @@ MODULES += drivers/ll40ls # MODULES += drivers/trone MODULES += drivers/gps MODULES += drivers/hil +MODULES += drivers/hott MODULES += drivers/hott/hott_telemetry MODULES += drivers/hott/hott_sensors # MODULES += drivers/blinkm diff --git a/src/drivers/hott/comms.h b/src/drivers/hott/comms.h index f5608122fc..0a586a8fd4 100644 --- a/src/drivers/hott/comms.h +++ b/src/drivers/hott/comms.h @@ -41,6 +41,6 @@ #ifndef COMMS_H_ #define COMMS_H -int open_uart(const char *device); +__EXPORT int open_uart(const char *device); #endif /* COMMS_H_ */ diff --git a/src/drivers/hott/hott_sensors/module.mk b/src/drivers/hott/hott_sensors/module.mk index 47aea6cafb..25a6f0ac04 100644 --- a/src/drivers/hott/hott_sensors/module.mk +++ b/src/drivers/hott/hott_sensors/module.mk @@ -37,8 +37,6 @@ MODULE_COMMAND = hott_sensors -SRCS = hott_sensors.cpp \ - ../messages.cpp \ - ../comms.cpp +SRCS = hott_sensors.cpp MAXOPTIMIZATION = -Os diff --git a/src/drivers/hott/hott_telemetry/module.mk b/src/drivers/hott/hott_telemetry/module.mk index cd7bdbc85b..9de47b356c 100644 --- a/src/drivers/hott/hott_telemetry/module.mk +++ b/src/drivers/hott/hott_telemetry/module.mk @@ -37,8 +37,6 @@ MODULE_COMMAND = hott_telemetry -SRCS = hott_telemetry.cpp \ - ../messages.cpp \ - ../comms.cpp +SRCS = hott_telemetry.cpp MAXOPTIMIZATION = -Os diff --git a/src/drivers/hott/messages.h b/src/drivers/hott/messages.h index 224f8fc56d..a116a50dd6 100644 --- a/src/drivers/hott/messages.h +++ b/src/drivers/hott/messages.h @@ -235,15 +235,15 @@ struct gps_module_msg { // The maximum size of a message. #define MAX_MESSAGE_BUFFER_SIZE 45 -void init_sub_messages(void); -void init_pub_messages(void); -void build_gam_request(uint8_t *buffer, size_t *size); -void publish_gam_message(const uint8_t *buffer); -void build_eam_response(uint8_t *buffer, size_t *size); -void build_gam_response(uint8_t *buffer, size_t *size); -void build_gps_response(uint8_t *buffer, size_t *size); -float _get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next); -void convert_to_degrees_minutes_seconds(double lat, int *deg, int *min, int *sec); +__EXPORT void init_sub_messages(void); +__EXPORT void init_pub_messages(void); +__EXPORT void build_gam_request(uint8_t *buffer, size_t *size); +__EXPORT void publish_gam_message(const uint8_t *buffer); +__EXPORT void build_eam_response(uint8_t *buffer, size_t *size); +__EXPORT void build_gam_response(uint8_t *buffer, size_t *size); +__EXPORT void build_gps_response(uint8_t *buffer, size_t *size); +__EXPORT float _get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next); +__EXPORT void convert_to_degrees_minutes_seconds(double lat, int *deg, int *min, int *sec); #endif /* MESSAGES_H_ */ diff --git a/src/drivers/hott/module.mk b/src/drivers/hott/module.mk new file mode 100644 index 0000000000..31a66d491d --- /dev/null +++ b/src/drivers/hott/module.mk @@ -0,0 +1,41 @@ +############################################################################ +# +# 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. +# +############################################################################ + +# +# Graupner HoTT Sensors messages. +# + +SRCS = messages.cpp \ + comms.cpp + +MAXOPTIMIZATION = -Os From 779f8f00554319facd5c2572c39dd3be4b9c5afd Mon Sep 17 00:00:00 2001 From: Stephan Brown Date: Fri, 26 Dec 2014 23:32:38 +0000 Subject: [PATCH 259/652] mavlink: Removed warnx that references a deleted pointer and causes a hardfault. --- src/modules/mavlink/mavlink_main.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 087ab3c99e..f9a3681b38 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -948,7 +948,6 @@ Mavlink::configure_stream(const char *stream_name, const float rate) /* delete stream */ LL_DELETE(_streams, stream); delete stream; - warnx("deleted stream %s", stream->get_name()); } return OK; From c29972424f6d7b99633c8497f0c25ab7cda4d2ca Mon Sep 17 00:00:00 2001 From: "M.H.Kabir" Date: Sat, 27 Dec 2014 17:18:57 +0530 Subject: [PATCH 260/652] fix timestamp --- src/modules/mavlink/mavlink_messages.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 2203ea1e74..0f25c969db 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1486,6 +1486,7 @@ protected: if (_pos_sp_triplet_sub->update(&pos_sp_triplet)) { mavlink_position_target_global_int_t msg{}; + msg.time_boot_ms = hrt_absolute_time()/1000; msg.coordinate_frame = MAV_FRAME_GLOBAL; msg.lat_int = pos_sp_triplet.current.lat * 1e7; msg.lon_int = pos_sp_triplet.current.lon * 1e7; From d40168dc4b46f922b1be76dcf8a6a37a675788ae Mon Sep 17 00:00:00 2001 From: "M.H.Kabir" Date: Sat, 27 Dec 2014 23:01:31 +0530 Subject: [PATCH 261/652] Add support for rotations of PX4flow --- src/drivers/drv_px4flow.h | 2 +- src/drivers/drv_sensor.h | 15 ++++++++++-- src/drivers/px4flow/px4flow.cpp | 22 ++++++++++++++---- src/modules/sensors/sensor_params.c | 36 +++++++++++++++++++++++++++++ src/modules/sensors/sensors.cpp | 32 +++++++++++++++++++------ 5 files changed, 92 insertions(+), 15 deletions(-) diff --git a/src/drivers/drv_px4flow.h b/src/drivers/drv_px4flow.h index ab640837bb..5aed3f02b0 100644 --- a/src/drivers/drv_px4flow.h +++ b/src/drivers/drv_px4flow.h @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file Rangefinder driver interface. + * @file PX4Flow driver interface. */ #ifndef _DRV_PX4FLOW_H diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h index 8e8b2c1b95..5e4598de86 100644 --- a/src/drivers/drv_sensor.h +++ b/src/drivers/drv_sensor.h @@ -82,8 +82,19 @@ #define SENSORIOCGQUEUEDEPTH _SENSORIOC(3) /** - * Reset the sensor to its default configuration. + * Reset the sensor to its default configuration */ #define SENSORIOCRESET _SENSORIOC(4) -#endif /* _DRV_SENSOR_H */ \ No newline at end of file +/** + * Set the sensor orientation + */ +#define SENSORIOCSROTATION _SENSORIOC(5) + +/** + * Get the sensor orientation + */ +#define SENSORIOCGROTATION _SENSORIOC(6) + +#endif /* _DRV_SENSOR_H */ + diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp index 62308fc654..99a9cc02a8 100644 --- a/src/drivers/px4flow/px4flow.cpp +++ b/src/drivers/px4flow/px4flow.cpp @@ -62,6 +62,8 @@ #include #include +#include + #include #include #include @@ -125,7 +127,7 @@ struct i2c_integral_frame f_integral; class PX4FLOW: public device::I2C { public: - PX4FLOW(int bus, int address = I2C_FLOW_ADDRESS); + PX4FLOW(int bus, int address = I2C_FLOW_ADDRESS, enum Rotation rotation = (enum Rotation)0); virtual ~PX4FLOW(); virtual int init(); @@ -154,6 +156,8 @@ private: perf_counter_t _sample_perf; perf_counter_t _comms_errors; perf_counter_t _buffer_overflows; + + enum Rotation _sensor_rotation; /** * Test whether the device supported by the driver is present at a @@ -199,7 +203,7 @@ private: */ extern "C" __EXPORT int px4flow_main(int argc, char *argv[]); -PX4FLOW::PX4FLOW(int bus, int address) : +PX4FLOW::PX4FLOW(int bus, int address, enum Rotation rotation) : I2C("PX4FLOW", PX4FLOW_DEVICE_PATH, bus, address, 400000), //400khz _reports(nullptr), _sensor_ok(false), @@ -208,7 +212,8 @@ PX4FLOW::PX4FLOW(int bus, int address) : _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")) + _buffer_overflows(perf_alloc(PC_COUNT, "px4flow_buffer_overflows")), + _sensor_rotation(rotation) { // enable debug() calls _debug_enabled = false; @@ -358,8 +363,12 @@ PX4FLOW::ioctl(struct file *filp, int cmd, unsigned long arg) return OK; } - case SENSORIOCGQUEUEDEPTH: - return _reports->size(); + case SENSORIOCSROTATION: + _sensor_rotation = (enum Rotation)arg; + return OK; + + case SENSORIOCGROTATION: + return _sensor_rotation; case SENSORIOCRESET: /* XXX implement this */ @@ -535,6 +544,9 @@ PX4FLOW::collect() report.gyro_temperature = f_integral.gyro_temperature;//Temperature * 100 in centi-degrees Celsius report.sensor_id = 0; + + /* rotate measurements to flight controller frame */ + rotate_3f(_sensor_rotation, report.pixel_flow_x_integral, report.pixel_flow_y_integral, report.ground_distance_m); if (_px4flow_topic < 0) { _px4flow_topic = orb_advertise(ORB_ID(optical_flow), &report); diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 229bfe3ce1..f5f6094d1d 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -261,6 +261,42 @@ PARAM_DEFINE_FLOAT(SENS_BARO_QNH, 1013.25f); */ PARAM_DEFINE_INT32(SENS_BOARD_ROT, 0); +/** + * PX4Flow board rotation + * + * This parameter defines the rotation of the PX4FLOW board relative to the platform. + * Possible values are: + * 0 = No rotation + * 1 = Yaw 45° + * 2 = Yaw 90° + * 3 = Yaw 135° + * 4 = Yaw 180° + * 5 = Yaw 225° + * 6 = Yaw 270° + * 7 = Yaw 315° + * 8 = Roll 180° + * 9 = Roll 180°, Yaw 45° + * 10 = Roll 180°, Yaw 90° + * 11 = Roll 180°, Yaw 135° + * 12 = Pitch 180° + * 13 = Roll 180°, Yaw 225° + * 14 = Roll 180°, Yaw 270° + * 15 = Roll 180°, Yaw 315° + * 16 = Roll 90° + * 17 = Roll 90°, Yaw 45° + * 18 = Roll 90°, Yaw 90° + * 19 = Roll 90°, Yaw 135° + * 20 = Roll 270° + * 21 = Roll 270°, Yaw 45° + * 22 = Roll 270°, Yaw 90° + * 23 = Roll 270°, Yaw 135° + * 24 = Pitch 90° + * 25 = Pitch 270° + * + * @group Sensor Calibration + */ +PARAM_DEFINE_INT32(SENS_FLOW_ROT, 0); + /** * Board rotation Y (Pitch) offset * diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index cdcb428dd1..d8df2c8ff2 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -63,6 +63,7 @@ #include #include #include +#include #include #include @@ -263,6 +264,7 @@ private: float diff_pres_analog_scale; int board_rotation; + int flow_rotation; int external_mag_rotation; float board_offset[3]; @@ -361,6 +363,7 @@ private: param_t battery_current_scaling; param_t board_rotation; + param_t flow_rotation; param_t external_mag_rotation; param_t board_offset[3]; @@ -614,6 +617,7 @@ Sensors::Sensors() : /* rotations */ _parameter_handles.board_rotation = param_find("SENS_BOARD_ROT"); + _parameter_handles.flow_rotation = param_find("SENS_FLOW_ROT"); _parameter_handles.external_mag_rotation = param_find("SENS_EXT_MAG_ROT"); /* rotation offsets */ @@ -831,8 +835,22 @@ Sensors::parameters_update() } param_get(_parameter_handles.board_rotation, &(_parameters.board_rotation)); + param_get(_parameter_handles.flow_rotation, &(_parameters.flow_rotation)); param_get(_parameter_handles.external_mag_rotation, &(_parameters.external_mag_rotation)); + /* set px4flow rotation */ + int flowfd; + flowfd = open(PX4FLOW_DEVICE_PATH, 0); + if (flowfd >= 0) { + int flowret = ioctl(flowfd, SENSORIOCSROTATION, _parameters.flow_rotation); + if (flowret) { + warnx("flow rotation could not be set"); + close(flowfd); + return ERROR; + } + close(flowfd); + } + get_rot_matrix((enum Rotation)_parameters.board_rotation, &_board_rotation); get_rot_matrix((enum Rotation)_parameters.external_mag_rotation, &_external_mag_rotation); @@ -850,20 +868,20 @@ Sensors::parameters_update() /* update barometer qnh setting */ param_get(_parameter_handles.baro_qnh, &(_parameters.baro_qnh)); - int fd; - fd = open(BARO_DEVICE_PATH, 0); - if (fd < 0) { + int barofd; + barofd = open(BARO_DEVICE_PATH, 0); + if (barofd < 0) { warn("%s", BARO_DEVICE_PATH); errx(1, "FATAL: no barometer found"); } else { - int ret = ioctl(fd, BAROIOCSMSLPRESSURE, (unsigned long)(_parameters.baro_qnh * 100)); - if (ret) { + int baroret = ioctl(barofd, BAROIOCSMSLPRESSURE, (unsigned long)(_parameters.baro_qnh * 100)); + if (baroret) { warnx("qnh could not be set"); - close(fd); + close(barofd); return ERROR; } - close(fd); + close(barofd); } return OK; From 11a14c2c3d6ffeff5a896496c0d673845b734d86 Mon Sep 17 00:00:00 2001 From: "M.H.Kabir" Date: Sat, 27 Dec 2014 23:16:09 +0530 Subject: [PATCH 262/652] Add rotation switching to flow from mavlink --- src/drivers/px4flow/px4flow.cpp | 2 +- src/modules/mavlink/mavlink_receiver.cpp | 7 +++++++ 2 files changed, 8 insertions(+), 1 deletion(-) diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp index 99a9cc02a8..9895f780ac 100644 --- a/src/drivers/px4flow/px4flow.cpp +++ b/src/drivers/px4flow/px4flow.cpp @@ -546,7 +546,7 @@ PX4FLOW::collect() report.sensor_id = 0; /* rotate measurements to flight controller frame */ - rotate_3f(_sensor_rotation, report.pixel_flow_x_integral, report.pixel_flow_y_integral, report.ground_distance_m); + rotate_3f(_sensor_rotation, report.pixel_flow_x_integral, report.pixel_flow_y_integral, report.ground_distance_m); // XXX Check this if (_px4flow_topic < 0) { _px4flow_topic = orb_advertise(ORB_ID(optical_flow), &report); diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index e98d72afe7..f91ae3c373 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -68,6 +68,8 @@ #include +#include + #include #include #include @@ -357,6 +359,9 @@ MavlinkReceiver::handle_message_optical_flow_rad(mavlink_message_t *msg) /* optical flow */ mavlink_optical_flow_rad_t flow; mavlink_msg_optical_flow_rad_decode(msg, &flow); + + enum Rotation flow_rot; + param_get(param_find("SENS_FLOW_ROT"),&flow_rot); struct optical_flow_s f; memset(&f, 0, sizeof(f)); @@ -374,6 +379,8 @@ MavlinkReceiver::handle_message_optical_flow_rad(mavlink_message_t *msg) f.sensor_id = flow.sensor_id; f.gyro_temperature = flow.temperature; + rotate_3f(flow_rot, f.pixel_flow_x_integral, f.pixel_flow_y_integral, f.ground_distance_m); // XXX Check this + if (_flow_pub < 0) { _flow_pub = orb_advertise(ORB_ID(optical_flow), &f); From 33653b25c6569013d0a1a5a4885457c0bdc23e06 Mon Sep 17 00:00:00 2001 From: "M.H.Kabir" Date: Sat, 27 Dec 2014 23:25:08 +0530 Subject: [PATCH 263/652] fix Z rotation --- src/drivers/px4flow/px4flow.cpp | 5 +++-- src/modules/mavlink/mavlink_receiver.cpp | 4 +++- 2 files changed, 6 insertions(+), 3 deletions(-) diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp index 9895f780ac..682cbf2415 100644 --- a/src/drivers/px4flow/px4flow.cpp +++ b/src/drivers/px4flow/px4flow.cpp @@ -545,8 +545,9 @@ PX4FLOW::collect() report.sensor_id = 0; - /* rotate measurements to flight controller frame */ - rotate_3f(_sensor_rotation, report.pixel_flow_x_integral, report.pixel_flow_y_integral, report.ground_distance_m); // XXX Check this + /* rotate measurements according to parameter */ + float zeroval = 0.0f; + rotate_3f(_sensor_rotation, report.pixel_flow_x_integral, report.pixel_flow_y_integral, zeroval); if (_px4flow_topic < 0) { _px4flow_topic = orb_advertise(ORB_ID(optical_flow), &report); diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index f91ae3c373..6f5adb5fe5 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -379,7 +379,9 @@ MavlinkReceiver::handle_message_optical_flow_rad(mavlink_message_t *msg) f.sensor_id = flow.sensor_id; f.gyro_temperature = flow.temperature; - rotate_3f(flow_rot, f.pixel_flow_x_integral, f.pixel_flow_y_integral, f.ground_distance_m); // XXX Check this + /* rotate measurements according to parameter */ + float zeroval = 0.0f; + rotate_3f(flow_rot, f.pixel_flow_x_integral, f.pixel_flow_y_integral, zeroval); if (_flow_pub < 0) { _flow_pub = orb_advertise(ORB_ID(optical_flow), &f); From e9b41528dc39ac8a8c565a4c16fc5aa512e37b1a Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 26 Dec 2014 19:21:50 +0100 Subject: [PATCH 264/652] prototype for changing params by rc --- mavlink/include/mavlink/v1.0 | 2 +- src/modules/mavlink/mavlink_parameters.cpp | 39 ++++++- src/modules/mavlink/mavlink_parameters.h | 5 + src/modules/sensors/sensor_params.c | 35 +++++++ src/modules/sensors/sensors.cpp | 114 ++++++++++++++++++++- src/modules/uORB/objects_common.cpp | 3 + src/modules/uORB/topics/rc_channels.h | 7 +- src/modules/uORB/topics/rc_parameter_map.h | 74 +++++++++++++ 8 files changed, 274 insertions(+), 5 deletions(-) create mode 100644 src/modules/uORB/topics/rc_parameter_map.h diff --git a/mavlink/include/mavlink/v1.0 b/mavlink/include/mavlink/v1.0 index ad5e5a645d..87350ba3d2 160000 --- a/mavlink/include/mavlink/v1.0 +++ b/mavlink/include/mavlink/v1.0 @@ -1 +1 @@ -Subproject commit ad5e5a645dec152419264ad32221f7c113ea5c30 +Subproject commit 87350ba3d2e42d15c5934fe3c3387167e9f8769f diff --git a/src/modules/mavlink/mavlink_parameters.cpp b/src/modules/mavlink/mavlink_parameters.cpp index cd5f53d88c..6e0813b38e 100644 --- a/src/modules/mavlink/mavlink_parameters.cpp +++ b/src/modules/mavlink/mavlink_parameters.cpp @@ -44,7 +44,9 @@ #include "mavlink_main.h" MavlinkParametersManager::MavlinkParametersManager(Mavlink *mavlink) : MavlinkStream(mavlink), - _send_all_index(-1) + _send_all_index(-1), + _rc_param_map_pub(-1), + _rc_param_map() { } @@ -135,6 +137,41 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg) break; } + case MAVLINK_MSG_ID_PARAM_MAP_RC: { + /* map a rc channel to a parameter */ + mavlink_param_map_rc_t map_rc; + mavlink_msg_param_map_rc_decode(msg, &map_rc); + + if (map_rc.target_system == mavlink_system.sysid && + (map_rc.target_component == mavlink_system.compid || + map_rc.target_component == MAV_COMP_ID_ALL)) { + + /* Copy values from msg to uorb using the parameter_rc_channel_index as index */ + size_t i = map_rc.parameter_rc_channel_index; + _rc_param_map.param_index[i] = map_rc.param_index; + strncpy(&(_rc_param_map.param_id[i][0]), map_rc.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); + /* enforce null termination */ + _rc_param_map.param_id[i][MAVLINK_MSG_PARAM_MAP_RC_FIELD_PARAM_ID_LEN] = '\0'; + _rc_param_map.scale[i] = map_rc.scale; + _rc_param_map.value0[i] = map_rc.param_value0; + if (map_rc.param_index == -2) { // -2 means unset map + _rc_param_map.valid[i] = false; + } else { + _rc_param_map.valid[i] = true; + } + _rc_param_map.timestamp = hrt_absolute_time(); + + if (_rc_param_map_pub < 0) { + _rc_param_map_pub = orb_advertise(ORB_ID(rc_parameter_map), &_rc_param_map); + + } else { + orb_publish(ORB_ID(rc_parameter_map), _rc_param_map_pub, &_rc_param_map); + } + + } + break; + } + default: break; } diff --git a/src/modules/mavlink/mavlink_parameters.h b/src/modules/mavlink/mavlink_parameters.h index 5576e6b84a..b6736f2128 100644 --- a/src/modules/mavlink/mavlink_parameters.h +++ b/src/modules/mavlink/mavlink_parameters.h @@ -44,6 +44,8 @@ #include "mavlink_bridge_header.h" #include "mavlink_stream.h" +#include +#include class MavlinkParametersManager : public MavlinkStream { @@ -112,4 +114,7 @@ protected: void send(const hrt_abstime t); void send_param(param_t param); + + orb_advert_t _rc_param_map_pub; + struct rc_parameter_map_s _rc_param_map; }; diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 229bfe3ce1..fca148ec54 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -747,6 +747,41 @@ PARAM_DEFINE_INT32(RC_MAP_AUX2, 0); /**< default function: camera roll */ */ PARAM_DEFINE_INT32(RC_MAP_AUX3, 0); +/** + * Channel which changes a parameter + * + * Can be used for parameter tuning with the RC. This one is further referenced as the 1st parameter channel. + * Set to 0 to deactivate * + * + * @min 0 + * @max 18 + * @group Radio Calibration + */ +PARAM_DEFINE_INT32(RC_MAP_PARAM1, 0); + +/** + * Channel which changes a parameter + * + * Can be used for parameter tuning with the RC. This one is further referenced as the 2nd parameter channel. + * Set to 0 to deactivate * + * + * @min 0 + * @max 18 + * @group Radio Calibration + */ +PARAM_DEFINE_INT32(RC_MAP_PARAM2, 0); + +/** + * Channel which changes a parameter + * + * Can be used for parameter tuning with the RC. This one is further referenced as the 3th parameter channel. + * Set to 0 to deactivate * + * + * @min 0 + * @max 18 + * @group Radio Calibration + */ +PARAM_DEFINE_INT32(RC_MAP_PARAM3, 0); /** * Failsafe channel PWM threshold. diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index cdcb428dd1..d404d8a4c7 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -37,7 +37,7 @@ * * @author Lorenz Meier * @author Julian Oes - * @author Thomas Gubler + * @author Thomas Gubler */ #include @@ -82,6 +82,7 @@ #include #include #include +#include #define GYRO_HEALTH_COUNTER_LIMIT_ERROR 20 /* 40 ms downtime at 500 Hz update rate */ #define ACC_HEALTH_COUNTER_LIMIT_ERROR 20 /* 40 ms downtime at 500 Hz update rate */ @@ -190,6 +191,14 @@ private: switch_pos_t get_rc_sw3pos_position(enum RC_CHANNELS_FUNCTION func, float on_th, bool on_inv, float mid_th, bool mid_inv); switch_pos_t get_rc_sw2pos_position(enum RC_CHANNELS_FUNCTION func, float on_th, bool on_inv); + /** + * Update paramters from RC channels if the functionality is activated and the + * input has changed since the last update + * + * @param + */ + void set_params_from_rc(); + /** * Gather and publish RC input data. */ @@ -220,6 +229,7 @@ private: int _diff_pres_sub; /**< raw differential pressure subscription */ int _vcontrol_mode_sub; /**< vehicle control mode subscription */ int _params_sub; /**< notification of parameter updates */ + int _rc_parameter_map_sub; /**< rc parameter map subscription */ int _manual_control_sub; /**< notification of manual control updates */ orb_advert_t _sensor_pub; /**< combined sensor data topic */ @@ -237,6 +247,7 @@ private: struct baro_report _barometer; /**< barometer data */ struct differential_pressure_s _diff_pres; struct airspeed_s _airspeed; + struct rc_parameter_map_s _rc_parameter_map; math::Matrix<3, 3> _board_rotation; /**< rotation matrix for the orientation that the board is mounted */ math::Matrix<3, 3> _external_mag_rotation; /**< rotation matrix for the orientation that an external mag is mounted */ @@ -288,6 +299,8 @@ private: int rc_map_aux4; int rc_map_aux5; + int rc_map_param[RC_PARAM_MAP_NCHAN]; + int32_t rc_fails_thr; float rc_assist_th; float rc_auto_th; @@ -348,6 +361,12 @@ private: param_t rc_map_aux4; param_t rc_map_aux5; + param_t rc_map_param[RC_PARAM_MAP_NCHAN]; + param_t rc_param[RC_PARAM_MAP_NCHAN]; /**< param handles for the paramters which are bound + to a RC channel, equivalent float values in the + _parameters struct are not existing + because these parameters are never read. */ + param_t rc_fails_thr; param_t rc_assist_th; param_t rc_auto_th; @@ -450,6 +469,11 @@ private: */ void parameter_update_poll(bool forced = false); + /** + * Check for changes in rc_parameter_map + */ + void rc_parameter_map_poll(bool forced = false); + /** * Poll the ADC and update readings to suit. * @@ -498,6 +522,7 @@ Sensors::Sensors() : _baro_sub(-1), _vcontrol_mode_sub(-1), _params_sub(-1), + _rc_parameter_map_sub(-1), _manual_control_sub(-1), /* publications */ @@ -518,6 +543,7 @@ Sensors::Sensors() : { memset(&_rc, 0, sizeof(_rc)); memset(&_diff_pres, 0, sizeof(_diff_pres)); + memset(&_rc_parameter_map, 0, sizeof(_rc_parameter_map)); /* basic r/c parameters */ for (unsigned i = 0; i < _rc_max_chan_count; i++) { @@ -570,6 +596,13 @@ Sensors::Sensors() : _parameter_handles.rc_map_aux4 = param_find("RC_MAP_AUX4"); _parameter_handles.rc_map_aux5 = param_find("RC_MAP_AUX5"); + /* RC to parameter mapping for changing parameters with RC */ + for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) { + char name[PARAM_ID_LEN]; + snprintf(name, PARAM_ID_LEN, "RC_MAP_PARAM%d", i + 1); // shifted by 1 because param name starts at 1 + _parameter_handles.rc_map_param[i] = param_find(name); + } + /* RC thresholds */ _parameter_handles.rc_fails_thr = param_find("RC_FAILS_THR"); _parameter_handles.rc_assist_th = param_find("RC_ASSIST_TH"); @@ -747,6 +780,9 @@ Sensors::parameters_update() param_get(_parameter_handles.rc_map_aux3, &(_parameters.rc_map_aux3)); param_get(_parameter_handles.rc_map_aux4, &(_parameters.rc_map_aux4)); param_get(_parameter_handles.rc_map_aux5, &(_parameters.rc_map_aux5)); + for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) { + param_get(_parameter_handles.rc_map_param[i], &(_parameters.rc_map_param[i])); + } param_get(_parameter_handles.rc_fails_thr, &(_parameters.rc_fails_thr)); param_get(_parameter_handles.rc_assist_th, &(_parameters.rc_assist_th)); _parameters.rc_assist_inv = (_parameters.rc_assist_th < 0); @@ -791,6 +827,10 @@ Sensors::parameters_update() _rc.function[AUX_4] = _parameters.rc_map_aux4 - 1; _rc.function[AUX_5] = _parameters.rc_map_aux5 - 1; + for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) { + _rc.function[PARAM_1 + i] = _parameters.rc_map_param[i] - 1; + } + /* gyro offsets */ param_get(_parameter_handles.gyro_offset[0], &(_parameters.gyro_offset[0])); param_get(_parameter_handles.gyro_offset[1], &(_parameters.gyro_offset[1])); @@ -1373,6 +1413,43 @@ Sensors::parameter_update_poll(bool forced) } } +void +Sensors::rc_parameter_map_poll(bool forced) +{ + bool map_updated; + orb_check(_rc_parameter_map_sub, &map_updated); + + if (map_updated) { + orb_copy(ORB_ID(rc_parameter_map), _rc_parameter_map_sub, &_rc_parameter_map); + /* update paramter handles to which the RC channels are mapped */ + for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) { + if (_rc.function[PARAM_1 + i] < 0 || !_rc_parameter_map.valid[i]) { + /* This RC channel is not mapped to a RC-Parameter Channel (e.g. RC_MAP_PARAM1 == 0) + * or no request to map this channel to a param has been sent via mavlink + */ + continue; + } + + /* Set the handle by index if the index is set, otherwise use the id */ + if (_rc_parameter_map.param_index[i] >= 0) { + _parameter_handles.rc_param[i] = param_for_index((unsigned)_rc_parameter_map.param_index[i]); + } else { + _parameter_handles.rc_param[i] = param_find(_rc_parameter_map.param_id[i]); + } + + } + warnx("rc to parameter map updated"); + for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) { + warnx("\ti %d param_id %s scale %.3f value0 %.3f", + i, + _rc_parameter_map.param_id[i], + (double)_rc_parameter_map.scale[i], + (double)_rc_parameter_map.value0[i] + ); + } + } +} + void Sensors::adc_poll(struct sensor_combined_s &raw) { @@ -1551,6 +1628,29 @@ Sensors::get_rc_sw2pos_position(enum RC_CHANNELS_FUNCTION func, float on_th, boo } } +void +Sensors::set_params_from_rc() +{ + static float param_rc_values[RC_PARAM_MAP_NCHAN] = {}; + for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) { + if (_rc.function[PARAM_1 + i] < 0 || !_rc_parameter_map.valid[i]) { + /* This RC channel is not mapped to a RC-Parameter Channel (e.g. RC_MAP_PARAM1 == 0) + * or no request to map this channel to a param has been sent via mavlink + */ + continue; + } + + float rc_val = get_rc_value((enum RC_CHANNELS_FUNCTION)(PARAM_1 + i), -1.0, 1.0); + /* Check if the value has changed, + * maybe we need to introduce a more aggressive limit here */ + if (rc_val > param_rc_values[i] + FLT_EPSILON || rc_val < param_rc_values[i] - FLT_EPSILON) { + param_rc_values[i] = rc_val; + float param_val = _rc_parameter_map.value0[i] + _rc_parameter_map.scale[i] * rc_val; + param_set(_parameter_handles.rc_param[i], ¶m_val); + } + } +} + void Sensors::rc_poll() { @@ -1717,6 +1817,13 @@ Sensors::rc_poll() } else { _actuator_group_3_pub = orb_advertise(ORB_ID(actuator_controls_3), &actuator_group_3); } + + /* Update parameters from RC Channels (tuning with RC) if activated */ + static hrt_abstime last_rc_to_param_map_time = 0; + if (hrt_elapsed_time(&last_rc_to_param_map_time) > 1e6) { + set_params_from_rc(); + last_rc_to_param_map_time = hrt_absolute_time(); + } } } } @@ -1755,6 +1862,7 @@ Sensors::task_main() _diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure)); _vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); + _rc_parameter_map_sub = orb_subscribe(ORB_ID(rc_parameter_map)); _manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); /* rate limit vehicle status updates to 5Hz */ @@ -1788,6 +1896,7 @@ Sensors::task_main() diff_pres_poll(raw); parameter_update_poll(true /* forced */); + rc_parameter_map_poll(true /* forced */); /* advertise the sensor_combined topic and make the initial publication */ _sensor_pub = orb_advertise(ORB_ID(sensor_combined), &raw); @@ -1820,6 +1929,9 @@ Sensors::task_main() /* check parameters for updates */ parameter_update_poll(); + /* check rc parameter map for updates */ + rc_parameter_map_poll(); + /* the timestamp of the raw struct is updated by the gyro_poll() method */ /* copy most recent sensor data */ diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 3d5755a465..d834c35747 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -246,3 +246,6 @@ ORB_DEFINE(tecs_status, struct tecs_status_s); #include "topics/wind_estimate.h" ORB_DEFINE(wind_estimate, struct wind_estimate_s); + +#include "topics/rc_parameter_map.h" +ORB_DEFINE(rc_parameter_map, struct rc_parameter_map_s); diff --git a/src/modules/uORB/topics/rc_channels.h b/src/modules/uORB/topics/rc_channels.h index 16916cc4df..2fde5d4240 100644 --- a/src/modules/uORB/topics/rc_channels.h +++ b/src/modules/uORB/topics/rc_channels.h @@ -65,12 +65,15 @@ enum RC_CHANNELS_FUNCTION { AUX_2, AUX_3, AUX_4, - AUX_5 + AUX_5, + PARAM_1, + PARAM_2, + PARAM_3 }; // MAXIMUM FUNCTIONS IS != MAXIMUM RC INPUT CHANNELS -#define RC_CHANNELS_FUNCTION_MAX 18 +#define RC_CHANNELS_FUNCTION_MAX 19 /** * @addtogroup topics diff --git a/src/modules/uORB/topics/rc_parameter_map.h b/src/modules/uORB/topics/rc_parameter_map.h new file mode 100644 index 0000000000..47672c5d5a --- /dev/null +++ b/src/modules/uORB/topics/rc_parameter_map.h @@ -0,0 +1,74 @@ +/**************************************************************************** + * + * 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 rc_parameter_map.h + * Maps RC channels to parameters + * + * @author Thomas Gubler + */ + +#ifndef TOPIC_RC_PARAMETER_MAP_H +#define TOPIC_RC_PARAMETER_MAP_H + +#include +#include "../uORB.h" + +#define RC_PARAM_MAP_NCHAN 3 // This limit is also hardcoded in the enum RC_CHANNELS_FUNCTION in rc_channels.h +#define PARAM_ID_LEN 16 // corresponds to MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + +/** + * @addtogroup topics + * @{ + */ + +struct rc_parameter_map_s { + uint64_t timestamp; /**< time at which the map was updated */ + + bool valid[RC_PARAM_MAP_NCHAN]; /**< true for RC-Param channels which are mapped to a param */ + + int param_index[RC_PARAM_MAP_NCHAN]; /**< corresponding param index, this + this field is ignored if set to -1, in this case param_id will + be used*/ + char param_id[RC_PARAM_MAP_NCHAN][PARAM_ID_LEN + 1]; /**< corresponding param id, null terminated */ + float scale[RC_PARAM_MAP_NCHAN]; /** scale to map the RC input [-1, 1] to a parameter value */ + float value0[RC_PARAM_MAP_NCHAN]; /** inital value around which the parameter value is changed */ +}; + +/** + * @} + */ + +ORB_DECLARE(rc_parameter_map); + +#endif From ef8abfbf148ac6fd7a405acdd4ee8eeb156867b6 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 28 Dec 2014 21:57:30 +0100 Subject: [PATCH 265/652] rc2param: min and max values --- mavlink/include/mavlink/v1.0 | 2 +- src/modules/mavlink/mavlink_parameters.cpp | 2 ++ src/modules/sensors/sensors.cpp | 10 +++++++--- src/modules/uORB/topics/rc_parameter_map.h | 2 ++ 4 files changed, 12 insertions(+), 4 deletions(-) diff --git a/mavlink/include/mavlink/v1.0 b/mavlink/include/mavlink/v1.0 index 87350ba3d2..b5732d07ec 160000 --- a/mavlink/include/mavlink/v1.0 +++ b/mavlink/include/mavlink/v1.0 @@ -1 +1 @@ -Subproject commit 87350ba3d2e42d15c5934fe3c3387167e9f8769f +Subproject commit b5732d07ec89025a9d321a923fe027d7df7b30a6 diff --git a/src/modules/mavlink/mavlink_parameters.cpp b/src/modules/mavlink/mavlink_parameters.cpp index 6e0813b38e..e9858b73c6 100644 --- a/src/modules/mavlink/mavlink_parameters.cpp +++ b/src/modules/mavlink/mavlink_parameters.cpp @@ -154,6 +154,8 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg) _rc_param_map.param_id[i][MAVLINK_MSG_PARAM_MAP_RC_FIELD_PARAM_ID_LEN] = '\0'; _rc_param_map.scale[i] = map_rc.scale; _rc_param_map.value0[i] = map_rc.param_value0; + _rc_param_map.value_min[i] = map_rc.param_value_min; + _rc_param_map.value_max[i] = map_rc.param_value_max; if (map_rc.param_index == -2) { // -2 means unset map _rc_param_map.valid[i] = false; } else { diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index d404d8a4c7..3fa1575f0c 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1440,11 +1440,13 @@ Sensors::rc_parameter_map_poll(bool forced) } warnx("rc to parameter map updated"); for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) { - warnx("\ti %d param_id %s scale %.3f value0 %.3f", + warnx("\ti %d param_id %s scale %.3f value0 %.3f, min %.3f, max %.3f", i, _rc_parameter_map.param_id[i], (double)_rc_parameter_map.scale[i], - (double)_rc_parameter_map.value0[i] + (double)_rc_parameter_map.value0[i], + (double)_rc_parameter_map.value_min[i], + (double)_rc_parameter_map.value_max[i] ); } } @@ -1645,7 +1647,9 @@ Sensors::set_params_from_rc() * maybe we need to introduce a more aggressive limit here */ if (rc_val > param_rc_values[i] + FLT_EPSILON || rc_val < param_rc_values[i] - FLT_EPSILON) { param_rc_values[i] = rc_val; - float param_val = _rc_parameter_map.value0[i] + _rc_parameter_map.scale[i] * rc_val; + float param_val = math::constrain( + _rc_parameter_map.value0[i] + _rc_parameter_map.scale[i] * rc_val, + _rc_parameter_map.value_min[i], _rc_parameter_map.value_max[i]); param_set(_parameter_handles.rc_param[i], ¶m_val); } } diff --git a/src/modules/uORB/topics/rc_parameter_map.h b/src/modules/uORB/topics/rc_parameter_map.h index 47672c5d5a..6e68dc4b6f 100644 --- a/src/modules/uORB/topics/rc_parameter_map.h +++ b/src/modules/uORB/topics/rc_parameter_map.h @@ -63,6 +63,8 @@ struct rc_parameter_map_s { char param_id[RC_PARAM_MAP_NCHAN][PARAM_ID_LEN + 1]; /**< corresponding param id, null terminated */ float scale[RC_PARAM_MAP_NCHAN]; /** scale to map the RC input [-1, 1] to a parameter value */ float value0[RC_PARAM_MAP_NCHAN]; /** inital value around which the parameter value is changed */ + float value_min[RC_PARAM_MAP_NCHAN]; /** minimal parameter value */ + float value_max[RC_PARAM_MAP_NCHAN]; /** minimal parameter value */ }; /** From c583f1fe8b9e66b42dd4697a5908541dfdd57f69 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 28 Dec 2014 23:22:02 +0100 Subject: [PATCH 266/652] increase commander framesize --- src/modules/commander/module.mk | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/module.mk b/src/modules/commander/module.mk index dac2ce59a6..0e2a5356be 100644 --- a/src/modules/commander/module.mk +++ b/src/modules/commander/module.mk @@ -52,5 +52,5 @@ MODULE_STACKSIZE = 1200 MAXOPTIMIZATION = -Os -EXTRACXXFLAGS = -Wframe-larger-than=1900 +EXTRACXXFLAGS = -Wframe-larger-than=2000 From ca97d0156c07ad6cc09e4623140a7f47214946f5 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 28 Dec 2014 23:22:34 +0100 Subject: [PATCH 267/652] -Wno-attributes -Wno-packed for mavlink --- src/modules/mavlink/module.mk | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/modules/mavlink/module.mk b/src/modules/mavlink/module.mk index 91fdd61547..f9d30dcbea 100644 --- a/src/modules/mavlink/module.mk +++ b/src/modules/mavlink/module.mk @@ -53,4 +53,6 @@ MAXOPTIMIZATION = -Os MODULE_STACKSIZE = 1024 -EXTRACXXFLAGS = -Weffc++ +EXTRACXXFLAGS = -Weffc++ -Wno-attributes -Wno-packed + +EXTRACFLAGS = -Wno-packed From fc780a172948822886845aa7ab0a16d10a09e9e6 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 28 Dec 2014 23:22:34 +0100 Subject: [PATCH 268/652] -Wno-attributes -Wno-packed for mavlink --- src/modules/mavlink/module.mk | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/modules/mavlink/module.mk b/src/modules/mavlink/module.mk index 91fdd61547..f9d30dcbea 100644 --- a/src/modules/mavlink/module.mk +++ b/src/modules/mavlink/module.mk @@ -53,4 +53,6 @@ MAXOPTIMIZATION = -Os MODULE_STACKSIZE = 1024 -EXTRACXXFLAGS = -Weffc++ +EXTRACXXFLAGS = -Weffc++ -Wno-attributes -Wno-packed + +EXTRACFLAGS = -Wno-packed From ee7e008008caa04f905654cb18e6d68fd980f8cd Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 28 Dec 2014 23:22:02 +0100 Subject: [PATCH 269/652] increase commander framesize --- src/modules/commander/module.mk | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/module.mk b/src/modules/commander/module.mk index dac2ce59a6..0e2a5356be 100644 --- a/src/modules/commander/module.mk +++ b/src/modules/commander/module.mk @@ -52,5 +52,5 @@ MODULE_STACKSIZE = 1200 MAXOPTIMIZATION = -Os -EXTRACXXFLAGS = -Wframe-larger-than=1900 +EXTRACXXFLAGS = -Wframe-larger-than=2000 From 277fde472c47aaec115a7a8dc8871daefe973d83 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Sun, 28 Dec 2014 16:37:32 -0600 Subject: [PATCH 270/652] Fixed FLOW sdlog2 format. --- src/modules/sdlog2/sdlog2_messages.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index b78b430aad..bd85136ae9 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -468,7 +468,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"), LOG_FORMAT(AIRS, "fff", "IndSpeed,TrueSpeed,AirTemp"), LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"), - LOG_FORMAT(FLOW, "hhfffBB", "RawX,RawY,CompX,CompY,Dist,Q,SensID"), + LOG_FORMAT(FLOW, "QBffffffLLHhB", "IntT,ID,RawX,RawY,CompX,CompY,CompZ,Dist,TSpan,DtSonar,FrmCnt,GyroT,Qlty"), LOG_FORMAT(GPOS, "LLfffffff", "Lat,Lon,Alt,VelN,VelE,VelD,EPH,EPV,TALT"), LOG_FORMAT(GPSP, "BLLffBfbf", "NavState,Lat,Lon,Alt,Yaw,Type,LoitR,LoitDir,PitMin"), LOG_FORMAT(ESC, "HBBBHHffiffH", "count,nESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"), From 22f744f0e17f83a706998381d1977c9cb24d457a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 29 Dec 2014 00:13:17 +0100 Subject: [PATCH 271/652] sdlog2: Fix flow message --- src/modules/sdlog2/sdlog2_messages.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index bd85136ae9..5b047f5387 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -468,7 +468,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"), LOG_FORMAT(AIRS, "fff", "IndSpeed,TrueSpeed,AirTemp"), LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"), - LOG_FORMAT(FLOW, "QBffffffLLHhB", "IntT,ID,RawX,RawY,CompX,CompY,CompZ,Dist,TSpan,DtSonar,FrmCnt,GyroT,Qlty"), + LOG_FORMAT(FLOW, "QBffffffLLHhB", "IntT,ID,RawX,RawY,RX,RY,RZ,Dist,TSpan,DtSonar,FrmCnt,GT,Qlty"), LOG_FORMAT(GPOS, "LLfffffff", "Lat,Lon,Alt,VelN,VelE,VelD,EPH,EPV,TALT"), LOG_FORMAT(GPSP, "BLLffBfbf", "NavState,Lat,Lon,Alt,Yaw,Type,LoitR,LoitDir,PitMin"), LOG_FORMAT(ESC, "HBBBHHffiffH", "count,nESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"), From a0b767f4676a9204260442c56ab309108d47638d Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Wed, 24 Dec 2014 13:45:04 -0700 Subject: [PATCH 272/652] add yaw_manual override variable, effective only in ALTCTL and do not publish it in attitude rates --- src/modules/fw_att_control/fw_att_control_main.cpp | 5 +++++ 1 file changed, 5 insertions(+) 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 8d18ae68cb..d8377899c1 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -824,6 +824,7 @@ FixedwingAttitudeControl::task_main() float roll_sp = _parameters.rollsp_offset_rad; float pitch_sp = _parameters.pitchsp_offset_rad; + float yaw_manual = 0.0f; float throttle_sp = 0.0f; /* Read attitude setpoint from uorb if @@ -884,6 +885,8 @@ FixedwingAttitudeControl::task_main() + _parameters.rollsp_offset_rad; pitch_sp = -(_manual.x * _parameters.man_pitch_max - _parameters.trim_pitch) + _parameters.pitchsp_offset_rad; + // allow manual control of rudder deflection + yaw_manual = _manual.r; throttle_sp = _manual.z; _actuators.control[4] = _manual.flaps; @@ -983,6 +986,8 @@ FixedwingAttitudeControl::task_main() _pitch_ctrl.get_desired_rate(), _parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator); _actuators.control[2] = (isfinite(yaw_u)) ? yaw_u + _parameters.trim_yaw : _parameters.trim_yaw; + // add in manual rudder control + _actuators.control[2] += yaw_manual; if (!isfinite(yaw_u)) { _yaw_ctrl.reset_integrator(); perf_count(_nonfinite_output_perf); From f05aa01e693dab6b16aee136f162a614c5931421 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 29 Dec 2014 16:55:22 +0100 Subject: [PATCH 273/652] fw att: fix comment style --- .../fw_att_control/fw_att_control_main.cpp | 22 +++++++++++-------- 1 file changed, 13 insertions(+), 9 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 d8377899c1..45dcddb9cb 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -400,7 +400,8 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : /* fetch initial parameter values */ parameters_update(); - // set correct uORB ID, depending on if vehicle is VTOL or not + + /* 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); @@ -724,22 +725,24 @@ FixedwingAttitudeControl::task_main() R.set(_att.R); R_adapted.set(_att.R); - //move z to x + /* 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 + + /* 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) + /* 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 + + /* fill in new attitude data */ _att.roll = euler_angles(0); _att.pitch = euler_angles(1); _att.yaw = euler_angles(2); @@ -753,7 +756,7 @@ FixedwingAttitudeControl::task_main() _att.R[2][1] = R_adapted(2, 1); _att.R[2][2] = R_adapted(2, 2); - // lastly, roll- and yawspeed have to be swaped + /* lastly, roll- and yawspeed have to be swaped */ float helper = _att.rollspeed; _att.rollspeed = -_att.yawspeed; _att.yawspeed = helper; @@ -850,7 +853,7 @@ FixedwingAttitudeControl::task_main() _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) @@ -885,7 +888,7 @@ FixedwingAttitudeControl::task_main() + _parameters.rollsp_offset_rad; pitch_sp = -(_manual.x * _parameters.man_pitch_max - _parameters.trim_pitch) + _parameters.pitchsp_offset_rad; - // allow manual control of rudder deflection + /* allow manual control of rudder deflection */ yaw_manual = _manual.r; throttle_sp = _manual.z; _actuators.control[4] = _manual.flaps; @@ -986,7 +989,8 @@ FixedwingAttitudeControl::task_main() _pitch_ctrl.get_desired_rate(), _parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator); _actuators.control[2] = (isfinite(yaw_u)) ? yaw_u + _parameters.trim_yaw : _parameters.trim_yaw; - // add in manual rudder control + + /* add in manual rudder control */ _actuators.control[2] += yaw_manual; if (!isfinite(yaw_u)) { _yaw_ctrl.reset_integrator(); From 59e0b67c8eaa4295c23f53500ff5c8e3b34ff5a8 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sat, 20 Dec 2014 17:35:31 +0900 Subject: [PATCH 274/652] NuttxConfig: increase I2C timeout to 10ms --- nuttx-configs/px4fmu-v2/nsh/defconfig | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index 90dde62946..50d5b0296e 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -323,7 +323,7 @@ CONFIG_STM32_USART_SINGLEWIRE=y # # CONFIG_STM32_I2C_DYNTIMEO is not set CONFIG_STM32_I2CTIMEOSEC=0 -CONFIG_STM32_I2CTIMEOMS=1 +CONFIG_STM32_I2CTIMEOMS=10 # CONFIG_STM32_I2C_DUTY16_9 is not set # From ac9e9835ac14284f704fda57593f16ba37159905 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 30 Dec 2014 11:13:05 +0900 Subject: [PATCH 275/652] i2c: const get_address --- src/drivers/device/i2c.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/device/i2c.h b/src/drivers/device/i2c.h index 705b398b05..206e71ddcc 100644 --- a/src/drivers/device/i2c.h +++ b/src/drivers/device/i2c.h @@ -58,7 +58,7 @@ public: /** * Get the address */ - int16_t get_address() { return _address; } + int16_t get_address() const { return _address; } protected: /** From df9547e9d36b4fd167f746dff1b809d2f4bfac3d Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 25 Nov 2014 15:03:44 +0900 Subject: [PATCH 276/652] batt_smbus: driver for smart battery --- src/drivers/batt_smbus/batt_smbus.cpp | 438 ++++++++++++++++++++++++++ src/drivers/batt_smbus/module.mk | 8 + src/drivers/drv_batt_smbus.h | 50 +++ 3 files changed, 496 insertions(+) create mode 100644 src/drivers/batt_smbus/batt_smbus.cpp create mode 100644 src/drivers/batt_smbus/module.mk create mode 100644 src/drivers/drv_batt_smbus.h diff --git a/src/drivers/batt_smbus/batt_smbus.cpp b/src/drivers/batt_smbus/batt_smbus.cpp new file mode 100644 index 0000000000..89abcf8385 --- /dev/null +++ b/src/drivers/batt_smbus/batt_smbus.cpp @@ -0,0 +1,438 @@ +/**************************************************************************** + * + * Copyright (C) 2012, 2013 PX4 Development Team. All rights reserved. + * Author: Randy Mackay + * + * 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 batt_smbus.cpp + * + * Driver for a battery monitor connected via SMBus (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 + +#include + +#include +#include +#include +#include + +#define BATT_SMBUS_I2C_BUS PX4_I2C_BUS_EXPANSION +#define BATT_SMBUS_ADDR 0x0B /* I2C address */ +#define BATT_SMBUS_TEMP 0x08 /* temperature register */ +#define BATT_SMBUS_VOLTAGE 0x09 /* voltage register */ +#define BATT_SMBUS_DESIGN_CAPACITY 0x18 /* design capacity register */ +#define BATT_SMBUS_DESIGN_VOLTAGE 0x19 /* design voltage register */ +#define BATT_SMBUS_SERIALNUM 0x1c /* serial number register */ +#define BATT_SMBUS_MANUFACTURE_INFO 0x25 /* cell voltage register */ +#define BATT_SMBUS_CURRENT 0x2a /* current register */ +#define BATT_SMBUS_MEASUREMENT_INTERVAL_MS (1000000 / 10) /* time in microseconds, measure at 10hz */ + +#ifndef CONFIG_SCHED_WORKQUEUE +# error This requires CONFIG_SCHED_WORKQUEUE. +#endif + +class BATT_SMBUS : public device::I2C +{ +public: + BATT_SMBUS(int bus = PX4_I2C_BUS_EXPANSION, uint16_t batt_smbus_addr = BATT_SMBUS_ADDR); + virtual ~BATT_SMBUS(); + + virtual int init(); + virtual int test(); + +protected: + virtual int probe(); // check if the device can be contacted + +private: + + // start periodic reads from the battery + void start(); + + // stop periodic reads from the battery + void stop(); + + // static function that is called by worker queue + static void cycle_trampoline(void *arg); + + // perform a read from the battery + void cycle(); + + // read_reg - read a word from specified register + int read_reg(uint8_t reg, uint16_t &val); + + // read_block - returns number of characters read if successful, zero if unsuccessful + uint8_t read_block(uint8_t reg, uint8_t* data, uint8_t max_len, bool append_zero); + + // internal variables + work_s _work; // work queue for scheduling reads + RingBuffer *_reports; // buffer of recorded voltages, currents + struct battery_status_s _last_report; // last published report, used for test() + orb_advert_t _batt_topic; + orb_id_t _batt_orb_id; +}; + +/* for now, we only support one BATT_SMBUS */ +namespace +{ +BATT_SMBUS *g_batt_smbus; +} + +void batt_smbus_usage(); + +extern "C" __EXPORT int batt_smbus_main(int argc, char *argv[]); + +// constructor +BATT_SMBUS::BATT_SMBUS(int bus, uint16_t batt_smbus_addr) : + I2C("batt_smbus", BATT_SMBUS_DEVICE_PATH, bus, batt_smbus_addr, 400000), + _work{}, + _reports(nullptr), + _batt_topic(-1), + _batt_orb_id(nullptr) +{ + // work_cancel in the dtor will explode if we don't do this... + memset(&_work, 0, sizeof(_work)); +} + +// destructor +BATT_SMBUS::~BATT_SMBUS() +{ + /* make sure we are truly inactive */ + stop(); + + if (_reports != nullptr) { + delete _reports; + } +} + +int +BATT_SMBUS::init() +{ + int ret = ENOTTY; + + // attempt to initialise I2C bus + ret = I2C::init(); + + if (ret != OK) { + errx(1,"failed to init I2C"); + return ret; + } else { + /* allocate basic report buffers */ + _reports = new RingBuffer(2, sizeof(struct battery_status_s)); + if (_reports == nullptr) { + ret = ENOTTY; + } else { + // start work queue + start(); + } + } + + // init orb id + _batt_orb_id = ORB_ID(battery_status); + + return ret; +} + +int +BATT_SMBUS::test() +{ + int sub = orb_subscribe(ORB_ID(battery_status)); + bool updated = false; + struct battery_status_s status; + uint64_t start_time = hrt_absolute_time(); + + // loop for 5 seconds + while ((hrt_absolute_time() - start_time) < 5000000) { + + // display new info that has arrived from the orb + orb_check(sub, &updated); + if (updated) { + if (orb_copy(ORB_ID(battery_status), sub, &status) == OK) { + warnx("V=%4.2f C=%4.2f", status.voltage_v, status.current_a); + } + } + + // sleep for 0.05 seconds + usleep(50000); + } + + return OK; +} + +int +BATT_SMBUS::probe() +{ + // always return OK to ensure device starts + return OK; +} + +// start periodic reads from the battery +void +BATT_SMBUS::start() +{ + /* reset the report ring and state machine */ + _reports->flush(); + + /* schedule a cycle to start things */ + work_queue(HPWORK, &_work, (worker_t)&BATT_SMBUS::cycle_trampoline, this, 1); +} + +// stop periodic reads from the battery +void +BATT_SMBUS::stop() +{ + work_cancel(HPWORK, &_work); +} + +// static function that is called by worker queue +void +BATT_SMBUS::cycle_trampoline(void *arg) +{ + BATT_SMBUS *dev = (BATT_SMBUS *)arg; + + dev->cycle(); +} + +// perform a read from the battery +void +BATT_SMBUS::cycle() +{ + // read data from sensor + struct battery_status_s new_report; + + // set time of reading + new_report.timestamp = hrt_absolute_time(); + + // read voltage + uint16_t tmp; + if (read_reg(BATT_SMBUS_VOLTAGE, tmp) == OK) { + // initialise new_report + memset(&new_report, 0, sizeof(new_report)); + + // convert millivolts to volts + new_report.voltage_v = ((float)tmp) / 1000.0f; + + // To-Do: read current as block from BATT_SMBUS_CURRENT register + + // publish to orb + if (_batt_topic != -1) { + orb_publish(_batt_orb_id, _batt_topic, &new_report); + } else { + _batt_topic = orb_advertise(_batt_orb_id, &new_report); + if (_batt_topic < 0) { + errx(1, "ADVERT FAIL"); + } + } + + // copy report for test() + _last_report = new_report; + + /* post a report to the ring */ + _reports->force(&new_report); + + /* notify anyone waiting for data */ + poll_notify(POLLIN); + } + + /* schedule a fresh cycle call when the measurement is done */ + work_queue(HPWORK, &_work, (worker_t)&BATT_SMBUS::cycle_trampoline, this, USEC2TICK(BATT_SMBUS_MEASUREMENT_INTERVAL_MS)); +} + +int +BATT_SMBUS::read_reg(uint8_t reg, uint16_t &val) +{ + uint8_t buff[2]; + + // short sleep to improve reliability in cases of two consecutive reads + usleep(1); + + // read from register + int ret = transfer(®, 1, buff, 2); + if (ret == OK) { + val = (uint16_t)buff[1] << 8 | (uint16_t)buff[0]; + } + + // return success or failure + return ret; +} + +// read_block - returns number of characters read if successful, zero if unsuccessful +uint8_t BATT_SMBUS::read_block(uint8_t reg, uint8_t* data, uint8_t max_len, bool append_zero) +{ + uint8_t buff[max_len+1]; // buffer to hold results + + usleep(1); + + // read bytes + int ret = transfer(®, 1,buff, max_len+1); + + // return zero on failure + if (ret != OK) { + return 0; + } + + // get length + uint8_t bufflen = buff[0]; + + // sanity check length returned by smbus + if (bufflen == 0 || bufflen > max_len) { + return 0; + } + + // copy data + memcpy(data, &buff[1], bufflen); + + // optionally add zero to end + if (append_zero) { + data[bufflen] = '\0'; + } + + // return success + return bufflen; +} + +///////////////////////// shell functions /////////////////////// + +void +batt_smbus_usage() +{ + warnx("missing command: try 'start', 'test', 'stop'"); + warnx("options:"); + warnx(" -b i2cbus (%d)", BATT_SMBUS_I2C_BUS); + warnx(" -a addr (0x%x)", BATT_SMBUS_ADDR); +} + +int +batt_smbus_main(int argc, char *argv[]) +{ + int i2cdevice = BATT_SMBUS_I2C_BUS; + int batt_smbusadr = BATT_SMBUS_ADDR; /* 7bit */ + + int ch; + + /* jump over start/off/etc and look at options first */ + while ((ch = getopt(argc, argv, "a:b:")) != EOF) { + switch (ch) { + case 'a': + batt_smbusadr = strtol(optarg, NULL, 0); + break; + + case 'b': + i2cdevice = strtol(optarg, NULL, 0); + break; + + default: + batt_smbus_usage(); + exit(0); + } + } + + if (optind >= argc) { + batt_smbus_usage(); + exit(1); + } + + const char *verb = argv[optind]; + + if (!strcmp(verb, "start")) { + if (g_batt_smbus != nullptr) { + errx(1, "already started"); + } else { + // create new global object + g_batt_smbus = new BATT_SMBUS(i2cdevice, batt_smbusadr); + + if (g_batt_smbus == nullptr) { + errx(1, "new failed"); + } + + if (OK != g_batt_smbus->init()) { + delete g_batt_smbus; + g_batt_smbus = nullptr; + errx(1, "init failed"); + } + } + + exit(0); + } + + /* need the driver past this point */ + if (g_batt_smbus == nullptr) { + warnx("not started"); + batt_smbus_usage(); + exit(1); + } + + if (!strcmp(verb, "test")) { + g_batt_smbus->test(); + exit(0); + } + + if (!strcmp(verb, "stop")) { + delete g_batt_smbus; + g_batt_smbus = nullptr; + exit(0); + } + + batt_smbus_usage(); + exit(0); +} diff --git a/src/drivers/batt_smbus/module.mk b/src/drivers/batt_smbus/module.mk new file mode 100644 index 0000000000..b32ea6e55f --- /dev/null +++ b/src/drivers/batt_smbus/module.mk @@ -0,0 +1,8 @@ +# +# driver for SMBus smart batteries +# + +MODULE_COMMAND = batt_smbus +SRCS = batt_smbus.cpp + +MAXOPTIMIZATION = -Os diff --git a/src/drivers/drv_batt_smbus.h b/src/drivers/drv_batt_smbus.h new file mode 100644 index 0000000000..6bba5fe167 --- /dev/null +++ b/src/drivers/drv_batt_smbus.h @@ -0,0 +1,50 @@ +/**************************************************************************** + * + * Copyright (C) 2012-2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file drv_batt_smbus.h + * + * SMBus battery monitor device API + */ + +#pragma once + +#include +#include +#include "drv_orb_dev.h" + +/* device path */ +#define BATT_SMBUS_DEVICE_PATH "/dev/batt_smbus" + +/* ObjDev tag for battery data */ +ORB_DECLARE(battery_status); From b8e818b38715b24889858c22ab0396a35df624c3 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 9 Dec 2014 19:29:55 +0900 Subject: [PATCH 277/652] batt_smbus: remove sleep before I2C transfer --- src/drivers/batt_smbus/batt_smbus.cpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/drivers/batt_smbus/batt_smbus.cpp b/src/drivers/batt_smbus/batt_smbus.cpp index 89abcf8385..494187ac02 100644 --- a/src/drivers/batt_smbus/batt_smbus.cpp +++ b/src/drivers/batt_smbus/batt_smbus.cpp @@ -303,9 +303,6 @@ BATT_SMBUS::read_reg(uint8_t reg, uint16_t &val) { uint8_t buff[2]; - // short sleep to improve reliability in cases of two consecutive reads - usleep(1); - // read from register int ret = transfer(®, 1, buff, 2); if (ret == OK) { From 78b9e06a154cf7ef1549c0c01f8611244d5c3b2c Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 15 Dec 2014 16:42:47 +0900 Subject: [PATCH 278/652] batt_smbus: remove redundant ORB_DECLARE --- src/drivers/drv_batt_smbus.h | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/drivers/drv_batt_smbus.h b/src/drivers/drv_batt_smbus.h index 6bba5fe167..ca130c84ee 100644 --- a/src/drivers/drv_batt_smbus.h +++ b/src/drivers/drv_batt_smbus.h @@ -45,6 +45,3 @@ /* device path */ #define BATT_SMBUS_DEVICE_PATH "/dev/batt_smbus" - -/* ObjDev tag for battery data */ -ORB_DECLARE(battery_status); From 65bcd0e1226f5f5d6db28dafe5faea771ccbde1d Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 29 Dec 2014 21:09:32 +0900 Subject: [PATCH 279/652] batt_smbus: minor format fix --- src/drivers/batt_smbus/batt_smbus.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/batt_smbus/batt_smbus.cpp b/src/drivers/batt_smbus/batt_smbus.cpp index 494187ac02..882b38f7ec 100644 --- a/src/drivers/batt_smbus/batt_smbus.cpp +++ b/src/drivers/batt_smbus/batt_smbus.cpp @@ -304,7 +304,7 @@ BATT_SMBUS::read_reg(uint8_t reg, uint16_t &val) uint8_t buff[2]; // read from register - int ret = transfer(®, 1, buff, 2); + int ret = transfer(®, 1, buff, 2); if (ret == OK) { val = (uint16_t)buff[1] << 8 | (uint16_t)buff[0]; } From a952a18b42d486e6f3ba1d52506fbd2ba2fc3425 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 15 Dec 2014 16:42:29 +0900 Subject: [PATCH 280/652] batt_smbus: add get_PEC --- src/drivers/batt_smbus/batt_smbus.cpp | 84 ++++++++++++++++++++++++--- 1 file changed, 77 insertions(+), 7 deletions(-) diff --git a/src/drivers/batt_smbus/batt_smbus.cpp b/src/drivers/batt_smbus/batt_smbus.cpp index 882b38f7ec..4d4a84049d 100644 --- a/src/drivers/batt_smbus/batt_smbus.cpp +++ b/src/drivers/batt_smbus/batt_smbus.cpp @@ -84,10 +84,13 @@ #define BATT_SMBUS_DESIGN_CAPACITY 0x18 /* design capacity register */ #define BATT_SMBUS_DESIGN_VOLTAGE 0x19 /* design voltage register */ #define BATT_SMBUS_SERIALNUM 0x1c /* serial number register */ +#define BATT_SMBUS_MANUFACTURE_NAME 0x20 /* manufacturer name */ #define BATT_SMBUS_MANUFACTURE_INFO 0x25 /* cell voltage register */ #define BATT_SMBUS_CURRENT 0x2a /* current register */ #define BATT_SMBUS_MEASUREMENT_INTERVAL_MS (1000000 / 10) /* time in microseconds, measure at 10hz */ +#define BATT_SMBUS_PEC_POLYNOMIAL 0x07 /* Polynomial for calculating PEC */ + #ifndef CONFIG_SCHED_WORKQUEUE # error This requires CONFIG_SCHED_WORKQUEUE. #endif @@ -124,6 +127,10 @@ private: // read_block - returns number of characters read if successful, zero if unsuccessful uint8_t read_block(uint8_t reg, uint8_t* data, uint8_t max_len, bool append_zero); + // get_PEC - calculate PEC for a read or write from the battery + // buff is the data that was read or will be written + uint8_t get_PEC(uint8_t cmd, bool reading, const uint8_t buff[], uint8_t len) const; + // internal variables work_s _work; // work queue for scheduling reads RingBuffer *_reports; // buffer of recorded voltages, currents @@ -272,7 +279,12 @@ BATT_SMBUS::cycle() // convert millivolts to volts new_report.voltage_v = ((float)tmp) / 1000.0f; - // To-Do: read current as block from BATT_SMBUS_CURRENT register + // read current + usleep(1); + uint8_t buff[4]; + if (read_block(BATT_SMBUS_CURRENT, buff, 4, false) == 4) { + new_report.current_a = (float)((int32_t)((uint32_t)buff[3]<<24 | (uint32_t)buff[2]<<16 | (uint32_t)buff[1]<<8 | (uint32_t)buff[0])) / 1000.0f; + } // publish to orb if (_batt_topic != -1) { @@ -301,12 +313,20 @@ BATT_SMBUS::cycle() int BATT_SMBUS::read_reg(uint8_t reg, uint16_t &val) { - uint8_t buff[2]; + uint8_t buff[3]; // 2 bytes of data + PEC // read from register - int ret = transfer(®, 1, buff, 2); + int ret = transfer(®, 1, buff, 3); if (ret == OK) { - val = (uint16_t)buff[1] << 8 | (uint16_t)buff[0]; + // check PEC + uint8_t pec = get_PEC(reg, true, buff, 2); + if (pec == buff[2]) { + val = (uint16_t)buff[1] << 8 | (uint16_t)buff[0]; + warnx("PEC ok: %x",(int)pec); + } else { + ret = ENOTTY; + warnx("PEC:%x vs MyPec:%x",(int)buff[2],(int)pec); + } } // return success or failure @@ -316,12 +336,12 @@ BATT_SMBUS::read_reg(uint8_t reg, uint16_t &val) // read_block - returns number of characters read if successful, zero if unsuccessful uint8_t BATT_SMBUS::read_block(uint8_t reg, uint8_t* data, uint8_t max_len, bool append_zero) { - uint8_t buff[max_len+1]; // buffer to hold results + uint8_t buff[max_len+2]; // buffer to hold results usleep(1); - // read bytes - int ret = transfer(®, 1,buff, max_len+1); + // read bytes including PEC + int ret = transfer(®, 1, buff, max_len+2); // return zero on failure if (ret != OK) { @@ -336,6 +356,16 @@ uint8_t BATT_SMBUS::read_block(uint8_t reg, uint8_t* data, uint8_t max_len, bool return 0; } + // check PEC + uint8_t pec = get_PEC(reg, true, buff, bufflen+1); + if (pec != buff[bufflen+1]) { + // debug + warnx("CurrPEC:%x vs MyPec:%x",(int)buff[bufflen+1],(int)pec); + return 0; + } else { + warnx("CurPEC ok: %x",(int)pec); + } + // copy data memcpy(data, &buff[1], bufflen); @@ -348,6 +378,46 @@ uint8_t BATT_SMBUS::read_block(uint8_t reg, uint8_t* data, uint8_t max_len, bool return bufflen; } +// get_PEC - calculate PEC for a read or write from the battery +// buff is the data that was read or will be written +uint8_t BATT_SMBUS::get_PEC(uint8_t cmd, bool reading, const uint8_t buff[], uint8_t len) const +{ + // exit immediately if no data + if (len <= 0) { + return 0; + } + + // prepare temp buffer for calcing crc + uint8_t tmp_buff[len+3]; + tmp_buff[0] = (uint8_t)get_address() << 1; + tmp_buff[1] = cmd; + tmp_buff[2] = tmp_buff[0] | (uint8_t)reading; + memcpy(&tmp_buff[3],buff,len); + + // initialise crc to zero + uint8_t crc = 0; + uint8_t shift_reg = 0; + bool do_invert; + + // for each byte in the stream + for (uint8_t i=0; i Date: Tue, 30 Dec 2014 12:14:54 +0900 Subject: [PATCH 281/652] batt_smbus: add search --- src/drivers/batt_smbus/batt_smbus.cpp | 41 +++++++++++++++++++++++++-- 1 file changed, 38 insertions(+), 3 deletions(-) diff --git a/src/drivers/batt_smbus/batt_smbus.cpp b/src/drivers/batt_smbus/batt_smbus.cpp index 4d4a84049d..dd83dacafc 100644 --- a/src/drivers/batt_smbus/batt_smbus.cpp +++ b/src/drivers/batt_smbus/batt_smbus.cpp @@ -77,6 +77,9 @@ #include #include +#define BATT_SMBUS_ADDR_MIN 0x08 /* lowest possible address */ +#define BATT_SMBUS_ADDR_MAX 0x7F /* highest possible address */ + #define BATT_SMBUS_I2C_BUS PX4_I2C_BUS_EXPANSION #define BATT_SMBUS_ADDR 0x0B /* I2C address */ #define BATT_SMBUS_TEMP 0x08 /* temperature register */ @@ -103,6 +106,7 @@ public: virtual int init(); virtual int test(); + int search(); /* search all possible slave addresses for a smart battery */ protected: virtual int probe(); // check if the device can be contacted @@ -226,6 +230,34 @@ BATT_SMBUS::test() return OK; } +/* search all possible slave addresses for a smart battery */ +int +BATT_SMBUS::search() +{ + bool found_slave = false; + uint16_t tmp; + + // search through all valid SMBus addresses + for (uint8_t i=BATT_SMBUS_ADDR_MIN; i<=BATT_SMBUS_ADDR_MAX; i++) { + set_address(i); + if (read_reg(BATT_SMBUS_VOLTAGE, tmp) == OK) { + warnx("battery found at 0x%x",(int)i); + found_slave = true; + } + // short sleep + usleep(1); + } + + // display completion message + if (found_slave) { + warnx("Done."); + } else { + warnx("No smart batteries found."); + } + + return OK; +} + int BATT_SMBUS::probe() { @@ -322,10 +354,8 @@ BATT_SMBUS::read_reg(uint8_t reg, uint16_t &val) uint8_t pec = get_PEC(reg, true, buff, 2); if (pec == buff[2]) { val = (uint16_t)buff[1] << 8 | (uint16_t)buff[0]; - warnx("PEC ok: %x",(int)pec); } else { ret = ENOTTY; - warnx("PEC:%x vs MyPec:%x",(int)buff[2],(int)pec); } } @@ -423,7 +453,7 @@ uint8_t BATT_SMBUS::get_PEC(uint8_t cmd, bool reading, const uint8_t buff[], uin void batt_smbus_usage() { - warnx("missing command: try 'start', 'test', 'stop'"); + warnx("missing command: try 'start', 'test', 'stop', 'search'"); warnx("options:"); warnx(" -b i2cbus (%d)", BATT_SMBUS_I2C_BUS); warnx(" -a addr (0x%x)", BATT_SMBUS_ADDR); @@ -500,6 +530,11 @@ batt_smbus_main(int argc, char *argv[]) exit(0); } + if (!strcmp(verb, "search")) { + g_batt_smbus->search(); + exit(0); + } + batt_smbus_usage(); exit(0); } From 7bed5b382f8c89a061970f94b61d610899475160 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 30 Dec 2014 10:28:35 +0100 Subject: [PATCH 282/652] Clean up docs in PX4FLOW driver --- src/drivers/px4flow/px4flow.cpp | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp index 62308fc654..7e3461ba10 100644 --- a/src/drivers/px4flow/px4flow.cpp +++ b/src/drivers/px4flow/px4flow.cpp @@ -73,13 +73,14 @@ #include /* Configuration Constants */ -#define I2C_FLOW_ADDRESS 0x42 //* 7-bit address. 8-bit address is 0x84 -//range 0x42 - 0x49 +#define I2C_FLOW_ADDRESS 0x42 ///< 7-bit address. 8-bit address is 0x84, range 0x42 - 0x49 /* PX4FLOW Registers addresses */ -#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 +#define PX4FLOW_I2C_MAX_BUS_SPEED 400000 ///< 400 KHz maximum speed -#define PX4FLOW_CONVERSION_INTERVAL 20000 //in microseconds! 20000 = 50 Hz 100000 = 10Hz /* oddly, ERROR is not defined for c++ */ #ifdef ERROR # undef ERROR @@ -200,7 +201,7 @@ 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 + I2C("PX4FLOW", PX4FLOW_DEVICE_PATH, bus, address, PX4FLOW_I2C_MAX_BUS_SPEED), /* 100-400 KHz */ _reports(nullptr), _sensor_ok(false), _measure_ticks(0), From d8ff76bf7ecd2c58939e9baf9ff6fb28180b124b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 30 Dec 2014 10:44:23 +0100 Subject: [PATCH 283/652] Fix parameter name for initial setup parameter config --- ROMFS/px4fmu_common/init.d/rc.fw_defaults | 2 +- ROMFS/px4fmu_common/init.d/rc.mc_defaults | 4 +++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_defaults b/ROMFS/px4fmu_common/init.d/rc.fw_defaults index fab2a7f187..e6de64054b 100644 --- a/ROMFS/px4fmu_common/init.d/rc.fw_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.fw_defaults @@ -10,7 +10,7 @@ then param set NAV_LAND_ALT 90 param set NAV_RTL_ALT 100 param set NAV_RTL_LAND_T -1 - param set NAV_ACCEPT_RAD 50 + param set NAV_ACC_RAD 50 param set FW_T_HRATE_P 0.01 param set FW_T_RLL2THR 15 param set FW_T_SRATE_P 0.01 diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_defaults b/ROMFS/px4fmu_common/init.d/rc.mc_defaults index 307a64c4d5..24d9650a0c 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.mc_defaults @@ -41,7 +41,9 @@ then param set PE_POSNE_NOISE 0.5 param set PE_POSD_NOISE 1.0 - param set NAV_ACCEPT_RAD 2.0 + param set NAV_ACC_RAD 2.0 + param set RTL_RETURN_ALT 30.0 + param set RTL_DESCEND_ALT 10.0 fi set PWM_RATE 400 From 4942883ddcb5d1a09e96335b1edbbf2d937937b4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 30 Dec 2014 10:45:09 +0100 Subject: [PATCH 284/652] RTL params: Set safer initial value for RTL altitude for all vehicles, improve dparam documentation --- src/modules/navigator/navigator_params.c | 8 +++++--- src/modules/navigator/rtl_params.c | 11 ++++++----- 2 files changed, 11 insertions(+), 8 deletions(-) diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c index 1f40e634ed..5f8f8d02f1 100644 --- a/src/modules/navigator/navigator_params.c +++ b/src/modules/navigator/navigator_params.c @@ -50,7 +50,8 @@ * Default value of loiter radius for missions, loiter, RTL, etc. (fixedwing only). * * @unit meters - * @min 0.0 + * @min 20 + * @max 200 * @group Mission */ PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 50.0f); @@ -61,10 +62,11 @@ PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 50.0f); * Default acceptance radius, overridden by acceptance radius of waypoint if set. * * @unit meters - * @min 1.0 + * @min 0.05 + * @max 200 * @group Mission */ -PARAM_DEFINE_FLOAT(NAV_ACC_RAD, 25.0f); +PARAM_DEFINE_FLOAT(NAV_ACC_RAD, 10.0f); /** * Set OBC mode for data link loss diff --git a/src/modules/navigator/rtl_params.c b/src/modules/navigator/rtl_params.c index bfe6ce7e18..1568233b05 100644 --- a/src/modules/navigator/rtl_params.c +++ b/src/modules/navigator/rtl_params.c @@ -53,7 +53,8 @@ * Default value of loiter radius after RTL (fixedwing only). * * @unit meters - * @min 0.0 + * @min 20 + * @max 200 * @group RTL */ PARAM_DEFINE_FLOAT(RTL_LOITER_RAD, 50.0f); @@ -65,10 +66,10 @@ PARAM_DEFINE_FLOAT(RTL_LOITER_RAD, 50.0f); * * @unit meters * @min 0 - * @max 1 + * @max 150 * @group RTL */ -PARAM_DEFINE_FLOAT(RTL_RETURN_ALT, 100); +PARAM_DEFINE_FLOAT(RTL_RETURN_ALT, 60); /** @@ -78,7 +79,7 @@ PARAM_DEFINE_FLOAT(RTL_RETURN_ALT, 100); * Land (i.e. slowly descend) from this altitude if autolanding allowed. * * @unit meters - * @min 0 + * @min 2 * @max 100 * @group RTL */ @@ -92,7 +93,7 @@ PARAM_DEFINE_FLOAT(RTL_DESCEND_ALT, 20); * * @unit seconds * @min -1 - * @max + * @max 300 * @group RTL */ PARAM_DEFINE_FLOAT(RTL_LAND_DELAY, -1.0f); From 5de80bbaf1e2d494304167fc7317abec919d9c41 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 30 Dec 2014 11:05:28 +0100 Subject: [PATCH 285/652] Clean up MC controller usage of VTOL topics --- .../mc_att_control/mc_att_control_main.cpp | 28 ++++++++++--------- 1 file changed, 15 insertions(+), 13 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 25b085b7b2..82d2ff23a5 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -126,8 +126,8 @@ private: orb_advert_t _v_rates_sp_pub; /**< rate setpoint publication */ orb_advert_t _actuators_0_pub; /**< attitude actuator controls publication */ - 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 + 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,8 +283,8 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _att_sp_pub(-1), _v_rates_sp_pub(-1), _actuators_0_pub(-1), - _rates_sp_id(ORB_ID(vehicle_rates_setpoint)), - _actuators_id(ORB_ID(actuator_controls_0)), + _rates_sp_id(0), + _actuators_id(0), _actuators_0_circuit_breaker_enabled(false), @@ -517,12 +517,14 @@ MulticopterAttitudeControl::vehicle_status_poll() if (vehicle_status_updated) { orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status); /* set correct uORB ID, depending on if vehicle is VTOL or not */ - if (_vehicle_status.is_vtol) { - _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); + if (!_rates_sp_id) { + if (_vehicle_status.is_vtol) { + _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); + } } } } @@ -837,7 +839,7 @@ MulticopterAttitudeControl::task_main() if (_v_rates_sp_pub > 0) { orb_publish(_rates_sp_id, _v_rates_sp_pub, &_v_rates_sp); - } else { + } else if (_rates_sp_id) { _v_rates_sp_pub = orb_advertise(_rates_sp_id, &_v_rates_sp); } @@ -861,7 +863,7 @@ MulticopterAttitudeControl::task_main() if (_v_rates_sp_pub > 0) { orb_publish(_rates_sp_id, _v_rates_sp_pub, &_v_rates_sp); - } else { + } else if (_rates_sp_id) { _v_rates_sp_pub = orb_advertise(_rates_sp_id, &_v_rates_sp); } @@ -889,7 +891,7 @@ MulticopterAttitudeControl::task_main() if (_actuators_0_pub > 0) { orb_publish(_actuators_id, _actuators_0_pub, &_actuators); - } else { + } else if (_actuators_id) { _actuators_0_pub = orb_advertise(_actuators_id, &_actuators); } From 32bfc6cdb8aef807259b1cd051c48d4ac90298e3 Mon Sep 17 00:00:00 2001 From: "M.H.Kabir" Date: Tue, 30 Dec 2014 16:32:16 +0530 Subject: [PATCH 286/652] Minor re-addition --- src/drivers/px4flow/px4flow.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp index 0367985431..9c9c1b0f81 100644 --- a/src/drivers/px4flow/px4flow.cpp +++ b/src/drivers/px4flow/px4flow.cpp @@ -364,6 +364,9 @@ PX4FLOW::ioctl(struct file *filp, int cmd, unsigned long arg) return OK; } + case SENSORIOCGQUEUEDEPTH: + return _reports->size(); + case SENSORIOCSROTATION: _sensor_rotation = (enum Rotation)arg; return OK; From c82996850be2df523394c2ac49db55258819a8a3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 30 Dec 2014 12:20:23 +0100 Subject: [PATCH 287/652] Update EKF estimator to most recent version from Paul Riseborough --- .../ekf_att_pos_estimator_main.cpp | 19 +- .../estimator_21states.cpp | 2142 ----------------- .../estimator_21states.h | 247 -- ...or_23states.cpp => estimator_22states.cpp} | 921 ++++--- ...imator_23states.h => estimator_22states.h} | 42 +- src/modules/ekf_att_pos_estimator/module.mk | 2 +- 6 files changed, 487 insertions(+), 2886 deletions(-) delete mode 100644 src/modules/ekf_att_pos_estimator/estimator_21states.cpp delete mode 100644 src/modules/ekf_att_pos_estimator/estimator_21states.h rename src/modules/ekf_att_pos_estimator/{estimator_23states.cpp => estimator_22states.cpp} (79%) rename src/modules/ekf_att_pos_estimator/{estimator_23states.h => estimator_22states.h} (91%) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index e7805daa91..9e32045ac3 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -84,7 +84,7 @@ #include #include -#include "estimator_23states.h" +#include "estimator_22states.h" @@ -1371,10 +1371,15 @@ FixedwingEstimator::task_main() } if (newRangeData) { - _ekf->fuseRngData = true; - _ekf->useRangeFinder = true; - _ekf->RecallStates(_ekf->statesAtRngTime, (IMUmsec - 100.0f)); - _ekf->GroundEKF(); + + if (_ekf->Tnb.z.z > 0.9f) { + // _ekf->rngMea is set in sensor readout already + _ekf->fuseRngData = true; + _ekf->fuseOptFlowData = false; + _ekf->RecallStates(_ekf->statesAtRngTime, (IMUmsec - 100.0f)); + _ekf->OpticalFlowEKF(); + _ekf->fuseRngData = false; + } } @@ -1514,8 +1519,8 @@ FixedwingEstimator::task_main() if (hrt_elapsed_time(&_wind.timestamp) > 99000) { _wind.timestamp = _global_pos.timestamp; - _wind.windspeed_north = _ekf->windSpdFiltNorth; - _wind.windspeed_east = _ekf->windSpdFiltEast; + _wind.windspeed_north = _ekf->states[14]; + _wind.windspeed_east = _ekf->states[15]; // XXX we need to do something smart about the covariance here // but we default to the estimate covariance for now _wind.covariance_north = _ekf->P[14][14]; diff --git a/src/modules/ekf_att_pos_estimator/estimator_21states.cpp b/src/modules/ekf_att_pos_estimator/estimator_21states.cpp deleted file mode 100644 index 67bfec4ea3..0000000000 --- a/src/modules/ekf_att_pos_estimator/estimator_21states.cpp +++ /dev/null @@ -1,2142 +0,0 @@ -#include "estimator_21states.h" - -#include - -AttPosEKF::AttPosEKF() : - fusionModeGPS(0), - covSkipCount(0), - EAS2TAS(1.0f), - statesInitialised(false), - fuseVelData(false), - fusePosData(false), - fuseHgtData(false), - fuseMagData(false), - fuseVtasData(false), - onGround(true), - staticMode(true), - useAirspeed(true), - useCompass(true), - numericalProtection(true), - storeIndex(0), - magDeclination(0.0f) -{ - InitialiseParameters(); -} - -AttPosEKF::~AttPosEKF() -{ -} - -void AttPosEKF::UpdateStrapdownEquationsNED() -{ - Vector3f delVelNav; - float q00; - float q11; - float q22; - float q33; - float q01; - float q02; - float q03; - float q12; - float q13; - float q23; - Mat3f Tbn; - Mat3f Tnb; - float rotationMag; - float qUpdated[4]; - float quatMag; - double deltaQuat[4]; - const Vector3f gravityNED = {0.0,0.0,GRAVITY_MSS}; - -// Remove sensor bias errors - correctedDelAng.x = dAngIMU.x - states[10]; - correctedDelAng.y = dAngIMU.y - states[11]; - correctedDelAng.z = dAngIMU.z - states[12]; - dVelIMU.x = dVelIMU.x; - dVelIMU.y = dVelIMU.y; - dVelIMU.z = dVelIMU.z; - -// Save current measurements - Vector3f prevDelAng = correctedDelAng; - -// Apply corrections for earths rotation rate and coning errors -// * and + operators have been overloaded - correctedDelAng = correctedDelAng - Tnb*earthRateNED*dtIMU + 8.333333333333333e-2f*(prevDelAng % correctedDelAng); -// Convert the rotation vector to its equivalent quaternion - rotationMag = correctedDelAng.length(); - if (rotationMag < 1e-12f) - { - deltaQuat[0] = 1.0; - deltaQuat[1] = 0.0; - deltaQuat[2] = 0.0; - deltaQuat[3] = 0.0; - } - else - { - deltaQuat[0] = cos(0.5f*rotationMag); - double rotScaler = (sin(0.5f*rotationMag))/rotationMag; - deltaQuat[1] = correctedDelAng.x*rotScaler; - deltaQuat[2] = correctedDelAng.y*rotScaler; - deltaQuat[3] = correctedDelAng.z*rotScaler; - } - -// Update the quaternions by rotating from the previous attitude through -// the delta angle rotation quaternion - qUpdated[0] = states[0]*deltaQuat[0] - states[1]*deltaQuat[1] - states[2]*deltaQuat[2] - states[3]*deltaQuat[3]; - qUpdated[1] = states[0]*deltaQuat[1] + states[1]*deltaQuat[0] + states[2]*deltaQuat[3] - states[3]*deltaQuat[2]; - qUpdated[2] = states[0]*deltaQuat[2] + states[2]*deltaQuat[0] + states[3]*deltaQuat[1] - states[1]*deltaQuat[3]; - qUpdated[3] = states[0]*deltaQuat[3] + states[3]*deltaQuat[0] + states[1]*deltaQuat[2] - states[2]*deltaQuat[1]; - -// Normalise the quaternions and update the quaternion states - quatMag = sqrtf(sq(qUpdated[0]) + sq(qUpdated[1]) + sq(qUpdated[2]) + sq(qUpdated[3])); - if (quatMag > 1e-16f) - { - float quatMagInv = 1.0f/quatMag; - states[0] = quatMagInv*qUpdated[0]; - states[1] = quatMagInv*qUpdated[1]; - states[2] = quatMagInv*qUpdated[2]; - states[3] = quatMagInv*qUpdated[3]; - } - -// Calculate the body to nav cosine matrix - q00 = sq(states[0]); - q11 = sq(states[1]); - q22 = sq(states[2]); - q33 = sq(states[3]); - q01 = states[0]*states[1]; - q02 = states[0]*states[2]; - q03 = states[0]*states[3]; - q12 = states[1]*states[2]; - q13 = states[1]*states[3]; - q23 = states[2]*states[3]; - - Tbn.x.x = q00 + q11 - q22 - q33; - Tbn.y.y = q00 - q11 + q22 - q33; - Tbn.z.z = q00 - q11 - q22 + q33; - Tbn.x.y = 2*(q12 - q03); - Tbn.x.z = 2*(q13 + q02); - Tbn.y.x = 2*(q12 + q03); - Tbn.y.z = 2*(q23 - q01); - Tbn.z.x = 2*(q13 - q02); - Tbn.z.y = 2*(q23 + q01); - - Tnb = Tbn.transpose(); - -// transform body delta velocities to delta velocities in the nav frame -// * and + operators have been overloaded - //delVelNav = Tbn*dVelIMU + gravityNED*dtIMU; - delVelNav.x = Tbn.x.x*dVelIMU.x + Tbn.x.y*dVelIMU.y + Tbn.x.z*dVelIMU.z + gravityNED.x*dtIMU; - delVelNav.y = Tbn.y.x*dVelIMU.x + Tbn.y.y*dVelIMU.y + Tbn.y.z*dVelIMU.z + gravityNED.y*dtIMU; - delVelNav.z = Tbn.z.x*dVelIMU.x + Tbn.z.y*dVelIMU.y + Tbn.z.z*dVelIMU.z + gravityNED.z*dtIMU; - -// calculate the magnitude of the nav acceleration (required for GPS -// variance estimation) - accNavMag = delVelNav.length()/dtIMU; - -// If calculating position save previous velocity - float lastVelocity[3]; - lastVelocity[0] = states[4]; - lastVelocity[1] = states[5]; - lastVelocity[2] = states[6]; - -// Sum delta velocities to get velocity - states[4] = states[4] + delVelNav.x; - states[5] = states[5] + delVelNav.y; - states[6] = states[6] + delVelNav.z; - -// If calculating postions, do a trapezoidal integration for position - states[7] = states[7] + 0.5f*(states[4] + lastVelocity[0])*dtIMU; - states[8] = states[8] + 0.5f*(states[5] + lastVelocity[1])*dtIMU; - states[9] = states[9] + 0.5f*(states[6] + lastVelocity[2])*dtIMU; - - // Constrain states (to protect against filter divergence) - //ConstrainStates(); -} - -void AttPosEKF::CovariancePrediction(float dt) -{ - // scalars - float daxCov; - float dayCov; - float dazCov; - float dvxCov; - float dvyCov; - float dvzCov; - float dvx; - float dvy; - float dvz; - float dax; - float day; - float daz; - float q0; - float q1; - float q2; - float q3; - float dax_b; - float day_b; - float daz_b; - - // arrays - float processNoise[21]; - float SF[14]; - float SG[8]; - float SQ[11]; - float SPP[13] = {0}; - float nextP[21][21]; - - // calculate covariance prediction process noise - windVelSigma = dt*0.1f; - dAngBiasSigma = dt*5.0e-7f; - magEarthSigma = dt*3.0e-4f; - magBodySigma = dt*3.0e-4f; - for (uint8_t i= 0; i<=9; i++) processNoise[i] = 1.0e-9f; - for (uint8_t i=10; i<=12; i++) processNoise[i] = dAngBiasSigma; - if (onGround) processNoise[12] = dAngBiasSigma * yawVarScale; - for (uint8_t i=13; i<=14; i++) processNoise[i] = windVelSigma; - for (uint8_t i=15; i<=17; i++) processNoise[i] = magEarthSigma; - for (uint8_t i=18; i<=20; i++) processNoise[i] = magBodySigma; - for (uint8_t i= 0; i<=20; i++) processNoise[i] = sq(processNoise[i]); - - // set variables used to calculate covariance growth - dvx = summedDelVel.x; - dvy = summedDelVel.y; - dvz = summedDelVel.z; - dax = summedDelAng.x; - day = summedDelAng.y; - daz = summedDelAng.z; - q0 = states[0]; - q1 = states[1]; - q2 = states[2]; - q3 = states[3]; - dax_b = states[10]; - day_b = states[11]; - daz_b = states[12]; - daxCov = sq(dt*1.4544411e-2f); - dayCov = sq(dt*1.4544411e-2f); - dazCov = sq(dt*1.4544411e-2f); - if (onGround) dazCov = dazCov * sq(yawVarScale); - dvxCov = sq(dt*0.5f); - dvyCov = sq(dt*0.5f); - dvzCov = sq(dt*0.5f); - - // Predicted covariance calculation - SF[0] = 2*dvx*q1 + 2*dvy*q2 + 2*dvz*q3; - SF[1] = 2*dvx*q3 + 2*dvy*q0 - 2*dvz*q1; - SF[2] = 2*dvx*q0 - 2*dvy*q3 + 2*dvz*q2; - SF[3] = day/2 - day_b/2; - SF[4] = daz/2 - daz_b/2; - SF[5] = dax/2 - dax_b/2; - SF[6] = dax_b/2 - dax/2; - SF[7] = daz_b/2 - daz/2; - SF[8] = day_b/2 - day/2; - SF[9] = q1/2; - SF[10] = q2/2; - SF[11] = q3/2; - SF[12] = 2*dvz*q0; - SF[13] = 2*dvy*q1; - - SG[0] = q0/2; - SG[1] = sq(q3); - SG[2] = sq(q2); - SG[3] = sq(q1); - SG[4] = sq(q0); - SG[5] = 2*q2*q3; - SG[6] = 2*q1*q3; - SG[7] = 2*q1*q2; - - SQ[0] = dvzCov*(SG[5] - 2*q0*q1)*(SG[1] - SG[2] - SG[3] + SG[4]) - dvyCov*(SG[5] + 2*q0*q1)*(SG[1] - SG[2] + SG[3] - SG[4]) + dvxCov*(SG[6] - 2*q0*q2)*(SG[7] + 2*q0*q3); - SQ[1] = dvzCov*(SG[6] + 2*q0*q2)*(SG[1] - SG[2] - SG[3] + SG[4]) - dvxCov*(SG[6] - 2*q0*q2)*(SG[1] + SG[2] - SG[3] - SG[4]) + dvyCov*(SG[5] + 2*q0*q1)*(SG[7] - 2*q0*q3); - SQ[2] = dvzCov*(SG[5] - 2*q0*q1)*(SG[6] + 2*q0*q2) - dvyCov*(SG[7] - 2*q0*q3)*(SG[1] - SG[2] + SG[3] - SG[4]) - dvxCov*(SG[7] + 2*q0*q3)*(SG[1] + SG[2] - SG[3] - SG[4]); - SQ[3] = (dayCov*q1*SG[0])/2 - (dazCov*q1*SG[0])/2 - (daxCov*q2*q3)/4; - SQ[4] = (dazCov*q2*SG[0])/2 - (daxCov*q2*SG[0])/2 - (dayCov*q1*q3)/4; - SQ[5] = (daxCov*q3*SG[0])/2 - (dayCov*q3*SG[0])/2 - (dazCov*q1*q2)/4; - SQ[6] = (daxCov*q1*q2)/4 - (dazCov*q3*SG[0])/2 - (dayCov*q1*q2)/4; - SQ[7] = (dazCov*q1*q3)/4 - (daxCov*q1*q3)/4 - (dayCov*q2*SG[0])/2; - SQ[8] = (dayCov*q2*q3)/4 - (daxCov*q1*SG[0])/2 - (dazCov*q2*q3)/4; - SQ[9] = sq(SG[0]); - SQ[10] = sq(q1); - - SPP[0] = SF[12] + SF[13] - 2*dvx*q2; - SPP[1] = 2*dvx*q0 - 2*dvy*q3 + 2*dvz*q2; - SPP[2] = 2*dvx*q3 + 2*dvy*q0 - 2*dvz*q1; - SPP[3] = SF[11]; - SPP[4] = SF[10]; - SPP[5] = SF[9]; - SPP[6] = SF[7]; - SPP[7] = SF[8]; - - nextP[0][0] = P[0][0] + P[1][0]*SF[6] + P[2][0]*SPP[7] + P[3][0]*SPP[6] + P[10][0]*SPP[5] + P[11][0]*SPP[4] + P[12][0]*SPP[3] + (daxCov*SQ[10])/4 + SF[6]*(P[0][1] + P[1][1]*SF[6] + P[2][1]*SPP[7] + P[3][1]*SPP[6] + P[10][1]*SPP[5] + P[11][1]*SPP[4] + P[12][1]*SPP[3]) + SPP[7]*(P[0][2] + P[1][2]*SF[6] + P[2][2]*SPP[7] + P[3][2]*SPP[6] + P[10][2]*SPP[5] + P[11][2]*SPP[4] + P[12][2]*SPP[3]) + SPP[6]*(P[0][3] + P[1][3]*SF[6] + P[2][3]*SPP[7] + P[3][3]*SPP[6] + P[10][3]*SPP[5] + P[11][3]*SPP[4] + P[12][3]*SPP[3]) + SPP[5]*(P[0][10] + P[1][10]*SF[6] + P[2][10]*SPP[7] + P[3][10]*SPP[6] + P[10][10]*SPP[5] + P[11][10]*SPP[4] + P[12][10]*SPP[3]) + SPP[4]*(P[0][11] + P[1][11]*SF[6] + P[2][11]*SPP[7] + P[3][11]*SPP[6] + P[10][11]*SPP[5] + P[11][11]*SPP[4] + P[12][11]*SPP[3]) + SPP[3]*(P[0][12] + P[1][12]*SF[6] + P[2][12]*SPP[7] + P[3][12]*SPP[6] + P[10][12]*SPP[5] + P[11][12]*SPP[4] + P[12][12]*SPP[3]) + (dayCov*sq(q2))/4 + (dazCov*sq(q3))/4; - nextP[0][1] = P[0][1] + SQ[8] + P[1][1]*SF[6] + P[2][1]*SPP[7] + P[3][1]*SPP[6] + P[10][1]*SPP[5] + P[11][1]*SPP[4] + P[12][1]*SPP[3] + SF[5]*(P[0][0] + P[1][0]*SF[6] + P[2][0]*SPP[7] + P[3][0]*SPP[6] + P[10][0]*SPP[5] + P[11][0]*SPP[4] + P[12][0]*SPP[3]) + SF[4]*(P[0][2] + P[1][2]*SF[6] + P[2][2]*SPP[7] + P[3][2]*SPP[6] + P[10][2]*SPP[5] + P[11][2]*SPP[4] + P[12][2]*SPP[3]) + SPP[7]*(P[0][3] + P[1][3]*SF[6] + P[2][3]*SPP[7] + P[3][3]*SPP[6] + P[10][3]*SPP[5] + P[11][3]*SPP[4] + P[12][3]*SPP[3]) + SPP[3]*(P[0][11] + P[1][11]*SF[6] + P[2][11]*SPP[7] + P[3][11]*SPP[6] + P[10][11]*SPP[5] + P[11][11]*SPP[4] + P[12][11]*SPP[3]) - SPP[4]*(P[0][12] + P[1][12]*SF[6] + P[2][12]*SPP[7] + P[3][12]*SPP[6] + P[10][12]*SPP[5] + P[11][12]*SPP[4] + P[12][12]*SPP[3]) - (q0*(P[0][10] + P[1][10]*SF[6] + P[2][10]*SPP[7] + P[3][10]*SPP[6] + P[10][10]*SPP[5] + P[11][10]*SPP[4] + P[12][10]*SPP[3]))/2; - nextP[0][2] = P[0][2] + SQ[7] + P[1][2]*SF[6] + P[2][2]*SPP[7] + P[3][2]*SPP[6] + P[10][2]*SPP[5] + P[11][2]*SPP[4] + P[12][2]*SPP[3] + SF[3]*(P[0][0] + P[1][0]*SF[6] + P[2][0]*SPP[7] + P[3][0]*SPP[6] + P[10][0]*SPP[5] + P[11][0]*SPP[4] + P[12][0]*SPP[3]) + SF[5]*(P[0][3] + P[1][3]*SF[6] + P[2][3]*SPP[7] + P[3][3]*SPP[6] + P[10][3]*SPP[5] + P[11][3]*SPP[4] + P[12][3]*SPP[3]) + SPP[6]*(P[0][1] + P[1][1]*SF[6] + P[2][1]*SPP[7] + P[3][1]*SPP[6] + P[10][1]*SPP[5] + P[11][1]*SPP[4] + P[12][1]*SPP[3]) - SPP[3]*(P[0][10] + P[1][10]*SF[6] + P[2][10]*SPP[7] + P[3][10]*SPP[6] + P[10][10]*SPP[5] + P[11][10]*SPP[4] + P[12][10]*SPP[3]) + SPP[5]*(P[0][12] + P[1][12]*SF[6] + P[2][12]*SPP[7] + P[3][12]*SPP[6] + P[10][12]*SPP[5] + P[11][12]*SPP[4] + P[12][12]*SPP[3]) - (q0*(P[0][11] + P[1][11]*SF[6] + P[2][11]*SPP[7] + P[3][11]*SPP[6] + P[10][11]*SPP[5] + P[11][11]*SPP[4] + P[12][11]*SPP[3]))/2; - nextP[0][3] = P[0][3] + SQ[6] + P[1][3]*SF[6] + P[2][3]*SPP[7] + P[3][3]*SPP[6] + P[10][3]*SPP[5] + P[11][3]*SPP[4] + P[12][3]*SPP[3] + SF[4]*(P[0][0] + P[1][0]*SF[6] + P[2][0]*SPP[7] + P[3][0]*SPP[6] + P[10][0]*SPP[5] + P[11][0]*SPP[4] + P[12][0]*SPP[3]) + SF[3]*(P[0][1] + P[1][1]*SF[6] + P[2][1]*SPP[7] + P[3][1]*SPP[6] + P[10][1]*SPP[5] + P[11][1]*SPP[4] + P[12][1]*SPP[3]) + SF[6]*(P[0][2] + P[1][2]*SF[6] + P[2][2]*SPP[7] + P[3][2]*SPP[6] + P[10][2]*SPP[5] + P[11][2]*SPP[4] + P[12][2]*SPP[3]) + SPP[4]*(P[0][10] + P[1][10]*SF[6] + P[2][10]*SPP[7] + P[3][10]*SPP[6] + P[10][10]*SPP[5] + P[11][10]*SPP[4] + P[12][10]*SPP[3]) - SPP[5]*(P[0][11] + P[1][11]*SF[6] + P[2][11]*SPP[7] + P[3][11]*SPP[6] + P[10][11]*SPP[5] + P[11][11]*SPP[4] + P[12][11]*SPP[3]) - (q0*(P[0][12] + P[1][12]*SF[6] + P[2][12]*SPP[7] + P[3][12]*SPP[6] + P[10][12]*SPP[5] + P[11][12]*SPP[4] + P[12][12]*SPP[3]))/2; - nextP[0][4] = P[0][4] + P[1][4]*SF[6] + P[2][4]*SPP[7] + P[3][4]*SPP[6] + P[10][4]*SPP[5] + P[11][4]*SPP[4] + P[12][4]*SPP[3] + SF[2]*(P[0][0] + P[1][0]*SF[6] + P[2][0]*SPP[7] + P[3][0]*SPP[6] + P[10][0]*SPP[5] + P[11][0]*SPP[4] + P[12][0]*SPP[3]) + SF[0]*(P[0][1] + P[1][1]*SF[6] + P[2][1]*SPP[7] + P[3][1]*SPP[6] + P[10][1]*SPP[5] + P[11][1]*SPP[4] + P[12][1]*SPP[3]) + SPP[0]*(P[0][2] + P[1][2]*SF[6] + P[2][2]*SPP[7] + P[3][2]*SPP[6] + P[10][2]*SPP[5] + P[11][2]*SPP[4] + P[12][2]*SPP[3]) - SPP[2]*(P[0][3] + P[1][3]*SF[6] + P[2][3]*SPP[7] + P[3][3]*SPP[6] + P[10][3]*SPP[5] + P[11][3]*SPP[4] + P[12][3]*SPP[3]); - nextP[0][5] = P[0][5] + P[1][5]*SF[6] + P[2][5]*SPP[7] + P[3][5]*SPP[6] + P[10][5]*SPP[5] + P[11][5]*SPP[4] + P[12][5]*SPP[3] + SF[1]*(P[0][0] + P[1][0]*SF[6] + P[2][0]*SPP[7] + P[3][0]*SPP[6] + P[10][0]*SPP[5] + P[11][0]*SPP[4] + P[12][0]*SPP[3]) + SF[0]*(P[0][2] + P[1][2]*SF[6] + P[2][2]*SPP[7] + P[3][2]*SPP[6] + P[10][2]*SPP[5] + P[11][2]*SPP[4] + P[12][2]*SPP[3]) + SF[2]*(P[0][3] + P[1][3]*SF[6] + P[2][3]*SPP[7] + P[3][3]*SPP[6] + P[10][3]*SPP[5] + P[11][3]*SPP[4] + P[12][3]*SPP[3]) - SPP[0]*(P[0][1] + P[1][1]*SF[6] + P[2][1]*SPP[7] + P[3][1]*SPP[6] + P[10][1]*SPP[5] + P[11][1]*SPP[4] + P[12][1]*SPP[3]); - nextP[0][6] = P[0][6] + P[1][6]*SF[6] + P[2][6]*SPP[7] + P[3][6]*SPP[6] + P[10][6]*SPP[5] + P[11][6]*SPP[4] + P[12][6]*SPP[3] + SF[1]*(P[0][1] + P[1][1]*SF[6] + P[2][1]*SPP[7] + P[3][1]*SPP[6] + P[10][1]*SPP[5] + P[11][1]*SPP[4] + P[12][1]*SPP[3]) + SF[0]*(P[0][3] + P[1][3]*SF[6] + P[2][3]*SPP[7] + P[3][3]*SPP[6] + P[10][3]*SPP[5] + P[11][3]*SPP[4] + P[12][3]*SPP[3]) + SPP[0]*(P[0][0] + P[1][0]*SF[6] + P[2][0]*SPP[7] + P[3][0]*SPP[6] + P[10][0]*SPP[5] + P[11][0]*SPP[4] + P[12][0]*SPP[3]) - SPP[1]*(P[0][2] + P[1][2]*SF[6] + P[2][2]*SPP[7] + P[3][2]*SPP[6] + P[10][2]*SPP[5] + P[11][2]*SPP[4] + P[12][2]*SPP[3]); - nextP[0][7] = P[0][7] + P[1][7]*SF[6] + P[2][7]*SPP[7] + P[3][7]*SPP[6] + P[10][7]*SPP[5] + P[11][7]*SPP[4] + P[12][7]*SPP[3] + dt*(P[0][4] + P[1][4]*SF[6] + P[2][4]*SPP[7] + P[3][4]*SPP[6] + P[10][4]*SPP[5] + P[11][4]*SPP[4] + P[12][4]*SPP[3]); - nextP[0][8] = P[0][8] + P[1][8]*SF[6] + P[2][8]*SPP[7] + P[3][8]*SPP[6] + P[10][8]*SPP[5] + P[11][8]*SPP[4] + P[12][8]*SPP[3] + dt*(P[0][5] + P[1][5]*SF[6] + P[2][5]*SPP[7] + P[3][5]*SPP[6] + P[10][5]*SPP[5] + P[11][5]*SPP[4] + P[12][5]*SPP[3]); - nextP[0][9] = P[0][9] + P[1][9]*SF[6] + P[2][9]*SPP[7] + P[3][9]*SPP[6] + P[10][9]*SPP[5] + P[11][9]*SPP[4] + P[12][9]*SPP[3] + dt*(P[0][6] + P[1][6]*SF[6] + P[2][6]*SPP[7] + P[3][6]*SPP[6] + P[10][6]*SPP[5] + P[11][6]*SPP[4] + P[12][6]*SPP[3]); - nextP[0][10] = P[0][10] + P[1][10]*SF[6] + P[2][10]*SPP[7] + P[3][10]*SPP[6] + P[10][10]*SPP[5] + P[11][10]*SPP[4] + P[12][10]*SPP[3]; - nextP[0][11] = P[0][11] + P[1][11]*SF[6] + P[2][11]*SPP[7] + P[3][11]*SPP[6] + P[10][11]*SPP[5] + P[11][11]*SPP[4] + P[12][11]*SPP[3]; - nextP[0][12] = P[0][12] + P[1][12]*SF[6] + P[2][12]*SPP[7] + P[3][12]*SPP[6] + P[10][12]*SPP[5] + P[11][12]*SPP[4] + P[12][12]*SPP[3]; - nextP[0][13] = P[0][13] + P[1][13]*SF[6] + P[2][13]*SPP[7] + P[3][13]*SPP[6] + P[10][13]*SPP[5] + P[11][13]*SPP[4] + P[12][13]*SPP[3]; - nextP[0][14] = P[0][14] + P[1][14]*SF[6] + P[2][14]*SPP[7] + P[3][14]*SPP[6] + P[10][14]*SPP[5] + P[11][14]*SPP[4] + P[12][14]*SPP[3]; - nextP[0][15] = P[0][15] + P[1][15]*SF[6] + P[2][15]*SPP[7] + P[3][15]*SPP[6] + P[10][15]*SPP[5] + P[11][15]*SPP[4] + P[12][15]*SPP[3]; - nextP[0][16] = P[0][16] + P[1][16]*SF[6] + P[2][16]*SPP[7] + P[3][16]*SPP[6] + P[10][16]*SPP[5] + P[11][16]*SPP[4] + P[12][16]*SPP[3]; - nextP[0][17] = P[0][17] + P[1][17]*SF[6] + P[2][17]*SPP[7] + P[3][17]*SPP[6] + P[10][17]*SPP[5] + P[11][17]*SPP[4] + P[12][17]*SPP[3]; - nextP[0][18] = P[0][18] + P[1][18]*SF[6] + P[2][18]*SPP[7] + P[3][18]*SPP[6] + P[10][18]*SPP[5] + P[11][18]*SPP[4] + P[12][18]*SPP[3]; - nextP[0][19] = P[0][19] + P[1][19]*SF[6] + P[2][19]*SPP[7] + P[3][19]*SPP[6] + P[10][19]*SPP[5] + P[11][19]*SPP[4] + P[12][19]*SPP[3]; - nextP[0][20] = P[0][20] + P[1][20]*SF[6] + P[2][20]*SPP[7] + P[3][20]*SPP[6] + P[10][20]*SPP[5] + P[11][20]*SPP[4] + P[12][20]*SPP[3]; - nextP[1][0] = P[1][0] + SQ[8] + P[0][0]*SF[5] + P[2][0]*SF[4] + P[3][0]*SPP[7] + P[11][0]*SPP[3] - P[12][0]*SPP[4] - (P[10][0]*q0)/2 + SF[6]*(P[1][1] + P[0][1]*SF[5] + P[2][1]*SF[4] + P[3][1]*SPP[7] + P[11][1]*SPP[3] - P[12][1]*SPP[4] - (P[10][1]*q0)/2) + SPP[7]*(P[1][2] + P[0][2]*SF[5] + P[2][2]*SF[4] + P[3][2]*SPP[7] + P[11][2]*SPP[3] - P[12][2]*SPP[4] - (P[10][2]*q0)/2) + SPP[6]*(P[1][3] + P[0][3]*SF[5] + P[2][3]*SF[4] + P[3][3]*SPP[7] + P[11][3]*SPP[3] - P[12][3]*SPP[4] - (P[10][3]*q0)/2) + SPP[5]*(P[1][10] + P[0][10]*SF[5] + P[2][10]*SF[4] + P[3][10]*SPP[7] + P[11][10]*SPP[3] - P[12][10]*SPP[4] - (P[10][10]*q0)/2) + SPP[4]*(P[1][11] + P[0][11]*SF[5] + P[2][11]*SF[4] + P[3][11]*SPP[7] + P[11][11]*SPP[3] - P[12][11]*SPP[4] - (P[10][11]*q0)/2) + SPP[3]*(P[1][12] + P[0][12]*SF[5] + P[2][12]*SF[4] + P[3][12]*SPP[7] + P[11][12]*SPP[3] - P[12][12]*SPP[4] - (P[10][12]*q0)/2); - nextP[1][1] = P[1][1] + P[0][1]*SF[5] + P[2][1]*SF[4] + P[3][1]*SPP[7] + P[11][1]*SPP[3] - P[12][1]*SPP[4] + daxCov*SQ[9] - (P[10][1]*q0)/2 + SF[5]*(P[1][0] + P[0][0]*SF[5] + P[2][0]*SF[4] + P[3][0]*SPP[7] + P[11][0]*SPP[3] - P[12][0]*SPP[4] - (P[10][0]*q0)/2) + SF[4]*(P[1][2] + P[0][2]*SF[5] + P[2][2]*SF[4] + P[3][2]*SPP[7] + P[11][2]*SPP[3] - P[12][2]*SPP[4] - (P[10][2]*q0)/2) + SPP[7]*(P[1][3] + P[0][3]*SF[5] + P[2][3]*SF[4] + P[3][3]*SPP[7] + P[11][3]*SPP[3] - P[12][3]*SPP[4] - (P[10][3]*q0)/2) + SPP[3]*(P[1][11] + P[0][11]*SF[5] + P[2][11]*SF[4] + P[3][11]*SPP[7] + P[11][11]*SPP[3] - P[12][11]*SPP[4] - (P[10][11]*q0)/2) - SPP[4]*(P[1][12] + P[0][12]*SF[5] + P[2][12]*SF[4] + P[3][12]*SPP[7] + P[11][12]*SPP[3] - P[12][12]*SPP[4] - (P[10][12]*q0)/2) + (dayCov*sq(q3))/4 + (dazCov*sq(q2))/4 - (q0*(P[1][10] + P[0][10]*SF[5] + P[2][10]*SF[4] + P[3][10]*SPP[7] + P[11][10]*SPP[3] - P[12][10]*SPP[4] - (P[10][10]*q0)/2))/2; - nextP[1][2] = P[1][2] + SQ[5] + P[0][2]*SF[5] + P[2][2]*SF[4] + P[3][2]*SPP[7] + P[11][2]*SPP[3] - P[12][2]*SPP[4] - (P[10][2]*q0)/2 + SF[3]*(P[1][0] + P[0][0]*SF[5] + P[2][0]*SF[4] + P[3][0]*SPP[7] + P[11][0]*SPP[3] - P[12][0]*SPP[4] - (P[10][0]*q0)/2) + SF[5]*(P[1][3] + P[0][3]*SF[5] + P[2][3]*SF[4] + P[3][3]*SPP[7] + P[11][3]*SPP[3] - P[12][3]*SPP[4] - (P[10][3]*q0)/2) + SPP[6]*(P[1][1] + P[0][1]*SF[5] + P[2][1]*SF[4] + P[3][1]*SPP[7] + P[11][1]*SPP[3] - P[12][1]*SPP[4] - (P[10][1]*q0)/2) - SPP[3]*(P[1][10] + P[0][10]*SF[5] + P[2][10]*SF[4] + P[3][10]*SPP[7] + P[11][10]*SPP[3] - P[12][10]*SPP[4] - (P[10][10]*q0)/2) + SPP[5]*(P[1][12] + P[0][12]*SF[5] + P[2][12]*SF[4] + P[3][12]*SPP[7] + P[11][12]*SPP[3] - P[12][12]*SPP[4] - (P[10][12]*q0)/2) - (q0*(P[1][11] + P[0][11]*SF[5] + P[2][11]*SF[4] + P[3][11]*SPP[7] + P[11][11]*SPP[3] - P[12][11]*SPP[4] - (P[10][11]*q0)/2))/2; - nextP[1][3] = P[1][3] + SQ[4] + P[0][3]*SF[5] + P[2][3]*SF[4] + P[3][3]*SPP[7] + P[11][3]*SPP[3] - P[12][3]*SPP[4] - (P[10][3]*q0)/2 + SF[4]*(P[1][0] + P[0][0]*SF[5] + P[2][0]*SF[4] + P[3][0]*SPP[7] + P[11][0]*SPP[3] - P[12][0]*SPP[4] - (P[10][0]*q0)/2) + SF[3]*(P[1][1] + P[0][1]*SF[5] + P[2][1]*SF[4] + P[3][1]*SPP[7] + P[11][1]*SPP[3] - P[12][1]*SPP[4] - (P[10][1]*q0)/2) + SF[6]*(P[1][2] + P[0][2]*SF[5] + P[2][2]*SF[4] + P[3][2]*SPP[7] + P[11][2]*SPP[3] - P[12][2]*SPP[4] - (P[10][2]*q0)/2) + SPP[4]*(P[1][10] + P[0][10]*SF[5] + P[2][10]*SF[4] + P[3][10]*SPP[7] + P[11][10]*SPP[3] - P[12][10]*SPP[4] - (P[10][10]*q0)/2) - SPP[5]*(P[1][11] + P[0][11]*SF[5] + P[2][11]*SF[4] + P[3][11]*SPP[7] + P[11][11]*SPP[3] - P[12][11]*SPP[4] - (P[10][11]*q0)/2) - (q0*(P[1][12] + P[0][12]*SF[5] + P[2][12]*SF[4] + P[3][12]*SPP[7] + P[11][12]*SPP[3] - P[12][12]*SPP[4] - (P[10][12]*q0)/2))/2; - nextP[1][4] = P[1][4] + P[0][4]*SF[5] + P[2][4]*SF[4] + P[3][4]*SPP[7] + P[11][4]*SPP[3] - P[12][4]*SPP[4] - (P[10][4]*q0)/2 + SF[2]*(P[1][0] + P[0][0]*SF[5] + P[2][0]*SF[4] + P[3][0]*SPP[7] + P[11][0]*SPP[3] - P[12][0]*SPP[4] - (P[10][0]*q0)/2) + SF[0]*(P[1][1] + P[0][1]*SF[5] + P[2][1]*SF[4] + P[3][1]*SPP[7] + P[11][1]*SPP[3] - P[12][1]*SPP[4] - (P[10][1]*q0)/2) + SPP[0]*(P[1][2] + P[0][2]*SF[5] + P[2][2]*SF[4] + P[3][2]*SPP[7] + P[11][2]*SPP[3] - P[12][2]*SPP[4] - (P[10][2]*q0)/2) - SPP[2]*(P[1][3] + P[0][3]*SF[5] + P[2][3]*SF[4] + P[3][3]*SPP[7] + P[11][3]*SPP[3] - P[12][3]*SPP[4] - (P[10][3]*q0)/2); - nextP[1][5] = P[1][5] + P[0][5]*SF[5] + P[2][5]*SF[4] + P[3][5]*SPP[7] + P[11][5]*SPP[3] - P[12][5]*SPP[4] - (P[10][5]*q0)/2 + SF[1]*(P[1][0] + P[0][0]*SF[5] + P[2][0]*SF[4] + P[3][0]*SPP[7] + P[11][0]*SPP[3] - P[12][0]*SPP[4] - (P[10][0]*q0)/2) + SF[0]*(P[1][2] + P[0][2]*SF[5] + P[2][2]*SF[4] + P[3][2]*SPP[7] + P[11][2]*SPP[3] - P[12][2]*SPP[4] - (P[10][2]*q0)/2) + SF[2]*(P[1][3] + P[0][3]*SF[5] + P[2][3]*SF[4] + P[3][3]*SPP[7] + P[11][3]*SPP[3] - P[12][3]*SPP[4] - (P[10][3]*q0)/2) - SPP[0]*(P[1][1] + P[0][1]*SF[5] + P[2][1]*SF[4] + P[3][1]*SPP[7] + P[11][1]*SPP[3] - P[12][1]*SPP[4] - (P[10][1]*q0)/2); - nextP[1][6] = P[1][6] + P[0][6]*SF[5] + P[2][6]*SF[4] + P[3][6]*SPP[7] + P[11][6]*SPP[3] - P[12][6]*SPP[4] - (P[10][6]*q0)/2 + SF[1]*(P[1][1] + P[0][1]*SF[5] + P[2][1]*SF[4] + P[3][1]*SPP[7] + P[11][1]*SPP[3] - P[12][1]*SPP[4] - (P[10][1]*q0)/2) + SF[0]*(P[1][3] + P[0][3]*SF[5] + P[2][3]*SF[4] + P[3][3]*SPP[7] + P[11][3]*SPP[3] - P[12][3]*SPP[4] - (P[10][3]*q0)/2) + SPP[0]*(P[1][0] + P[0][0]*SF[5] + P[2][0]*SF[4] + P[3][0]*SPP[7] + P[11][0]*SPP[3] - P[12][0]*SPP[4] - (P[10][0]*q0)/2) - SPP[1]*(P[1][2] + P[0][2]*SF[5] + P[2][2]*SF[4] + P[3][2]*SPP[7] + P[11][2]*SPP[3] - P[12][2]*SPP[4] - (P[10][2]*q0)/2); - nextP[1][7] = P[1][7] + P[0][7]*SF[5] + P[2][7]*SF[4] + P[3][7]*SPP[7] + P[11][7]*SPP[3] - P[12][7]*SPP[4] - (P[10][7]*q0)/2 + dt*(P[1][4] + P[0][4]*SF[5] + P[2][4]*SF[4] + P[3][4]*SPP[7] + P[11][4]*SPP[3] - P[12][4]*SPP[4] - (P[10][4]*q0)/2); - nextP[1][8] = P[1][8] + P[0][8]*SF[5] + P[2][8]*SF[4] + P[3][8]*SPP[7] + P[11][8]*SPP[3] - P[12][8]*SPP[4] - (P[10][8]*q0)/2 + dt*(P[1][5] + P[0][5]*SF[5] + P[2][5]*SF[4] + P[3][5]*SPP[7] + P[11][5]*SPP[3] - P[12][5]*SPP[4] - (P[10][5]*q0)/2); - nextP[1][9] = P[1][9] + P[0][9]*SF[5] + P[2][9]*SF[4] + P[3][9]*SPP[7] + P[11][9]*SPP[3] - P[12][9]*SPP[4] - (P[10][9]*q0)/2 + dt*(P[1][6] + P[0][6]*SF[5] + P[2][6]*SF[4] + P[3][6]*SPP[7] + P[11][6]*SPP[3] - P[12][6]*SPP[4] - (P[10][6]*q0)/2); - nextP[1][10] = P[1][10] + P[0][10]*SF[5] + P[2][10]*SF[4] + P[3][10]*SPP[7] + P[11][10]*SPP[3] - P[12][10]*SPP[4] - (P[10][10]*q0)/2; - nextP[1][11] = P[1][11] + P[0][11]*SF[5] + P[2][11]*SF[4] + P[3][11]*SPP[7] + P[11][11]*SPP[3] - P[12][11]*SPP[4] - (P[10][11]*q0)/2; - nextP[1][12] = P[1][12] + P[0][12]*SF[5] + P[2][12]*SF[4] + P[3][12]*SPP[7] + P[11][12]*SPP[3] - P[12][12]*SPP[4] - (P[10][12]*q0)/2; - nextP[1][13] = P[1][13] + P[0][13]*SF[5] + P[2][13]*SF[4] + P[3][13]*SPP[7] + P[11][13]*SPP[3] - P[12][13]*SPP[4] - (P[10][13]*q0)/2; - nextP[1][14] = P[1][14] + P[0][14]*SF[5] + P[2][14]*SF[4] + P[3][14]*SPP[7] + P[11][14]*SPP[3] - P[12][14]*SPP[4] - (P[10][14]*q0)/2; - nextP[1][15] = P[1][15] + P[0][15]*SF[5] + P[2][15]*SF[4] + P[3][15]*SPP[7] + P[11][15]*SPP[3] - P[12][15]*SPP[4] - (P[10][15]*q0)/2; - nextP[1][16] = P[1][16] + P[0][16]*SF[5] + P[2][16]*SF[4] + P[3][16]*SPP[7] + P[11][16]*SPP[3] - P[12][16]*SPP[4] - (P[10][16]*q0)/2; - nextP[1][17] = P[1][17] + P[0][17]*SF[5] + P[2][17]*SF[4] + P[3][17]*SPP[7] + P[11][17]*SPP[3] - P[12][17]*SPP[4] - (P[10][17]*q0)/2; - nextP[1][18] = P[1][18] + P[0][18]*SF[5] + P[2][18]*SF[4] + P[3][18]*SPP[7] + P[11][18]*SPP[3] - P[12][18]*SPP[4] - (P[10][18]*q0)/2; - nextP[1][19] = P[1][19] + P[0][19]*SF[5] + P[2][19]*SF[4] + P[3][19]*SPP[7] + P[11][19]*SPP[3] - P[12][19]*SPP[4] - (P[10][19]*q0)/2; - nextP[1][20] = P[1][20] + P[0][20]*SF[5] + P[2][20]*SF[4] + P[3][20]*SPP[7] + P[11][20]*SPP[3] - P[12][20]*SPP[4] - (P[10][20]*q0)/2; - nextP[2][0] = P[2][0] + SQ[7] + P[0][0]*SF[3] + P[3][0]*SF[5] + P[1][0]*SPP[6] - P[10][0]*SPP[3] + P[12][0]*SPP[5] - (P[11][0]*q0)/2 + SF[6]*(P[2][1] + P[0][1]*SF[3] + P[3][1]*SF[5] + P[1][1]*SPP[6] - P[10][1]*SPP[3] + P[12][1]*SPP[5] - (P[11][1]*q0)/2) + SPP[7]*(P[2][2] + P[0][2]*SF[3] + P[3][2]*SF[5] + P[1][2]*SPP[6] - P[10][2]*SPP[3] + P[12][2]*SPP[5] - (P[11][2]*q0)/2) + SPP[6]*(P[2][3] + P[0][3]*SF[3] + P[3][3]*SF[5] + P[1][3]*SPP[6] - P[10][3]*SPP[3] + P[12][3]*SPP[5] - (P[11][3]*q0)/2) + SPP[5]*(P[2][10] + P[0][10]*SF[3] + P[3][10]*SF[5] + P[1][10]*SPP[6] - P[10][10]*SPP[3] + P[12][10]*SPP[5] - (P[11][10]*q0)/2) + SPP[4]*(P[2][11] + P[0][11]*SF[3] + P[3][11]*SF[5] + P[1][11]*SPP[6] - P[10][11]*SPP[3] + P[12][11]*SPP[5] - (P[11][11]*q0)/2) + SPP[3]*(P[2][12] + P[0][12]*SF[3] + P[3][12]*SF[5] + P[1][12]*SPP[6] - P[10][12]*SPP[3] + P[12][12]*SPP[5] - (P[11][12]*q0)/2); - nextP[2][1] = P[2][1] + SQ[5] + P[0][1]*SF[3] + P[3][1]*SF[5] + P[1][1]*SPP[6] - P[10][1]*SPP[3] + P[12][1]*SPP[5] - (P[11][1]*q0)/2 + SF[5]*(P[2][0] + P[0][0]*SF[3] + P[3][0]*SF[5] + P[1][0]*SPP[6] - P[10][0]*SPP[3] + P[12][0]*SPP[5] - (P[11][0]*q0)/2) + SF[4]*(P[2][2] + P[0][2]*SF[3] + P[3][2]*SF[5] + P[1][2]*SPP[6] - P[10][2]*SPP[3] + P[12][2]*SPP[5] - (P[11][2]*q0)/2) + SPP[7]*(P[2][3] + P[0][3]*SF[3] + P[3][3]*SF[5] + P[1][3]*SPP[6] - P[10][3]*SPP[3] + P[12][3]*SPP[5] - (P[11][3]*q0)/2) + SPP[3]*(P[2][11] + P[0][11]*SF[3] + P[3][11]*SF[5] + P[1][11]*SPP[6] - P[10][11]*SPP[3] + P[12][11]*SPP[5] - (P[11][11]*q0)/2) - SPP[4]*(P[2][12] + P[0][12]*SF[3] + P[3][12]*SF[5] + P[1][12]*SPP[6] - P[10][12]*SPP[3] + P[12][12]*SPP[5] - (P[11][12]*q0)/2) - (q0*(P[2][10] + P[0][10]*SF[3] + P[3][10]*SF[5] + P[1][10]*SPP[6] - P[10][10]*SPP[3] + P[12][10]*SPP[5] - (P[11][10]*q0)/2))/2; - nextP[2][2] = P[2][2] + P[0][2]*SF[3] + P[3][2]*SF[5] + P[1][2]*SPP[6] - P[10][2]*SPP[3] + P[12][2]*SPP[5] + dayCov*SQ[9] + (dazCov*SQ[10])/4 - (P[11][2]*q0)/2 + SF[3]*(P[2][0] + P[0][0]*SF[3] + P[3][0]*SF[5] + P[1][0]*SPP[6] - P[10][0]*SPP[3] + P[12][0]*SPP[5] - (P[11][0]*q0)/2) + SF[5]*(P[2][3] + P[0][3]*SF[3] + P[3][3]*SF[5] + P[1][3]*SPP[6] - P[10][3]*SPP[3] + P[12][3]*SPP[5] - (P[11][3]*q0)/2) + SPP[6]*(P[2][1] + P[0][1]*SF[3] + P[3][1]*SF[5] + P[1][1]*SPP[6] - P[10][1]*SPP[3] + P[12][1]*SPP[5] - (P[11][1]*q0)/2) - SPP[3]*(P[2][10] + P[0][10]*SF[3] + P[3][10]*SF[5] + P[1][10]*SPP[6] - P[10][10]*SPP[3] + P[12][10]*SPP[5] - (P[11][10]*q0)/2) + SPP[5]*(P[2][12] + P[0][12]*SF[3] + P[3][12]*SF[5] + P[1][12]*SPP[6] - P[10][12]*SPP[3] + P[12][12]*SPP[5] - (P[11][12]*q0)/2) + (daxCov*sq(q3))/4 - (q0*(P[2][11] + P[0][11]*SF[3] + P[3][11]*SF[5] + P[1][11]*SPP[6] - P[10][11]*SPP[3] + P[12][11]*SPP[5] - (P[11][11]*q0)/2))/2; - nextP[2][3] = P[2][3] + SQ[3] + P[0][3]*SF[3] + P[3][3]*SF[5] + P[1][3]*SPP[6] - P[10][3]*SPP[3] + P[12][3]*SPP[5] - (P[11][3]*q0)/2 + SF[4]*(P[2][0] + P[0][0]*SF[3] + P[3][0]*SF[5] + P[1][0]*SPP[6] - P[10][0]*SPP[3] + P[12][0]*SPP[5] - (P[11][0]*q0)/2) + SF[3]*(P[2][1] + P[0][1]*SF[3] + P[3][1]*SF[5] + P[1][1]*SPP[6] - P[10][1]*SPP[3] + P[12][1]*SPP[5] - (P[11][1]*q0)/2) + SF[6]*(P[2][2] + P[0][2]*SF[3] + P[3][2]*SF[5] + P[1][2]*SPP[6] - P[10][2]*SPP[3] + P[12][2]*SPP[5] - (P[11][2]*q0)/2) + SPP[4]*(P[2][10] + P[0][10]*SF[3] + P[3][10]*SF[5] + P[1][10]*SPP[6] - P[10][10]*SPP[3] + P[12][10]*SPP[5] - (P[11][10]*q0)/2) - SPP[5]*(P[2][11] + P[0][11]*SF[3] + P[3][11]*SF[5] + P[1][11]*SPP[6] - P[10][11]*SPP[3] + P[12][11]*SPP[5] - (P[11][11]*q0)/2) - (q0*(P[2][12] + P[0][12]*SF[3] + P[3][12]*SF[5] + P[1][12]*SPP[6] - P[10][12]*SPP[3] + P[12][12]*SPP[5] - (P[11][12]*q0)/2))/2; - nextP[2][4] = P[2][4] + P[0][4]*SF[3] + P[3][4]*SF[5] + P[1][4]*SPP[6] - P[10][4]*SPP[3] + P[12][4]*SPP[5] - (P[11][4]*q0)/2 + SF[2]*(P[2][0] + P[0][0]*SF[3] + P[3][0]*SF[5] + P[1][0]*SPP[6] - P[10][0]*SPP[3] + P[12][0]*SPP[5] - (P[11][0]*q0)/2) + SF[0]*(P[2][1] + P[0][1]*SF[3] + P[3][1]*SF[5] + P[1][1]*SPP[6] - P[10][1]*SPP[3] + P[12][1]*SPP[5] - (P[11][1]*q0)/2) + SPP[0]*(P[2][2] + P[0][2]*SF[3] + P[3][2]*SF[5] + P[1][2]*SPP[6] - P[10][2]*SPP[3] + P[12][2]*SPP[5] - (P[11][2]*q0)/2) - SPP[2]*(P[2][3] + P[0][3]*SF[3] + P[3][3]*SF[5] + P[1][3]*SPP[6] - P[10][3]*SPP[3] + P[12][3]*SPP[5] - (P[11][3]*q0)/2); - nextP[2][5] = P[2][5] + P[0][5]*SF[3] + P[3][5]*SF[5] + P[1][5]*SPP[6] - P[10][5]*SPP[3] + P[12][5]*SPP[5] - (P[11][5]*q0)/2 + SF[1]*(P[2][0] + P[0][0]*SF[3] + P[3][0]*SF[5] + P[1][0]*SPP[6] - P[10][0]*SPP[3] + P[12][0]*SPP[5] - (P[11][0]*q0)/2) + SF[0]*(P[2][2] + P[0][2]*SF[3] + P[3][2]*SF[5] + P[1][2]*SPP[6] - P[10][2]*SPP[3] + P[12][2]*SPP[5] - (P[11][2]*q0)/2) + SF[2]*(P[2][3] + P[0][3]*SF[3] + P[3][3]*SF[5] + P[1][3]*SPP[6] - P[10][3]*SPP[3] + P[12][3]*SPP[5] - (P[11][3]*q0)/2) - SPP[0]*(P[2][1] + P[0][1]*SF[3] + P[3][1]*SF[5] + P[1][1]*SPP[6] - P[10][1]*SPP[3] + P[12][1]*SPP[5] - (P[11][1]*q0)/2); - nextP[2][6] = P[2][6] + P[0][6]*SF[3] + P[3][6]*SF[5] + P[1][6]*SPP[6] - P[10][6]*SPP[3] + P[12][6]*SPP[5] - (P[11][6]*q0)/2 + SF[1]*(P[2][1] + P[0][1]*SF[3] + P[3][1]*SF[5] + P[1][1]*SPP[6] - P[10][1]*SPP[3] + P[12][1]*SPP[5] - (P[11][1]*q0)/2) + SF[0]*(P[2][3] + P[0][3]*SF[3] + P[3][3]*SF[5] + P[1][3]*SPP[6] - P[10][3]*SPP[3] + P[12][3]*SPP[5] - (P[11][3]*q0)/2) + SPP[0]*(P[2][0] + P[0][0]*SF[3] + P[3][0]*SF[5] + P[1][0]*SPP[6] - P[10][0]*SPP[3] + P[12][0]*SPP[5] - (P[11][0]*q0)/2) - SPP[1]*(P[2][2] + P[0][2]*SF[3] + P[3][2]*SF[5] + P[1][2]*SPP[6] - P[10][2]*SPP[3] + P[12][2]*SPP[5] - (P[11][2]*q0)/2); - nextP[2][7] = P[2][7] + P[0][7]*SF[3] + P[3][7]*SF[5] + P[1][7]*SPP[6] - P[10][7]*SPP[3] + P[12][7]*SPP[5] - (P[11][7]*q0)/2 + dt*(P[2][4] + P[0][4]*SF[3] + P[3][4]*SF[5] + P[1][4]*SPP[6] - P[10][4]*SPP[3] + P[12][4]*SPP[5] - (P[11][4]*q0)/2); - nextP[2][8] = P[2][8] + P[0][8]*SF[3] + P[3][8]*SF[5] + P[1][8]*SPP[6] - P[10][8]*SPP[3] + P[12][8]*SPP[5] - (P[11][8]*q0)/2 + dt*(P[2][5] + P[0][5]*SF[3] + P[3][5]*SF[5] + P[1][5]*SPP[6] - P[10][5]*SPP[3] + P[12][5]*SPP[5] - (P[11][5]*q0)/2); - nextP[2][9] = P[2][9] + P[0][9]*SF[3] + P[3][9]*SF[5] + P[1][9]*SPP[6] - P[10][9]*SPP[3] + P[12][9]*SPP[5] - (P[11][9]*q0)/2 + dt*(P[2][6] + P[0][6]*SF[3] + P[3][6]*SF[5] + P[1][6]*SPP[6] - P[10][6]*SPP[3] + P[12][6]*SPP[5] - (P[11][6]*q0)/2); - nextP[2][10] = P[2][10] + P[0][10]*SF[3] + P[3][10]*SF[5] + P[1][10]*SPP[6] - P[10][10]*SPP[3] + P[12][10]*SPP[5] - (P[11][10]*q0)/2; - nextP[2][11] = P[2][11] + P[0][11]*SF[3] + P[3][11]*SF[5] + P[1][11]*SPP[6] - P[10][11]*SPP[3] + P[12][11]*SPP[5] - (P[11][11]*q0)/2; - nextP[2][12] = P[2][12] + P[0][12]*SF[3] + P[3][12]*SF[5] + P[1][12]*SPP[6] - P[10][12]*SPP[3] + P[12][12]*SPP[5] - (P[11][12]*q0)/2; - nextP[2][13] = P[2][13] + P[0][13]*SF[3] + P[3][13]*SF[5] + P[1][13]*SPP[6] - P[10][13]*SPP[3] + P[12][13]*SPP[5] - (P[11][13]*q0)/2; - nextP[2][14] = P[2][14] + P[0][14]*SF[3] + P[3][14]*SF[5] + P[1][14]*SPP[6] - P[10][14]*SPP[3] + P[12][14]*SPP[5] - (P[11][14]*q0)/2; - nextP[2][15] = P[2][15] + P[0][15]*SF[3] + P[3][15]*SF[5] + P[1][15]*SPP[6] - P[10][15]*SPP[3] + P[12][15]*SPP[5] - (P[11][15]*q0)/2; - nextP[2][16] = P[2][16] + P[0][16]*SF[3] + P[3][16]*SF[5] + P[1][16]*SPP[6] - P[10][16]*SPP[3] + P[12][16]*SPP[5] - (P[11][16]*q0)/2; - nextP[2][17] = P[2][17] + P[0][17]*SF[3] + P[3][17]*SF[5] + P[1][17]*SPP[6] - P[10][17]*SPP[3] + P[12][17]*SPP[5] - (P[11][17]*q0)/2; - nextP[2][18] = P[2][18] + P[0][18]*SF[3] + P[3][18]*SF[5] + P[1][18]*SPP[6] - P[10][18]*SPP[3] + P[12][18]*SPP[5] - (P[11][18]*q0)/2; - nextP[2][19] = P[2][19] + P[0][19]*SF[3] + P[3][19]*SF[5] + P[1][19]*SPP[6] - P[10][19]*SPP[3] + P[12][19]*SPP[5] - (P[11][19]*q0)/2; - nextP[2][20] = P[2][20] + P[0][20]*SF[3] + P[3][20]*SF[5] + P[1][20]*SPP[6] - P[10][20]*SPP[3] + P[12][20]*SPP[5] - (P[11][20]*q0)/2; - nextP[3][0] = P[3][0] + SQ[6] + P[0][0]*SF[4] + P[1][0]*SF[3] + P[2][0]*SF[6] + P[10][0]*SPP[4] - P[11][0]*SPP[5] - (P[12][0]*q0)/2 + SF[6]*(P[3][1] + P[0][1]*SF[4] + P[1][1]*SF[3] + P[2][1]*SF[6] + P[10][1]*SPP[4] - P[11][1]*SPP[5] - (P[12][1]*q0)/2) + SPP[7]*(P[3][2] + P[0][2]*SF[4] + P[1][2]*SF[3] + P[2][2]*SF[6] + P[10][2]*SPP[4] - P[11][2]*SPP[5] - (P[12][2]*q0)/2) + SPP[6]*(P[3][3] + P[0][3]*SF[4] + P[1][3]*SF[3] + P[2][3]*SF[6] + P[10][3]*SPP[4] - P[11][3]*SPP[5] - (P[12][3]*q0)/2) + SPP[5]*(P[3][10] + P[0][10]*SF[4] + P[1][10]*SF[3] + P[2][10]*SF[6] + P[10][10]*SPP[4] - P[11][10]*SPP[5] - (P[12][10]*q0)/2) + SPP[4]*(P[3][11] + P[0][11]*SF[4] + P[1][11]*SF[3] + P[2][11]*SF[6] + P[10][11]*SPP[4] - P[11][11]*SPP[5] - (P[12][11]*q0)/2) + SPP[3]*(P[3][12] + P[0][12]*SF[4] + P[1][12]*SF[3] + P[2][12]*SF[6] + P[10][12]*SPP[4] - P[11][12]*SPP[5] - (P[12][12]*q0)/2); - nextP[3][1] = P[3][1] + SQ[4] + P[0][1]*SF[4] + P[1][1]*SF[3] + P[2][1]*SF[6] + P[10][1]*SPP[4] - P[11][1]*SPP[5] - (P[12][1]*q0)/2 + SF[5]*(P[3][0] + P[0][0]*SF[4] + P[1][0]*SF[3] + P[2][0]*SF[6] + P[10][0]*SPP[4] - P[11][0]*SPP[5] - (P[12][0]*q0)/2) + SF[4]*(P[3][2] + P[0][2]*SF[4] + P[1][2]*SF[3] + P[2][2]*SF[6] + P[10][2]*SPP[4] - P[11][2]*SPP[5] - (P[12][2]*q0)/2) + SPP[7]*(P[3][3] + P[0][3]*SF[4] + P[1][3]*SF[3] + P[2][3]*SF[6] + P[10][3]*SPP[4] - P[11][3]*SPP[5] - (P[12][3]*q0)/2) + SPP[3]*(P[3][11] + P[0][11]*SF[4] + P[1][11]*SF[3] + P[2][11]*SF[6] + P[10][11]*SPP[4] - P[11][11]*SPP[5] - (P[12][11]*q0)/2) - SPP[4]*(P[3][12] + P[0][12]*SF[4] + P[1][12]*SF[3] + P[2][12]*SF[6] + P[10][12]*SPP[4] - P[11][12]*SPP[5] - (P[12][12]*q0)/2) - (q0*(P[3][10] + P[0][10]*SF[4] + P[1][10]*SF[3] + P[2][10]*SF[6] + P[10][10]*SPP[4] - P[11][10]*SPP[5] - (P[12][10]*q0)/2))/2; - nextP[3][2] = P[3][2] + SQ[3] + P[0][2]*SF[4] + P[1][2]*SF[3] + P[2][2]*SF[6] + P[10][2]*SPP[4] - P[11][2]*SPP[5] - (P[12][2]*q0)/2 + SF[3]*(P[3][0] + P[0][0]*SF[4] + P[1][0]*SF[3] + P[2][0]*SF[6] + P[10][0]*SPP[4] - P[11][0]*SPP[5] - (P[12][0]*q0)/2) + SF[5]*(P[3][3] + P[0][3]*SF[4] + P[1][3]*SF[3] + P[2][3]*SF[6] + P[10][3]*SPP[4] - P[11][3]*SPP[5] - (P[12][3]*q0)/2) + SPP[6]*(P[3][1] + P[0][1]*SF[4] + P[1][1]*SF[3] + P[2][1]*SF[6] + P[10][1]*SPP[4] - P[11][1]*SPP[5] - (P[12][1]*q0)/2) - SPP[3]*(P[3][10] + P[0][10]*SF[4] + P[1][10]*SF[3] + P[2][10]*SF[6] + P[10][10]*SPP[4] - P[11][10]*SPP[5] - (P[12][10]*q0)/2) + SPP[5]*(P[3][12] + P[0][12]*SF[4] + P[1][12]*SF[3] + P[2][12]*SF[6] + P[10][12]*SPP[4] - P[11][12]*SPP[5] - (P[12][12]*q0)/2) - (q0*(P[3][11] + P[0][11]*SF[4] + P[1][11]*SF[3] + P[2][11]*SF[6] + P[10][11]*SPP[4] - P[11][11]*SPP[5] - (P[12][11]*q0)/2))/2; - nextP[3][3] = P[3][3] + P[0][3]*SF[4] + P[1][3]*SF[3] + P[2][3]*SF[6] + P[10][3]*SPP[4] - P[11][3]*SPP[5] + (dayCov*SQ[10])/4 + dazCov*SQ[9] - (P[12][3]*q0)/2 + SF[4]*(P[3][0] + P[0][0]*SF[4] + P[1][0]*SF[3] + P[2][0]*SF[6] + P[10][0]*SPP[4] - P[11][0]*SPP[5] - (P[12][0]*q0)/2) + SF[3]*(P[3][1] + P[0][1]*SF[4] + P[1][1]*SF[3] + P[2][1]*SF[6] + P[10][1]*SPP[4] - P[11][1]*SPP[5] - (P[12][1]*q0)/2) + SF[6]*(P[3][2] + P[0][2]*SF[4] + P[1][2]*SF[3] + P[2][2]*SF[6] + P[10][2]*SPP[4] - P[11][2]*SPP[5] - (P[12][2]*q0)/2) + SPP[4]*(P[3][10] + P[0][10]*SF[4] + P[1][10]*SF[3] + P[2][10]*SF[6] + P[10][10]*SPP[4] - P[11][10]*SPP[5] - (P[12][10]*q0)/2) - SPP[5]*(P[3][11] + P[0][11]*SF[4] + P[1][11]*SF[3] + P[2][11]*SF[6] + P[10][11]*SPP[4] - P[11][11]*SPP[5] - (P[12][11]*q0)/2) + (daxCov*sq(q2))/4 - (q0*(P[3][12] + P[0][12]*SF[4] + P[1][12]*SF[3] + P[2][12]*SF[6] + P[10][12]*SPP[4] - P[11][12]*SPP[5] - (P[12][12]*q0)/2))/2; - nextP[3][4] = P[3][4] + P[0][4]*SF[4] + P[1][4]*SF[3] + P[2][4]*SF[6] + P[10][4]*SPP[4] - P[11][4]*SPP[5] - (P[12][4]*q0)/2 + SF[2]*(P[3][0] + P[0][0]*SF[4] + P[1][0]*SF[3] + P[2][0]*SF[6] + P[10][0]*SPP[4] - P[11][0]*SPP[5] - (P[12][0]*q0)/2) + SF[0]*(P[3][1] + P[0][1]*SF[4] + P[1][1]*SF[3] + P[2][1]*SF[6] + P[10][1]*SPP[4] - P[11][1]*SPP[5] - (P[12][1]*q0)/2) + SPP[0]*(P[3][2] + P[0][2]*SF[4] + P[1][2]*SF[3] + P[2][2]*SF[6] + P[10][2]*SPP[4] - P[11][2]*SPP[5] - (P[12][2]*q0)/2) - SPP[2]*(P[3][3] + P[0][3]*SF[4] + P[1][3]*SF[3] + P[2][3]*SF[6] + P[10][3]*SPP[4] - P[11][3]*SPP[5] - (P[12][3]*q0)/2); - nextP[3][5] = P[3][5] + P[0][5]*SF[4] + P[1][5]*SF[3] + P[2][5]*SF[6] + P[10][5]*SPP[4] - P[11][5]*SPP[5] - (P[12][5]*q0)/2 + SF[1]*(P[3][0] + P[0][0]*SF[4] + P[1][0]*SF[3] + P[2][0]*SF[6] + P[10][0]*SPP[4] - P[11][0]*SPP[5] - (P[12][0]*q0)/2) + SF[0]*(P[3][2] + P[0][2]*SF[4] + P[1][2]*SF[3] + P[2][2]*SF[6] + P[10][2]*SPP[4] - P[11][2]*SPP[5] - (P[12][2]*q0)/2) + SF[2]*(P[3][3] + P[0][3]*SF[4] + P[1][3]*SF[3] + P[2][3]*SF[6] + P[10][3]*SPP[4] - P[11][3]*SPP[5] - (P[12][3]*q0)/2) - SPP[0]*(P[3][1] + P[0][1]*SF[4] + P[1][1]*SF[3] + P[2][1]*SF[6] + P[10][1]*SPP[4] - P[11][1]*SPP[5] - (P[12][1]*q0)/2); - nextP[3][6] = P[3][6] + P[0][6]*SF[4] + P[1][6]*SF[3] + P[2][6]*SF[6] + P[10][6]*SPP[4] - P[11][6]*SPP[5] - (P[12][6]*q0)/2 + SF[1]*(P[3][1] + P[0][1]*SF[4] + P[1][1]*SF[3] + P[2][1]*SF[6] + P[10][1]*SPP[4] - P[11][1]*SPP[5] - (P[12][1]*q0)/2) + SF[0]*(P[3][3] + P[0][3]*SF[4] + P[1][3]*SF[3] + P[2][3]*SF[6] + P[10][3]*SPP[4] - P[11][3]*SPP[5] - (P[12][3]*q0)/2) + SPP[0]*(P[3][0] + P[0][0]*SF[4] + P[1][0]*SF[3] + P[2][0]*SF[6] + P[10][0]*SPP[4] - P[11][0]*SPP[5] - (P[12][0]*q0)/2) - SPP[1]*(P[3][2] + P[0][2]*SF[4] + P[1][2]*SF[3] + P[2][2]*SF[6] + P[10][2]*SPP[4] - P[11][2]*SPP[5] - (P[12][2]*q0)/2); - nextP[3][7] = P[3][7] + P[0][7]*SF[4] + P[1][7]*SF[3] + P[2][7]*SF[6] + P[10][7]*SPP[4] - P[11][7]*SPP[5] - (P[12][7]*q0)/2 + dt*(P[3][4] + P[0][4]*SF[4] + P[1][4]*SF[3] + P[2][4]*SF[6] + P[10][4]*SPP[4] - P[11][4]*SPP[5] - (P[12][4]*q0)/2); - nextP[3][8] = P[3][8] + P[0][8]*SF[4] + P[1][8]*SF[3] + P[2][8]*SF[6] + P[10][8]*SPP[4] - P[11][8]*SPP[5] - (P[12][8]*q0)/2 + dt*(P[3][5] + P[0][5]*SF[4] + P[1][5]*SF[3] + P[2][5]*SF[6] + P[10][5]*SPP[4] - P[11][5]*SPP[5] - (P[12][5]*q0)/2); - nextP[3][9] = P[3][9] + P[0][9]*SF[4] + P[1][9]*SF[3] + P[2][9]*SF[6] + P[10][9]*SPP[4] - P[11][9]*SPP[5] - (P[12][9]*q0)/2 + dt*(P[3][6] + P[0][6]*SF[4] + P[1][6]*SF[3] + P[2][6]*SF[6] + P[10][6]*SPP[4] - P[11][6]*SPP[5] - (P[12][6]*q0)/2); - nextP[3][10] = P[3][10] + P[0][10]*SF[4] + P[1][10]*SF[3] + P[2][10]*SF[6] + P[10][10]*SPP[4] - P[11][10]*SPP[5] - (P[12][10]*q0)/2; - nextP[3][11] = P[3][11] + P[0][11]*SF[4] + P[1][11]*SF[3] + P[2][11]*SF[6] + P[10][11]*SPP[4] - P[11][11]*SPP[5] - (P[12][11]*q0)/2; - nextP[3][12] = P[3][12] + P[0][12]*SF[4] + P[1][12]*SF[3] + P[2][12]*SF[6] + P[10][12]*SPP[4] - P[11][12]*SPP[5] - (P[12][12]*q0)/2; - nextP[3][13] = P[3][13] + P[0][13]*SF[4] + P[1][13]*SF[3] + P[2][13]*SF[6] + P[10][13]*SPP[4] - P[11][13]*SPP[5] - (P[12][13]*q0)/2; - nextP[3][14] = P[3][14] + P[0][14]*SF[4] + P[1][14]*SF[3] + P[2][14]*SF[6] + P[10][14]*SPP[4] - P[11][14]*SPP[5] - (P[12][14]*q0)/2; - nextP[3][15] = P[3][15] + P[0][15]*SF[4] + P[1][15]*SF[3] + P[2][15]*SF[6] + P[10][15]*SPP[4] - P[11][15]*SPP[5] - (P[12][15]*q0)/2; - nextP[3][16] = P[3][16] + P[0][16]*SF[4] + P[1][16]*SF[3] + P[2][16]*SF[6] + P[10][16]*SPP[4] - P[11][16]*SPP[5] - (P[12][16]*q0)/2; - nextP[3][17] = P[3][17] + P[0][17]*SF[4] + P[1][17]*SF[3] + P[2][17]*SF[6] + P[10][17]*SPP[4] - P[11][17]*SPP[5] - (P[12][17]*q0)/2; - nextP[3][18] = P[3][18] + P[0][18]*SF[4] + P[1][18]*SF[3] + P[2][18]*SF[6] + P[10][18]*SPP[4] - P[11][18]*SPP[5] - (P[12][18]*q0)/2; - nextP[3][19] = P[3][19] + P[0][19]*SF[4] + P[1][19]*SF[3] + P[2][19]*SF[6] + P[10][19]*SPP[4] - P[11][19]*SPP[5] - (P[12][19]*q0)/2; - nextP[3][20] = P[3][20] + P[0][20]*SF[4] + P[1][20]*SF[3] + P[2][20]*SF[6] + P[10][20]*SPP[4] - P[11][20]*SPP[5] - (P[12][20]*q0)/2; - nextP[4][0] = P[4][0] + P[0][0]*SF[2] + P[1][0]*SF[0] + P[2][0]*SPP[0] - P[3][0]*SPP[2] + SF[6]*(P[4][1] + P[0][1]*SF[2] + P[1][1]*SF[0] + P[2][1]*SPP[0] - P[3][1]*SPP[2]) + SPP[7]*(P[4][2] + P[0][2]*SF[2] + P[1][2]*SF[0] + P[2][2]*SPP[0] - P[3][2]*SPP[2]) + SPP[6]*(P[4][3] + P[0][3]*SF[2] + P[1][3]*SF[0] + P[2][3]*SPP[0] - P[3][3]*SPP[2]) + SPP[5]*(P[4][10] + P[0][10]*SF[2] + P[1][10]*SF[0] + P[2][10]*SPP[0] - P[3][10]*SPP[2]) + SPP[4]*(P[4][11] + P[0][11]*SF[2] + P[1][11]*SF[0] + P[2][11]*SPP[0] - P[3][11]*SPP[2]) + SPP[3]*(P[4][12] + P[0][12]*SF[2] + P[1][12]*SF[0] + P[2][12]*SPP[0] - P[3][12]*SPP[2]); - nextP[4][1] = P[4][1] + P[0][1]*SF[2] + P[1][1]*SF[0] + P[2][1]*SPP[0] - P[3][1]*SPP[2] + SF[5]*(P[4][0] + P[0][0]*SF[2] + P[1][0]*SF[0] + P[2][0]*SPP[0] - P[3][0]*SPP[2]) + SF[4]*(P[4][2] + P[0][2]*SF[2] + P[1][2]*SF[0] + P[2][2]*SPP[0] - P[3][2]*SPP[2]) + SPP[7]*(P[4][3] + P[0][3]*SF[2] + P[1][3]*SF[0] + P[2][3]*SPP[0] - P[3][3]*SPP[2]) + SPP[3]*(P[4][11] + P[0][11]*SF[2] + P[1][11]*SF[0] + P[2][11]*SPP[0] - P[3][11]*SPP[2]) - SPP[4]*(P[4][12] + P[0][12]*SF[2] + P[1][12]*SF[0] + P[2][12]*SPP[0] - P[3][12]*SPP[2]) - (q0*(P[4][10] + P[0][10]*SF[2] + P[1][10]*SF[0] + P[2][10]*SPP[0] - P[3][10]*SPP[2]))/2; - nextP[4][2] = P[4][2] + P[0][2]*SF[2] + P[1][2]*SF[0] + P[2][2]*SPP[0] - P[3][2]*SPP[2] + SF[3]*(P[4][0] + P[0][0]*SF[2] + P[1][0]*SF[0] + P[2][0]*SPP[0] - P[3][0]*SPP[2]) + SF[5]*(P[4][3] + P[0][3]*SF[2] + P[1][3]*SF[0] + P[2][3]*SPP[0] - P[3][3]*SPP[2]) + SPP[6]*(P[4][1] + P[0][1]*SF[2] + P[1][1]*SF[0] + P[2][1]*SPP[0] - P[3][1]*SPP[2]) - SPP[3]*(P[4][10] + P[0][10]*SF[2] + P[1][10]*SF[0] + P[2][10]*SPP[0] - P[3][10]*SPP[2]) + SPP[5]*(P[4][12] + P[0][12]*SF[2] + P[1][12]*SF[0] + P[2][12]*SPP[0] - P[3][12]*SPP[2]) - (q0*(P[4][11] + P[0][11]*SF[2] + P[1][11]*SF[0] + P[2][11]*SPP[0] - P[3][11]*SPP[2]))/2; - nextP[4][3] = P[4][3] + P[0][3]*SF[2] + P[1][3]*SF[0] + P[2][3]*SPP[0] - P[3][3]*SPP[2] + SF[4]*(P[4][0] + P[0][0]*SF[2] + P[1][0]*SF[0] + P[2][0]*SPP[0] - P[3][0]*SPP[2]) + SF[3]*(P[4][1] + P[0][1]*SF[2] + P[1][1]*SF[0] + P[2][1]*SPP[0] - P[3][1]*SPP[2]) + SF[6]*(P[4][2] + P[0][2]*SF[2] + P[1][2]*SF[0] + P[2][2]*SPP[0] - P[3][2]*SPP[2]) + SPP[4]*(P[4][10] + P[0][10]*SF[2] + P[1][10]*SF[0] + P[2][10]*SPP[0] - P[3][10]*SPP[2]) - SPP[5]*(P[4][11] + P[0][11]*SF[2] + P[1][11]*SF[0] + P[2][11]*SPP[0] - P[3][11]*SPP[2]) - (q0*(P[4][12] + P[0][12]*SF[2] + P[1][12]*SF[0] + P[2][12]*SPP[0] - P[3][12]*SPP[2]))/2; - nextP[4][4] = P[4][4] + P[0][4]*SF[2] + P[1][4]*SF[0] + P[2][4]*SPP[0] - P[3][4]*SPP[2] + dvyCov*sq(SG[7] - 2*q0*q3) + dvzCov*sq(SG[6] + 2*q0*q2) + SF[2]*(P[4][0] + P[0][0]*SF[2] + P[1][0]*SF[0] + P[2][0]*SPP[0] - P[3][0]*SPP[2]) + SF[0]*(P[4][1] + P[0][1]*SF[2] + P[1][1]*SF[0] + P[2][1]*SPP[0] - P[3][1]*SPP[2]) + SPP[0]*(P[4][2] + P[0][2]*SF[2] + P[1][2]*SF[0] + P[2][2]*SPP[0] - P[3][2]*SPP[2]) - SPP[2]*(P[4][3] + P[0][3]*SF[2] + P[1][3]*SF[0] + P[2][3]*SPP[0] - P[3][3]*SPP[2]) + dvxCov*sq(SG[1] + SG[2] - SG[3] - SG[4]); - nextP[4][5] = P[4][5] + SQ[2] + P[0][5]*SF[2] + P[1][5]*SF[0] + P[2][5]*SPP[0] - P[3][5]*SPP[2] + SF[1]*(P[4][0] + P[0][0]*SF[2] + P[1][0]*SF[0] + P[2][0]*SPP[0] - P[3][0]*SPP[2]) + SF[0]*(P[4][2] + P[0][2]*SF[2] + P[1][2]*SF[0] + P[2][2]*SPP[0] - P[3][2]*SPP[2]) + SF[2]*(P[4][3] + P[0][3]*SF[2] + P[1][3]*SF[0] + P[2][3]*SPP[0] - P[3][3]*SPP[2]) - SPP[0]*(P[4][1] + P[0][1]*SF[2] + P[1][1]*SF[0] + P[2][1]*SPP[0] - P[3][1]*SPP[2]); - nextP[4][6] = P[4][6] + SQ[1] + P[0][6]*SF[2] + P[1][6]*SF[0] + P[2][6]*SPP[0] - P[3][6]*SPP[2] + SF[1]*(P[4][1] + P[0][1]*SF[2] + P[1][1]*SF[0] + P[2][1]*SPP[0] - P[3][1]*SPP[2]) + SF[0]*(P[4][3] + P[0][3]*SF[2] + P[1][3]*SF[0] + P[2][3]*SPP[0] - P[3][3]*SPP[2]) + SPP[0]*(P[4][0] + P[0][0]*SF[2] + P[1][0]*SF[0] + P[2][0]*SPP[0] - P[3][0]*SPP[2]) - SPP[1]*(P[4][2] + P[0][2]*SF[2] + P[1][2]*SF[0] + P[2][2]*SPP[0] - P[3][2]*SPP[2]); - nextP[4][7] = P[4][7] + P[0][7]*SF[2] + P[1][7]*SF[0] + P[2][7]*SPP[0] - P[3][7]*SPP[2] + dt*(P[4][4] + P[0][4]*SF[2] + P[1][4]*SF[0] + P[2][4]*SPP[0] - P[3][4]*SPP[2]); - nextP[4][8] = P[4][8] + P[0][8]*SF[2] + P[1][8]*SF[0] + P[2][8]*SPP[0] - P[3][8]*SPP[2] + dt*(P[4][5] + P[0][5]*SF[2] + P[1][5]*SF[0] + P[2][5]*SPP[0] - P[3][5]*SPP[2]); - nextP[4][9] = P[4][9] + P[0][9]*SF[2] + P[1][9]*SF[0] + P[2][9]*SPP[0] - P[3][9]*SPP[2] + dt*(P[4][6] + P[0][6]*SF[2] + P[1][6]*SF[0] + P[2][6]*SPP[0] - P[3][6]*SPP[2]); - nextP[4][10] = P[4][10] + P[0][10]*SF[2] + P[1][10]*SF[0] + P[2][10]*SPP[0] - P[3][10]*SPP[2]; - nextP[4][11] = P[4][11] + P[0][11]*SF[2] + P[1][11]*SF[0] + P[2][11]*SPP[0] - P[3][11]*SPP[2]; - nextP[4][12] = P[4][12] + P[0][12]*SF[2] + P[1][12]*SF[0] + P[2][12]*SPP[0] - P[3][12]*SPP[2]; - nextP[4][13] = P[4][13] + P[0][13]*SF[2] + P[1][13]*SF[0] + P[2][13]*SPP[0] - P[3][13]*SPP[2]; - nextP[4][14] = P[4][14] + P[0][14]*SF[2] + P[1][14]*SF[0] + P[2][14]*SPP[0] - P[3][14]*SPP[2]; - nextP[4][15] = P[4][15] + P[0][15]*SF[2] + P[1][15]*SF[0] + P[2][15]*SPP[0] - P[3][15]*SPP[2]; - nextP[4][16] = P[4][16] + P[0][16]*SF[2] + P[1][16]*SF[0] + P[2][16]*SPP[0] - P[3][16]*SPP[2]; - nextP[4][17] = P[4][17] + P[0][17]*SF[2] + P[1][17]*SF[0] + P[2][17]*SPP[0] - P[3][17]*SPP[2]; - nextP[4][18] = P[4][18] + P[0][18]*SF[2] + P[1][18]*SF[0] + P[2][18]*SPP[0] - P[3][18]*SPP[2]; - nextP[4][19] = P[4][19] + P[0][19]*SF[2] + P[1][19]*SF[0] + P[2][19]*SPP[0] - P[3][19]*SPP[2]; - nextP[4][20] = P[4][20] + P[0][20]*SF[2] + P[1][20]*SF[0] + P[2][20]*SPP[0] - P[3][20]*SPP[2]; - nextP[5][0] = P[5][0] + P[0][0]*SF[1] + P[2][0]*SF[0] + P[3][0]*SF[2] - P[1][0]*SPP[0] + SF[6]*(P[5][1] + P[0][1]*SF[1] + P[2][1]*SF[0] + P[3][1]*SF[2] - P[1][1]*SPP[0]) + SPP[7]*(P[5][2] + P[0][2]*SF[1] + P[2][2]*SF[0] + P[3][2]*SF[2] - P[1][2]*SPP[0]) + SPP[6]*(P[5][3] + P[0][3]*SF[1] + P[2][3]*SF[0] + P[3][3]*SF[2] - P[1][3]*SPP[0]) + SPP[5]*(P[5][10] + P[0][10]*SF[1] + P[2][10]*SF[0] + P[3][10]*SF[2] - P[1][10]*SPP[0]) + SPP[4]*(P[5][11] + P[0][11]*SF[1] + P[2][11]*SF[0] + P[3][11]*SF[2] - P[1][11]*SPP[0]) + SPP[3]*(P[5][12] + P[0][12]*SF[1] + P[2][12]*SF[0] + P[3][12]*SF[2] - P[1][12]*SPP[0]); - nextP[5][1] = P[5][1] + P[0][1]*SF[1] + P[2][1]*SF[0] + P[3][1]*SF[2] - P[1][1]*SPP[0] + SF[5]*(P[5][0] + P[0][0]*SF[1] + P[2][0]*SF[0] + P[3][0]*SF[2] - P[1][0]*SPP[0]) + SF[4]*(P[5][2] + P[0][2]*SF[1] + P[2][2]*SF[0] + P[3][2]*SF[2] - P[1][2]*SPP[0]) + SPP[7]*(P[5][3] + P[0][3]*SF[1] + P[2][3]*SF[0] + P[3][3]*SF[2] - P[1][3]*SPP[0]) + SPP[3]*(P[5][11] + P[0][11]*SF[1] + P[2][11]*SF[0] + P[3][11]*SF[2] - P[1][11]*SPP[0]) - SPP[4]*(P[5][12] + P[0][12]*SF[1] + P[2][12]*SF[0] + P[3][12]*SF[2] - P[1][12]*SPP[0]) - (q0*(P[5][10] + P[0][10]*SF[1] + P[2][10]*SF[0] + P[3][10]*SF[2] - P[1][10]*SPP[0]))/2; - nextP[5][2] = P[5][2] + P[0][2]*SF[1] + P[2][2]*SF[0] + P[3][2]*SF[2] - P[1][2]*SPP[0] + SF[3]*(P[5][0] + P[0][0]*SF[1] + P[2][0]*SF[0] + P[3][0]*SF[2] - P[1][0]*SPP[0]) + SF[5]*(P[5][3] + P[0][3]*SF[1] + P[2][3]*SF[0] + P[3][3]*SF[2] - P[1][3]*SPP[0]) + SPP[6]*(P[5][1] + P[0][1]*SF[1] + P[2][1]*SF[0] + P[3][1]*SF[2] - P[1][1]*SPP[0]) - SPP[3]*(P[5][10] + P[0][10]*SF[1] + P[2][10]*SF[0] + P[3][10]*SF[2] - P[1][10]*SPP[0]) + SPP[5]*(P[5][12] + P[0][12]*SF[1] + P[2][12]*SF[0] + P[3][12]*SF[2] - P[1][12]*SPP[0]) - (q0*(P[5][11] + P[0][11]*SF[1] + P[2][11]*SF[0] + P[3][11]*SF[2] - P[1][11]*SPP[0]))/2; - nextP[5][3] = P[5][3] + P[0][3]*SF[1] + P[2][3]*SF[0] + P[3][3]*SF[2] - P[1][3]*SPP[0] + SF[4]*(P[5][0] + P[0][0]*SF[1] + P[2][0]*SF[0] + P[3][0]*SF[2] - P[1][0]*SPP[0]) + SF[3]*(P[5][1] + P[0][1]*SF[1] + P[2][1]*SF[0] + P[3][1]*SF[2] - P[1][1]*SPP[0]) + SF[6]*(P[5][2] + P[0][2]*SF[1] + P[2][2]*SF[0] + P[3][2]*SF[2] - P[1][2]*SPP[0]) + SPP[4]*(P[5][10] + P[0][10]*SF[1] + P[2][10]*SF[0] + P[3][10]*SF[2] - P[1][10]*SPP[0]) - SPP[5]*(P[5][11] + P[0][11]*SF[1] + P[2][11]*SF[0] + P[3][11]*SF[2] - P[1][11]*SPP[0]) - (q0*(P[5][12] + P[0][12]*SF[1] + P[2][12]*SF[0] + P[3][12]*SF[2] - P[1][12]*SPP[0]))/2; - nextP[5][4] = P[5][4] + SQ[2] + P[0][4]*SF[1] + P[2][4]*SF[0] + P[3][4]*SF[2] - P[1][4]*SPP[0] + SF[2]*(P[5][0] + P[0][0]*SF[1] + P[2][0]*SF[0] + P[3][0]*SF[2] - P[1][0]*SPP[0]) + SF[0]*(P[5][1] + P[0][1]*SF[1] + P[2][1]*SF[0] + P[3][1]*SF[2] - P[1][1]*SPP[0]) + SPP[0]*(P[5][2] + P[0][2]*SF[1] + P[2][2]*SF[0] + P[3][2]*SF[2] - P[1][2]*SPP[0]) - SPP[2]*(P[5][3] + P[0][3]*SF[1] + P[2][3]*SF[0] + P[3][3]*SF[2] - P[1][3]*SPP[0]); - nextP[5][5] = P[5][5] + P[0][5]*SF[1] + P[2][5]*SF[0] + P[3][5]*SF[2] - P[1][5]*SPP[0] + dvxCov*sq(SG[7] + 2*q0*q3) + dvzCov*sq(SG[5] - 2*q0*q1) + SF[1]*(P[5][0] + P[0][0]*SF[1] + P[2][0]*SF[0] + P[3][0]*SF[2] - P[1][0]*SPP[0]) + SF[0]*(P[5][2] + P[0][2]*SF[1] + P[2][2]*SF[0] + P[3][2]*SF[2] - P[1][2]*SPP[0]) + SF[2]*(P[5][3] + P[0][3]*SF[1] + P[2][3]*SF[0] + P[3][3]*SF[2] - P[1][3]*SPP[0]) - SPP[0]*(P[5][1] + P[0][1]*SF[1] + P[2][1]*SF[0] + P[3][1]*SF[2] - P[1][1]*SPP[0]) + dvyCov*sq(SG[1] - SG[2] + SG[3] - SG[4]); - nextP[5][6] = P[5][6] + SQ[0] + P[0][6]*SF[1] + P[2][6]*SF[0] + P[3][6]*SF[2] - P[1][6]*SPP[0] + SF[1]*(P[5][1] + P[0][1]*SF[1] + P[2][1]*SF[0] + P[3][1]*SF[2] - P[1][1]*SPP[0]) + SF[0]*(P[5][3] + P[0][3]*SF[1] + P[2][3]*SF[0] + P[3][3]*SF[2] - P[1][3]*SPP[0]) + SPP[0]*(P[5][0] + P[0][0]*SF[1] + P[2][0]*SF[0] + P[3][0]*SF[2] - P[1][0]*SPP[0]) - SPP[1]*(P[5][2] + P[0][2]*SF[1] + P[2][2]*SF[0] + P[3][2]*SF[2] - P[1][2]*SPP[0]); - nextP[5][7] = P[5][7] + P[0][7]*SF[1] + P[2][7]*SF[0] + P[3][7]*SF[2] - P[1][7]*SPP[0] + dt*(P[5][4] + P[0][4]*SF[1] + P[2][4]*SF[0] + P[3][4]*SF[2] - P[1][4]*SPP[0]); - nextP[5][8] = P[5][8] + P[0][8]*SF[1] + P[2][8]*SF[0] + P[3][8]*SF[2] - P[1][8]*SPP[0] + dt*(P[5][5] + P[0][5]*SF[1] + P[2][5]*SF[0] + P[3][5]*SF[2] - P[1][5]*SPP[0]); - nextP[5][9] = P[5][9] + P[0][9]*SF[1] + P[2][9]*SF[0] + P[3][9]*SF[2] - P[1][9]*SPP[0] + dt*(P[5][6] + P[0][6]*SF[1] + P[2][6]*SF[0] + P[3][6]*SF[2] - P[1][6]*SPP[0]); - nextP[5][10] = P[5][10] + P[0][10]*SF[1] + P[2][10]*SF[0] + P[3][10]*SF[2] - P[1][10]*SPP[0]; - nextP[5][11] = P[5][11] + P[0][11]*SF[1] + P[2][11]*SF[0] + P[3][11]*SF[2] - P[1][11]*SPP[0]; - nextP[5][12] = P[5][12] + P[0][12]*SF[1] + P[2][12]*SF[0] + P[3][12]*SF[2] - P[1][12]*SPP[0]; - nextP[5][13] = P[5][13] + P[0][13]*SF[1] + P[2][13]*SF[0] + P[3][13]*SF[2] - P[1][13]*SPP[0]; - nextP[5][14] = P[5][14] + P[0][14]*SF[1] + P[2][14]*SF[0] + P[3][14]*SF[2] - P[1][14]*SPP[0]; - nextP[5][15] = P[5][15] + P[0][15]*SF[1] + P[2][15]*SF[0] + P[3][15]*SF[2] - P[1][15]*SPP[0]; - nextP[5][16] = P[5][16] + P[0][16]*SF[1] + P[2][16]*SF[0] + P[3][16]*SF[2] - P[1][16]*SPP[0]; - nextP[5][17] = P[5][17] + P[0][17]*SF[1] + P[2][17]*SF[0] + P[3][17]*SF[2] - P[1][17]*SPP[0]; - nextP[5][18] = P[5][18] + P[0][18]*SF[1] + P[2][18]*SF[0] + P[3][18]*SF[2] - P[1][18]*SPP[0]; - nextP[5][19] = P[5][19] + P[0][19]*SF[1] + P[2][19]*SF[0] + P[3][19]*SF[2] - P[1][19]*SPP[0]; - nextP[5][20] = P[5][20] + P[0][20]*SF[1] + P[2][20]*SF[0] + P[3][20]*SF[2] - P[1][20]*SPP[0]; - nextP[6][0] = P[6][0] + P[1][0]*SF[1] + P[3][0]*SF[0] + P[0][0]*SPP[0] - P[2][0]*SPP[1] + SF[6]*(P[6][1] + P[1][1]*SF[1] + P[3][1]*SF[0] + P[0][1]*SPP[0] - P[2][1]*SPP[1]) + SPP[7]*(P[6][2] + P[1][2]*SF[1] + P[3][2]*SF[0] + P[0][2]*SPP[0] - P[2][2]*SPP[1]) + SPP[6]*(P[6][3] + P[1][3]*SF[1] + P[3][3]*SF[0] + P[0][3]*SPP[0] - P[2][3]*SPP[1]) + SPP[5]*(P[6][10] + P[1][10]*SF[1] + P[3][10]*SF[0] + P[0][10]*SPP[0] - P[2][10]*SPP[1]) + SPP[4]*(P[6][11] + P[1][11]*SF[1] + P[3][11]*SF[0] + P[0][11]*SPP[0] - P[2][11]*SPP[1]) + SPP[3]*(P[6][12] + P[1][12]*SF[1] + P[3][12]*SF[0] + P[0][12]*SPP[0] - P[2][12]*SPP[1]); - nextP[6][1] = P[6][1] + P[1][1]*SF[1] + P[3][1]*SF[0] + P[0][1]*SPP[0] - P[2][1]*SPP[1] + SF[5]*(P[6][0] + P[1][0]*SF[1] + P[3][0]*SF[0] + P[0][0]*SPP[0] - P[2][0]*SPP[1]) + SF[4]*(P[6][2] + P[1][2]*SF[1] + P[3][2]*SF[0] + P[0][2]*SPP[0] - P[2][2]*SPP[1]) + SPP[7]*(P[6][3] + P[1][3]*SF[1] + P[3][3]*SF[0] + P[0][3]*SPP[0] - P[2][3]*SPP[1]) + SPP[3]*(P[6][11] + P[1][11]*SF[1] + P[3][11]*SF[0] + P[0][11]*SPP[0] - P[2][11]*SPP[1]) - SPP[4]*(P[6][12] + P[1][12]*SF[1] + P[3][12]*SF[0] + P[0][12]*SPP[0] - P[2][12]*SPP[1]) - (q0*(P[6][10] + P[1][10]*SF[1] + P[3][10]*SF[0] + P[0][10]*SPP[0] - P[2][10]*SPP[1]))/2; - nextP[6][2] = P[6][2] + P[1][2]*SF[1] + P[3][2]*SF[0] + P[0][2]*SPP[0] - P[2][2]*SPP[1] + SF[3]*(P[6][0] + P[1][0]*SF[1] + P[3][0]*SF[0] + P[0][0]*SPP[0] - P[2][0]*SPP[1]) + SF[5]*(P[6][3] + P[1][3]*SF[1] + P[3][3]*SF[0] + P[0][3]*SPP[0] - P[2][3]*SPP[1]) + SPP[6]*(P[6][1] + P[1][1]*SF[1] + P[3][1]*SF[0] + P[0][1]*SPP[0] - P[2][1]*SPP[1]) - SPP[3]*(P[6][10] + P[1][10]*SF[1] + P[3][10]*SF[0] + P[0][10]*SPP[0] - P[2][10]*SPP[1]) + SPP[5]*(P[6][12] + P[1][12]*SF[1] + P[3][12]*SF[0] + P[0][12]*SPP[0] - P[2][12]*SPP[1]) - (q0*(P[6][11] + P[1][11]*SF[1] + P[3][11]*SF[0] + P[0][11]*SPP[0] - P[2][11]*SPP[1]))/2; - nextP[6][3] = P[6][3] + P[1][3]*SF[1] + P[3][3]*SF[0] + P[0][3]*SPP[0] - P[2][3]*SPP[1] + SF[4]*(P[6][0] + P[1][0]*SF[1] + P[3][0]*SF[0] + P[0][0]*SPP[0] - P[2][0]*SPP[1]) + SF[3]*(P[6][1] + P[1][1]*SF[1] + P[3][1]*SF[0] + P[0][1]*SPP[0] - P[2][1]*SPP[1]) + SF[6]*(P[6][2] + P[1][2]*SF[1] + P[3][2]*SF[0] + P[0][2]*SPP[0] - P[2][2]*SPP[1]) + SPP[4]*(P[6][10] + P[1][10]*SF[1] + P[3][10]*SF[0] + P[0][10]*SPP[0] - P[2][10]*SPP[1]) - SPP[5]*(P[6][11] + P[1][11]*SF[1] + P[3][11]*SF[0] + P[0][11]*SPP[0] - P[2][11]*SPP[1]) - (q0*(P[6][12] + P[1][12]*SF[1] + P[3][12]*SF[0] + P[0][12]*SPP[0] - P[2][12]*SPP[1]))/2; - nextP[6][4] = P[6][4] + SQ[1] + P[1][4]*SF[1] + P[3][4]*SF[0] + P[0][4]*SPP[0] - P[2][4]*SPP[1] + SF[2]*(P[6][0] + P[1][0]*SF[1] + P[3][0]*SF[0] + P[0][0]*SPP[0] - P[2][0]*SPP[1]) + SF[0]*(P[6][1] + P[1][1]*SF[1] + P[3][1]*SF[0] + P[0][1]*SPP[0] - P[2][1]*SPP[1]) + SPP[0]*(P[6][2] + P[1][2]*SF[1] + P[3][2]*SF[0] + P[0][2]*SPP[0] - P[2][2]*SPP[1]) - SPP[2]*(P[6][3] + P[1][3]*SF[1] + P[3][3]*SF[0] + P[0][3]*SPP[0] - P[2][3]*SPP[1]); - nextP[6][5] = P[6][5] + SQ[0] + P[1][5]*SF[1] + P[3][5]*SF[0] + P[0][5]*SPP[0] - P[2][5]*SPP[1] + SF[1]*(P[6][0] + P[1][0]*SF[1] + P[3][0]*SF[0] + P[0][0]*SPP[0] - P[2][0]*SPP[1]) + SF[0]*(P[6][2] + P[1][2]*SF[1] + P[3][2]*SF[0] + P[0][2]*SPP[0] - P[2][2]*SPP[1]) + SF[2]*(P[6][3] + P[1][3]*SF[1] + P[3][3]*SF[0] + P[0][3]*SPP[0] - P[2][3]*SPP[1]) - SPP[0]*(P[6][1] + P[1][1]*SF[1] + P[3][1]*SF[0] + P[0][1]*SPP[0] - P[2][1]*SPP[1]); - nextP[6][6] = P[6][6] + P[1][6]*SF[1] + P[3][6]*SF[0] + P[0][6]*SPP[0] - P[2][6]*SPP[1] + dvxCov*sq(SG[6] - 2*q0*q2) + dvyCov*sq(SG[5] + 2*q0*q1) + SF[1]*(P[6][1] + P[1][1]*SF[1] + P[3][1]*SF[0] + P[0][1]*SPP[0] - P[2][1]*SPP[1]) + SF[0]*(P[6][3] + P[1][3]*SF[1] + P[3][3]*SF[0] + P[0][3]*SPP[0] - P[2][3]*SPP[1]) + SPP[0]*(P[6][0] + P[1][0]*SF[1] + P[3][0]*SF[0] + P[0][0]*SPP[0] - P[2][0]*SPP[1]) - SPP[1]*(P[6][2] + P[1][2]*SF[1] + P[3][2]*SF[0] + P[0][2]*SPP[0] - P[2][2]*SPP[1]) + dvzCov*sq(SG[1] - SG[2] - SG[3] + SG[4]); - nextP[6][7] = P[6][7] + P[1][7]*SF[1] + P[3][7]*SF[0] + P[0][7]*SPP[0] - P[2][7]*SPP[1] + dt*(P[6][4] + P[1][4]*SF[1] + P[3][4]*SF[0] + P[0][4]*SPP[0] - P[2][4]*SPP[1]); - nextP[6][8] = P[6][8] + P[1][8]*SF[1] + P[3][8]*SF[0] + P[0][8]*SPP[0] - P[2][8]*SPP[1] + dt*(P[6][5] + P[1][5]*SF[1] + P[3][5]*SF[0] + P[0][5]*SPP[0] - P[2][5]*SPP[1]); - nextP[6][9] = P[6][9] + P[1][9]*SF[1] + P[3][9]*SF[0] + P[0][9]*SPP[0] - P[2][9]*SPP[1] + dt*(P[6][6] + P[1][6]*SF[1] + P[3][6]*SF[0] + P[0][6]*SPP[0] - P[2][6]*SPP[1]); - nextP[6][10] = P[6][10] + P[1][10]*SF[1] + P[3][10]*SF[0] + P[0][10]*SPP[0] - P[2][10]*SPP[1]; - nextP[6][11] = P[6][11] + P[1][11]*SF[1] + P[3][11]*SF[0] + P[0][11]*SPP[0] - P[2][11]*SPP[1]; - nextP[6][12] = P[6][12] + P[1][12]*SF[1] + P[3][12]*SF[0] + P[0][12]*SPP[0] - P[2][12]*SPP[1]; - nextP[6][13] = P[6][13] + P[1][13]*SF[1] + P[3][13]*SF[0] + P[0][13]*SPP[0] - P[2][13]*SPP[1]; - nextP[6][14] = P[6][14] + P[1][14]*SF[1] + P[3][14]*SF[0] + P[0][14]*SPP[0] - P[2][14]*SPP[1]; - nextP[6][15] = P[6][15] + P[1][15]*SF[1] + P[3][15]*SF[0] + P[0][15]*SPP[0] - P[2][15]*SPP[1]; - nextP[6][16] = P[6][16] + P[1][16]*SF[1] + P[3][16]*SF[0] + P[0][16]*SPP[0] - P[2][16]*SPP[1]; - nextP[6][17] = P[6][17] + P[1][17]*SF[1] + P[3][17]*SF[0] + P[0][17]*SPP[0] - P[2][17]*SPP[1]; - nextP[6][18] = P[6][18] + P[1][18]*SF[1] + P[3][18]*SF[0] + P[0][18]*SPP[0] - P[2][18]*SPP[1]; - nextP[6][19] = P[6][19] + P[1][19]*SF[1] + P[3][19]*SF[0] + P[0][19]*SPP[0] - P[2][19]*SPP[1]; - nextP[6][20] = P[6][20] + P[1][20]*SF[1] + P[3][20]*SF[0] + P[0][20]*SPP[0] - P[2][20]*SPP[1]; - nextP[7][0] = P[7][0] + P[4][0]*dt + SF[6]*(P[7][1] + P[4][1]*dt) + SPP[7]*(P[7][2] + P[4][2]*dt) + SPP[6]*(P[7][3] + P[4][3]*dt) + SPP[5]*(P[7][10] + P[4][10]*dt) + SPP[4]*(P[7][11] + P[4][11]*dt) + SPP[3]*(P[7][12] + P[4][12]*dt); - nextP[7][1] = P[7][1] + P[4][1]*dt + SF[5]*(P[7][0] + P[4][0]*dt) + SF[4]*(P[7][2] + P[4][2]*dt) + SPP[7]*(P[7][3] + P[4][3]*dt) + SPP[3]*(P[7][11] + P[4][11]*dt) - SPP[4]*(P[7][12] + P[4][12]*dt) - (q0*(P[7][10] + P[4][10]*dt))/2; - nextP[7][2] = P[7][2] + P[4][2]*dt + SF[3]*(P[7][0] + P[4][0]*dt) + SF[5]*(P[7][3] + P[4][3]*dt) + SPP[6]*(P[7][1] + P[4][1]*dt) - SPP[3]*(P[7][10] + P[4][10]*dt) + SPP[5]*(P[7][12] + P[4][12]*dt) - (q0*(P[7][11] + P[4][11]*dt))/2; - nextP[7][3] = P[7][3] + P[4][3]*dt + SF[4]*(P[7][0] + P[4][0]*dt) + SF[3]*(P[7][1] + P[4][1]*dt) + SF[6]*(P[7][2] + P[4][2]*dt) + SPP[4]*(P[7][10] + P[4][10]*dt) - SPP[5]*(P[7][11] + P[4][11]*dt) - (q0*(P[7][12] + P[4][12]*dt))/2; - nextP[7][4] = P[7][4] + P[4][4]*dt + SF[0]*(P[7][1] + P[4][1]*dt) + SF[2]*(P[7][0] + P[4][0]*dt) + SPP[0]*(P[7][2] + P[4][2]*dt) - SPP[2]*(P[7][3] + P[4][3]*dt); - nextP[7][5] = P[7][5] + P[4][5]*dt + SF[1]*(P[7][0] + P[4][0]*dt) + SF[0]*(P[7][2] + P[4][2]*dt) + SF[2]*(P[7][3] + P[4][3]*dt) - SPP[0]*(P[7][1] + P[4][1]*dt); - nextP[7][6] = P[7][6] + P[4][6]*dt + SF[1]*(P[7][1] + P[4][1]*dt) + SF[0]*(P[7][3] + P[4][3]*dt) + SPP[0]*(P[7][0] + P[4][0]*dt) - SPP[1]*(P[7][2] + P[4][2]*dt); - nextP[7][7] = P[7][7] + P[4][7]*dt + dt*(P[7][4] + P[4][4]*dt); - nextP[7][8] = P[7][8] + P[4][8]*dt + dt*(P[7][5] + P[4][5]*dt); - nextP[7][9] = P[7][9] + P[4][9]*dt + dt*(P[7][6] + P[4][6]*dt); - nextP[7][10] = P[7][10] + P[4][10]*dt; - nextP[7][11] = P[7][11] + P[4][11]*dt; - nextP[7][12] = P[7][12] + P[4][12]*dt; - nextP[7][13] = P[7][13] + P[4][13]*dt; - nextP[7][14] = P[7][14] + P[4][14]*dt; - nextP[7][15] = P[7][15] + P[4][15]*dt; - nextP[7][16] = P[7][16] + P[4][16]*dt; - nextP[7][17] = P[7][17] + P[4][17]*dt; - nextP[7][18] = P[7][18] + P[4][18]*dt; - nextP[7][19] = P[7][19] + P[4][19]*dt; - nextP[7][20] = P[7][20] + P[4][20]*dt; - nextP[8][0] = P[8][0] + P[5][0]*dt + SF[6]*(P[8][1] + P[5][1]*dt) + SPP[7]*(P[8][2] + P[5][2]*dt) + SPP[6]*(P[8][3] + P[5][3]*dt) + SPP[5]*(P[8][10] + P[5][10]*dt) + SPP[4]*(P[8][11] + P[5][11]*dt) + SPP[3]*(P[8][12] + P[5][12]*dt); - nextP[8][1] = P[8][1] + P[5][1]*dt + SF[5]*(P[8][0] + P[5][0]*dt) + SF[4]*(P[8][2] + P[5][2]*dt) + SPP[7]*(P[8][3] + P[5][3]*dt) + SPP[3]*(P[8][11] + P[5][11]*dt) - SPP[4]*(P[8][12] + P[5][12]*dt) - (q0*(P[8][10] + P[5][10]*dt))/2; - nextP[8][2] = P[8][2] + P[5][2]*dt + SF[3]*(P[8][0] + P[5][0]*dt) + SF[5]*(P[8][3] + P[5][3]*dt) + SPP[6]*(P[8][1] + P[5][1]*dt) - SPP[3]*(P[8][10] + P[5][10]*dt) + SPP[5]*(P[8][12] + P[5][12]*dt) - (q0*(P[8][11] + P[5][11]*dt))/2; - nextP[8][3] = P[8][3] + P[5][3]*dt + SF[4]*(P[8][0] + P[5][0]*dt) + SF[3]*(P[8][1] + P[5][1]*dt) + SF[6]*(P[8][2] + P[5][2]*dt) + SPP[4]*(P[8][10] + P[5][10]*dt) - SPP[5]*(P[8][11] + P[5][11]*dt) - (q0*(P[8][12] + P[5][12]*dt))/2; - nextP[8][4] = P[8][4] + P[5][4]*dt + SF[0]*(P[8][1] + P[5][1]*dt) + SF[2]*(P[8][0] + P[5][0]*dt) + SPP[0]*(P[8][2] + P[5][2]*dt) - SPP[2]*(P[8][3] + P[5][3]*dt); - nextP[8][5] = P[8][5] + P[5][5]*dt + SF[1]*(P[8][0] + P[5][0]*dt) + SF[0]*(P[8][2] + P[5][2]*dt) + SF[2]*(P[8][3] + P[5][3]*dt) - SPP[0]*(P[8][1] + P[5][1]*dt); - nextP[8][6] = P[8][6] + P[5][6]*dt + SF[1]*(P[8][1] + P[5][1]*dt) + SF[0]*(P[8][3] + P[5][3]*dt) + SPP[0]*(P[8][0] + P[5][0]*dt) - SPP[1]*(P[8][2] + P[5][2]*dt); - nextP[8][7] = P[8][7] + P[5][7]*dt + dt*(P[8][4] + P[5][4]*dt); - nextP[8][8] = P[8][8] + P[5][8]*dt + dt*(P[8][5] + P[5][5]*dt); - nextP[8][9] = P[8][9] + P[5][9]*dt + dt*(P[8][6] + P[5][6]*dt); - nextP[8][10] = P[8][10] + P[5][10]*dt; - nextP[8][11] = P[8][11] + P[5][11]*dt; - nextP[8][12] = P[8][12] + P[5][12]*dt; - nextP[8][13] = P[8][13] + P[5][13]*dt; - nextP[8][14] = P[8][14] + P[5][14]*dt; - nextP[8][15] = P[8][15] + P[5][15]*dt; - nextP[8][16] = P[8][16] + P[5][16]*dt; - nextP[8][17] = P[8][17] + P[5][17]*dt; - nextP[8][18] = P[8][18] + P[5][18]*dt; - nextP[8][19] = P[8][19] + P[5][19]*dt; - nextP[8][20] = P[8][20] + P[5][20]*dt; - nextP[9][0] = P[9][0] + P[6][0]*dt + SF[6]*(P[9][1] + P[6][1]*dt) + SPP[7]*(P[9][2] + P[6][2]*dt) + SPP[6]*(P[9][3] + P[6][3]*dt) + SPP[5]*(P[9][10] + P[6][10]*dt) + SPP[4]*(P[9][11] + P[6][11]*dt) + SPP[3]*(P[9][12] + P[6][12]*dt); - nextP[9][1] = P[9][1] + P[6][1]*dt + SF[5]*(P[9][0] + P[6][0]*dt) + SF[4]*(P[9][2] + P[6][2]*dt) + SPP[7]*(P[9][3] + P[6][3]*dt) + SPP[3]*(P[9][11] + P[6][11]*dt) - SPP[4]*(P[9][12] + P[6][12]*dt) - (q0*(P[9][10] + P[6][10]*dt))/2; - nextP[9][2] = P[9][2] + P[6][2]*dt + SF[3]*(P[9][0] + P[6][0]*dt) + SF[5]*(P[9][3] + P[6][3]*dt) + SPP[6]*(P[9][1] + P[6][1]*dt) - SPP[3]*(P[9][10] + P[6][10]*dt) + SPP[5]*(P[9][12] + P[6][12]*dt) - (q0*(P[9][11] + P[6][11]*dt))/2; - nextP[9][3] = P[9][3] + P[6][3]*dt + SF[4]*(P[9][0] + P[6][0]*dt) + SF[3]*(P[9][1] + P[6][1]*dt) + SF[6]*(P[9][2] + P[6][2]*dt) + SPP[4]*(P[9][10] + P[6][10]*dt) - SPP[5]*(P[9][11] + P[6][11]*dt) - (q0*(P[9][12] + P[6][12]*dt))/2; - nextP[9][4] = P[9][4] + P[6][4]*dt + SF[0]*(P[9][1] + P[6][1]*dt) + SF[2]*(P[9][0] + P[6][0]*dt) + SPP[0]*(P[9][2] + P[6][2]*dt) - SPP[2]*(P[9][3] + P[6][3]*dt); - nextP[9][5] = P[9][5] + P[6][5]*dt + SF[1]*(P[9][0] + P[6][0]*dt) + SF[0]*(P[9][2] + P[6][2]*dt) + SF[2]*(P[9][3] + P[6][3]*dt) - SPP[0]*(P[9][1] + P[6][1]*dt); - nextP[9][6] = P[9][6] + P[6][6]*dt + SF[1]*(P[9][1] + P[6][1]*dt) + SF[0]*(P[9][3] + P[6][3]*dt) + SPP[0]*(P[9][0] + P[6][0]*dt) - SPP[1]*(P[9][2] + P[6][2]*dt); - nextP[9][7] = P[9][7] + P[6][7]*dt + dt*(P[9][4] + P[6][4]*dt); - nextP[9][8] = P[9][8] + P[6][8]*dt + dt*(P[9][5] + P[6][5]*dt); - nextP[9][9] = P[9][9] + P[6][9]*dt + dt*(P[9][6] + P[6][6]*dt); - nextP[9][10] = P[9][10] + P[6][10]*dt; - nextP[9][11] = P[9][11] + P[6][11]*dt; - nextP[9][12] = P[9][12] + P[6][12]*dt; - nextP[9][13] = P[9][13] + P[6][13]*dt; - nextP[9][14] = P[9][14] + P[6][14]*dt; - nextP[9][15] = P[9][15] + P[6][15]*dt; - nextP[9][16] = P[9][16] + P[6][16]*dt; - nextP[9][17] = P[9][17] + P[6][17]*dt; - nextP[9][18] = P[9][18] + P[6][18]*dt; - nextP[9][19] = P[9][19] + P[6][19]*dt; - nextP[9][20] = P[9][20] + P[6][20]*dt; - nextP[10][0] = P[10][0] + P[10][1]*SF[6] + P[10][2]*SPP[7] + P[10][3]*SPP[6] + P[10][10]*SPP[5] + P[10][11]*SPP[4] + P[10][12]*SPP[3]; - nextP[10][1] = P[10][1] + P[10][0]*SF[5] + P[10][2]*SF[4] + P[10][3]*SPP[7] + P[10][11]*SPP[3] - P[10][12]*SPP[4] - (P[10][10]*q0)/2; - nextP[10][2] = P[10][2] + P[10][0]*SF[3] + P[10][3]*SF[5] + P[10][1]*SPP[6] - P[10][10]*SPP[3] + P[10][12]*SPP[5] - (P[10][11]*q0)/2; - nextP[10][3] = P[10][3] + P[10][0]*SF[4] + P[10][1]*SF[3] + P[10][2]*SF[6] + P[10][10]*SPP[4] - P[10][11]*SPP[5] - (P[10][12]*q0)/2; - nextP[10][4] = P[10][4] + P[10][1]*SF[0] + P[10][0]*SF[2] + P[10][2]*SPP[0] - P[10][3]*SPP[2]; - nextP[10][5] = P[10][5] + P[10][0]*SF[1] + P[10][2]*SF[0] + P[10][3]*SF[2] - P[10][1]*SPP[0]; - nextP[10][6] = P[10][6] + P[10][1]*SF[1] + P[10][3]*SF[0] + P[10][0]*SPP[0] - P[10][2]*SPP[1]; - nextP[10][7] = P[10][7] + P[10][4]*dt; - nextP[10][8] = P[10][8] + P[10][5]*dt; - nextP[10][9] = P[10][9] + P[10][6]*dt; - nextP[10][10] = P[10][10]; - nextP[10][11] = P[10][11]; - nextP[10][12] = P[10][12]; - nextP[10][13] = P[10][13]; - nextP[10][14] = P[10][14]; - nextP[10][15] = P[10][15]; - nextP[10][16] = P[10][16]; - nextP[10][17] = P[10][17]; - nextP[10][18] = P[10][18]; - nextP[10][19] = P[10][19]; - nextP[10][20] = P[10][20]; - nextP[11][0] = P[11][0] + P[11][1]*SF[6] + P[11][2]*SPP[7] + P[11][3]*SPP[6] + P[11][10]*SPP[5] + P[11][11]*SPP[4] + P[11][12]*SPP[3]; - nextP[11][1] = P[11][1] + P[11][0]*SF[5] + P[11][2]*SF[4] + P[11][3]*SPP[7] + P[11][11]*SPP[3] - P[11][12]*SPP[4] - (P[11][10]*q0)/2; - nextP[11][2] = P[11][2] + P[11][0]*SF[3] + P[11][3]*SF[5] + P[11][1]*SPP[6] - P[11][10]*SPP[3] + P[11][12]*SPP[5] - (P[11][11]*q0)/2; - nextP[11][3] = P[11][3] + P[11][0]*SF[4] + P[11][1]*SF[3] + P[11][2]*SF[6] + P[11][10]*SPP[4] - P[11][11]*SPP[5] - (P[11][12]*q0)/2; - nextP[11][4] = P[11][4] + P[11][1]*SF[0] + P[11][0]*SF[2] + P[11][2]*SPP[0] - P[11][3]*SPP[2]; - nextP[11][5] = P[11][5] + P[11][0]*SF[1] + P[11][2]*SF[0] + P[11][3]*SF[2] - P[11][1]*SPP[0]; - nextP[11][6] = P[11][6] + P[11][1]*SF[1] + P[11][3]*SF[0] + P[11][0]*SPP[0] - P[11][2]*SPP[1]; - nextP[11][7] = P[11][7] + P[11][4]*dt; - nextP[11][8] = P[11][8] + P[11][5]*dt; - nextP[11][9] = P[11][9] + P[11][6]*dt; - nextP[11][10] = P[11][10]; - nextP[11][11] = P[11][11]; - nextP[11][12] = P[11][12]; - nextP[11][13] = P[11][13]; - nextP[11][14] = P[11][14]; - nextP[11][15] = P[11][15]; - nextP[11][16] = P[11][16]; - nextP[11][17] = P[11][17]; - nextP[11][18] = P[11][18]; - nextP[11][19] = P[11][19]; - nextP[11][20] = P[11][20]; - nextP[12][0] = P[12][0] + P[12][1]*SF[6] + P[12][2]*SPP[7] + P[12][3]*SPP[6] + P[12][10]*SPP[5] + P[12][11]*SPP[4] + P[12][12]*SPP[3]; - nextP[12][1] = P[12][1] + P[12][0]*SF[5] + P[12][2]*SF[4] + P[12][3]*SPP[7] + P[12][11]*SPP[3] - P[12][12]*SPP[4] - (P[12][10]*q0)/2; - nextP[12][2] = P[12][2] + P[12][0]*SF[3] + P[12][3]*SF[5] + P[12][1]*SPP[6] - P[12][10]*SPP[3] + P[12][12]*SPP[5] - (P[12][11]*q0)/2; - nextP[12][3] = P[12][3] + P[12][0]*SF[4] + P[12][1]*SF[3] + P[12][2]*SF[6] + P[12][10]*SPP[4] - P[12][11]*SPP[5] - (P[12][12]*q0)/2; - nextP[12][4] = P[12][4] + P[12][1]*SF[0] + P[12][0]*SF[2] + P[12][2]*SPP[0] - P[12][3]*SPP[2]; - nextP[12][5] = P[12][5] + P[12][0]*SF[1] + P[12][2]*SF[0] + P[12][3]*SF[2] - P[12][1]*SPP[0]; - nextP[12][6] = P[12][6] + P[12][1]*SF[1] + P[12][3]*SF[0] + P[12][0]*SPP[0] - P[12][2]*SPP[1]; - nextP[12][7] = P[12][7] + P[12][4]*dt; - nextP[12][8] = P[12][8] + P[12][5]*dt; - nextP[12][9] = P[12][9] + P[12][6]*dt; - nextP[12][10] = P[12][10]; - nextP[12][11] = P[12][11]; - nextP[12][12] = P[12][12]; - nextP[12][13] = P[12][13]; - nextP[12][14] = P[12][14]; - nextP[12][15] = P[12][15]; - nextP[12][16] = P[12][16]; - nextP[12][17] = P[12][17]; - nextP[12][18] = P[12][18]; - nextP[12][19] = P[12][19]; - nextP[12][20] = P[12][20]; - nextP[13][0] = P[13][0] + P[13][1]*SF[6] + P[13][2]*SPP[7] + P[13][3]*SPP[6] + P[13][10]*SPP[5] + P[13][11]*SPP[4] + P[13][12]*SPP[3]; - nextP[13][1] = P[13][1] + P[13][0]*SF[5] + P[13][2]*SF[4] + P[13][3]*SPP[7] + P[13][11]*SPP[3] - P[13][12]*SPP[4] - (P[13][10]*q0)/2; - nextP[13][2] = P[13][2] + P[13][0]*SF[3] + P[13][3]*SF[5] + P[13][1]*SPP[6] - P[13][10]*SPP[3] + P[13][12]*SPP[5] - (P[13][11]*q0)/2; - nextP[13][3] = P[13][3] + P[13][0]*SF[4] + P[13][1]*SF[3] + P[13][2]*SF[6] + P[13][10]*SPP[4] - P[13][11]*SPP[5] - (P[13][12]*q0)/2; - nextP[13][4] = P[13][4] + P[13][1]*SF[0] + P[13][0]*SF[2] + P[13][2]*SPP[0] - P[13][3]*SPP[2]; - nextP[13][5] = P[13][5] + P[13][0]*SF[1] + P[13][2]*SF[0] + P[13][3]*SF[2] - P[13][1]*SPP[0]; - nextP[13][6] = P[13][6] + P[13][1]*SF[1] + P[13][3]*SF[0] + P[13][0]*SPP[0] - P[13][2]*SPP[1]; - nextP[13][7] = P[13][7] + P[13][4]*dt; - nextP[13][8] = P[13][8] + P[13][5]*dt; - nextP[13][9] = P[13][9] + P[13][6]*dt; - nextP[13][10] = P[13][10]; - nextP[13][11] = P[13][11]; - nextP[13][12] = P[13][12]; - nextP[13][13] = P[13][13]; - nextP[13][14] = P[13][14]; - nextP[13][15] = P[13][15]; - nextP[13][16] = P[13][16]; - nextP[13][17] = P[13][17]; - nextP[13][18] = P[13][18]; - nextP[13][19] = P[13][19]; - nextP[13][20] = P[13][20]; - nextP[14][0] = P[14][0] + P[14][1]*SF[6] + P[14][2]*SPP[7] + P[14][3]*SPP[6] + P[14][10]*SPP[5] + P[14][11]*SPP[4] + P[14][12]*SPP[3]; - nextP[14][1] = P[14][1] + P[14][0]*SF[5] + P[14][2]*SF[4] + P[14][3]*SPP[7] + P[14][11]*SPP[3] - P[14][12]*SPP[4] - (P[14][10]*q0)/2; - nextP[14][2] = P[14][2] + P[14][0]*SF[3] + P[14][3]*SF[5] + P[14][1]*SPP[6] - P[14][10]*SPP[3] + P[14][12]*SPP[5] - (P[14][11]*q0)/2; - nextP[14][3] = P[14][3] + P[14][0]*SF[4] + P[14][1]*SF[3] + P[14][2]*SF[6] + P[14][10]*SPP[4] - P[14][11]*SPP[5] - (P[14][12]*q0)/2; - nextP[14][4] = P[14][4] + P[14][1]*SF[0] + P[14][0]*SF[2] + P[14][2]*SPP[0] - P[14][3]*SPP[2]; - nextP[14][5] = P[14][5] + P[14][0]*SF[1] + P[14][2]*SF[0] + P[14][3]*SF[2] - P[14][1]*SPP[0]; - nextP[14][6] = P[14][6] + P[14][1]*SF[1] + P[14][3]*SF[0] + P[14][0]*SPP[0] - P[14][2]*SPP[1]; - nextP[14][7] = P[14][7] + P[14][4]*dt; - nextP[14][8] = P[14][8] + P[14][5]*dt; - nextP[14][9] = P[14][9] + P[14][6]*dt; - nextP[14][10] = P[14][10]; - nextP[14][11] = P[14][11]; - nextP[14][12] = P[14][12]; - nextP[14][13] = P[14][13]; - nextP[14][14] = P[14][14]; - nextP[14][15] = P[14][15]; - nextP[14][16] = P[14][16]; - nextP[14][17] = P[14][17]; - nextP[14][18] = P[14][18]; - nextP[14][19] = P[14][19]; - nextP[14][20] = P[14][20]; - nextP[15][0] = P[15][0] + P[15][1]*SF[6] + P[15][2]*SPP[7] + P[15][3]*SPP[6] + P[15][10]*SPP[5] + P[15][11]*SPP[4] + P[15][12]*SPP[3]; - nextP[15][1] = P[15][1] + P[15][0]*SF[5] + P[15][2]*SF[4] + P[15][3]*SPP[7] + P[15][11]*SPP[3] - P[15][12]*SPP[4] - (P[15][10]*q0)/2; - nextP[15][2] = P[15][2] + P[15][0]*SF[3] + P[15][3]*SF[5] + P[15][1]*SPP[6] - P[15][10]*SPP[3] + P[15][12]*SPP[5] - (P[15][11]*q0)/2; - nextP[15][3] = P[15][3] + P[15][0]*SF[4] + P[15][1]*SF[3] + P[15][2]*SF[6] + P[15][10]*SPP[4] - P[15][11]*SPP[5] - (P[15][12]*q0)/2; - nextP[15][4] = P[15][4] + P[15][1]*SF[0] + P[15][0]*SF[2] + P[15][2]*SPP[0] - P[15][3]*SPP[2]; - nextP[15][5] = P[15][5] + P[15][0]*SF[1] + P[15][2]*SF[0] + P[15][3]*SF[2] - P[15][1]*SPP[0]; - nextP[15][6] = P[15][6] + P[15][1]*SF[1] + P[15][3]*SF[0] + P[15][0]*SPP[0] - P[15][2]*SPP[1]; - nextP[15][7] = P[15][7] + P[15][4]*dt; - nextP[15][8] = P[15][8] + P[15][5]*dt; - nextP[15][9] = P[15][9] + P[15][6]*dt; - nextP[15][10] = P[15][10]; - nextP[15][11] = P[15][11]; - nextP[15][12] = P[15][12]; - nextP[15][13] = P[15][13]; - nextP[15][14] = P[15][14]; - nextP[15][15] = P[15][15]; - nextP[15][16] = P[15][16]; - nextP[15][17] = P[15][17]; - nextP[15][18] = P[15][18]; - nextP[15][19] = P[15][19]; - nextP[15][20] = P[15][20]; - nextP[16][0] = P[16][0] + P[16][1]*SF[6] + P[16][2]*SPP[7] + P[16][3]*SPP[6] + P[16][10]*SPP[5] + P[16][11]*SPP[4] + P[16][12]*SPP[3]; - nextP[16][1] = P[16][1] + P[16][0]*SF[5] + P[16][2]*SF[4] + P[16][3]*SPP[7] + P[16][11]*SPP[3] - P[16][12]*SPP[4] - (P[16][10]*q0)/2; - nextP[16][2] = P[16][2] + P[16][0]*SF[3] + P[16][3]*SF[5] + P[16][1]*SPP[6] - P[16][10]*SPP[3] + P[16][12]*SPP[5] - (P[16][11]*q0)/2; - nextP[16][3] = P[16][3] + P[16][0]*SF[4] + P[16][1]*SF[3] + P[16][2]*SF[6] + P[16][10]*SPP[4] - P[16][11]*SPP[5] - (P[16][12]*q0)/2; - nextP[16][4] = P[16][4] + P[16][1]*SF[0] + P[16][0]*SF[2] + P[16][2]*SPP[0] - P[16][3]*SPP[2]; - nextP[16][5] = P[16][5] + P[16][0]*SF[1] + P[16][2]*SF[0] + P[16][3]*SF[2] - P[16][1]*SPP[0]; - nextP[16][6] = P[16][6] + P[16][1]*SF[1] + P[16][3]*SF[0] + P[16][0]*SPP[0] - P[16][2]*SPP[1]; - nextP[16][7] = P[16][7] + P[16][4]*dt; - nextP[16][8] = P[16][8] + P[16][5]*dt; - nextP[16][9] = P[16][9] + P[16][6]*dt; - nextP[16][10] = P[16][10]; - nextP[16][11] = P[16][11]; - nextP[16][12] = P[16][12]; - nextP[16][13] = P[16][13]; - nextP[16][14] = P[16][14]; - nextP[16][15] = P[16][15]; - nextP[16][16] = P[16][16]; - nextP[16][17] = P[16][17]; - nextP[16][18] = P[16][18]; - nextP[16][19] = P[16][19]; - nextP[16][20] = P[16][20]; - nextP[17][0] = P[17][0] + P[17][1]*SF[6] + P[17][2]*SPP[7] + P[17][3]*SPP[6] + P[17][10]*SPP[5] + P[17][11]*SPP[4] + P[17][12]*SPP[3]; - nextP[17][1] = P[17][1] + P[17][0]*SF[5] + P[17][2]*SF[4] + P[17][3]*SPP[7] + P[17][11]*SPP[3] - P[17][12]*SPP[4] - (P[17][10]*q0)/2; - nextP[17][2] = P[17][2] + P[17][0]*SF[3] + P[17][3]*SF[5] + P[17][1]*SPP[6] - P[17][10]*SPP[3] + P[17][12]*SPP[5] - (P[17][11]*q0)/2; - nextP[17][3] = P[17][3] + P[17][0]*SF[4] + P[17][1]*SF[3] + P[17][2]*SF[6] + P[17][10]*SPP[4] - P[17][11]*SPP[5] - (P[17][12]*q0)/2; - nextP[17][4] = P[17][4] + P[17][1]*SF[0] + P[17][0]*SF[2] + P[17][2]*SPP[0] - P[17][3]*SPP[2]; - nextP[17][5] = P[17][5] + P[17][0]*SF[1] + P[17][2]*SF[0] + P[17][3]*SF[2] - P[17][1]*SPP[0]; - nextP[17][6] = P[17][6] + P[17][1]*SF[1] + P[17][3]*SF[0] + P[17][0]*SPP[0] - P[17][2]*SPP[1]; - nextP[17][7] = P[17][7] + P[17][4]*dt; - nextP[17][8] = P[17][8] + P[17][5]*dt; - nextP[17][9] = P[17][9] + P[17][6]*dt; - nextP[17][10] = P[17][10]; - nextP[17][11] = P[17][11]; - nextP[17][12] = P[17][12]; - nextP[17][13] = P[17][13]; - nextP[17][14] = P[17][14]; - nextP[17][15] = P[17][15]; - nextP[17][16] = P[17][16]; - nextP[17][17] = P[17][17]; - nextP[17][18] = P[17][18]; - nextP[17][19] = P[17][19]; - nextP[17][20] = P[17][20]; - nextP[18][0] = P[18][0] + P[18][1]*SF[6] + P[18][2]*SPP[7] + P[18][3]*SPP[6] + P[18][10]*SPP[5] + P[18][11]*SPP[4] + P[18][12]*SPP[3]; - nextP[18][1] = P[18][1] + P[18][0]*SF[5] + P[18][2]*SF[4] + P[18][3]*SPP[7] + P[18][11]*SPP[3] - P[18][12]*SPP[4] - (P[18][10]*q0)/2; - nextP[18][2] = P[18][2] + P[18][0]*SF[3] + P[18][3]*SF[5] + P[18][1]*SPP[6] - P[18][10]*SPP[3] + P[18][12]*SPP[5] - (P[18][11]*q0)/2; - nextP[18][3] = P[18][3] + P[18][0]*SF[4] + P[18][1]*SF[3] + P[18][2]*SF[6] + P[18][10]*SPP[4] - P[18][11]*SPP[5] - (P[18][12]*q0)/2; - nextP[18][4] = P[18][4] + P[18][1]*SF[0] + P[18][0]*SF[2] + P[18][2]*SPP[0] - P[18][3]*SPP[2]; - nextP[18][5] = P[18][5] + P[18][0]*SF[1] + P[18][2]*SF[0] + P[18][3]*SF[2] - P[18][1]*SPP[0]; - nextP[18][6] = P[18][6] + P[18][1]*SF[1] + P[18][3]*SF[0] + P[18][0]*SPP[0] - P[18][2]*SPP[1]; - nextP[18][7] = P[18][7] + P[18][4]*dt; - nextP[18][8] = P[18][8] + P[18][5]*dt; - nextP[18][9] = P[18][9] + P[18][6]*dt; - nextP[18][10] = P[18][10]; - nextP[18][11] = P[18][11]; - nextP[18][12] = P[18][12]; - nextP[18][13] = P[18][13]; - nextP[18][14] = P[18][14]; - nextP[18][15] = P[18][15]; - nextP[18][16] = P[18][16]; - nextP[18][17] = P[18][17]; - nextP[18][18] = P[18][18]; - nextP[18][19] = P[18][19]; - nextP[18][20] = P[18][20]; - nextP[19][0] = P[19][0] + P[19][1]*SF[6] + P[19][2]*SPP[7] + P[19][3]*SPP[6] + P[19][10]*SPP[5] + P[19][11]*SPP[4] + P[19][12]*SPP[3]; - nextP[19][1] = P[19][1] + P[19][0]*SF[5] + P[19][2]*SF[4] + P[19][3]*SPP[7] + P[19][11]*SPP[3] - P[19][12]*SPP[4] - (P[19][10]*q0)/2; - nextP[19][2] = P[19][2] + P[19][0]*SF[3] + P[19][3]*SF[5] + P[19][1]*SPP[6] - P[19][10]*SPP[3] + P[19][12]*SPP[5] - (P[19][11]*q0)/2; - nextP[19][3] = P[19][3] + P[19][0]*SF[4] + P[19][1]*SF[3] + P[19][2]*SF[6] + P[19][10]*SPP[4] - P[19][11]*SPP[5] - (P[19][12]*q0)/2; - nextP[19][4] = P[19][4] + P[19][1]*SF[0] + P[19][0]*SF[2] + P[19][2]*SPP[0] - P[19][3]*SPP[2]; - nextP[19][5] = P[19][5] + P[19][0]*SF[1] + P[19][2]*SF[0] + P[19][3]*SF[2] - P[19][1]*SPP[0]; - nextP[19][6] = P[19][6] + P[19][1]*SF[1] + P[19][3]*SF[0] + P[19][0]*SPP[0] - P[19][2]*SPP[1]; - nextP[19][7] = P[19][7] + P[19][4]*dt; - nextP[19][8] = P[19][8] + P[19][5]*dt; - nextP[19][9] = P[19][9] + P[19][6]*dt; - nextP[19][10] = P[19][10]; - nextP[19][11] = P[19][11]; - nextP[19][12] = P[19][12]; - nextP[19][13] = P[19][13]; - nextP[19][14] = P[19][14]; - nextP[19][15] = P[19][15]; - nextP[19][16] = P[19][16]; - nextP[19][17] = P[19][17]; - nextP[19][18] = P[19][18]; - nextP[19][19] = P[19][19]; - nextP[19][20] = P[19][20]; - nextP[20][0] = P[20][0] + P[20][1]*SF[6] + P[20][2]*SPP[7] + P[20][3]*SPP[6] + P[20][10]*SPP[5] + P[20][11]*SPP[4] + P[20][12]*SPP[3]; - nextP[20][1] = P[20][1] + P[20][0]*SF[5] + P[20][2]*SF[4] + P[20][3]*SPP[7] + P[20][11]*SPP[3] - P[20][12]*SPP[4] - (P[20][10]*q0)/2; - nextP[20][2] = P[20][2] + P[20][0]*SF[3] + P[20][3]*SF[5] + P[20][1]*SPP[6] - P[20][10]*SPP[3] + P[20][12]*SPP[5] - (P[20][11]*q0)/2; - nextP[20][3] = P[20][3] + P[20][0]*SF[4] + P[20][1]*SF[3] + P[20][2]*SF[6] + P[20][10]*SPP[4] - P[20][11]*SPP[5] - (P[20][12]*q0)/2; - nextP[20][4] = P[20][4] + P[20][1]*SF[0] + P[20][0]*SF[2] + P[20][2]*SPP[0] - P[20][3]*SPP[2]; - nextP[20][5] = P[20][5] + P[20][0]*SF[1] + P[20][2]*SF[0] + P[20][3]*SF[2] - P[20][1]*SPP[0]; - nextP[20][6] = P[20][6] + P[20][1]*SF[1] + P[20][3]*SF[0] + P[20][0]*SPP[0] - P[20][2]*SPP[1]; - nextP[20][7] = P[20][7] + P[20][4]*dt; - nextP[20][8] = P[20][8] + P[20][5]*dt; - nextP[20][9] = P[20][9] + P[20][6]*dt; - nextP[20][10] = P[20][10]; - nextP[20][11] = P[20][11]; - nextP[20][12] = P[20][12]; - nextP[20][13] = P[20][13]; - nextP[20][14] = P[20][14]; - nextP[20][15] = P[20][15]; - nextP[20][16] = P[20][16]; - nextP[20][17] = P[20][17]; - nextP[20][18] = P[20][18]; - nextP[20][19] = P[20][19]; - nextP[20][20] = P[20][20]; - - for (unsigned i = 0; i < n_states; i++) - { - nextP[i][i] = nextP[i][i] + processNoise[i]; - } - - // If on ground or no magnetometer fitted, inhibit magnetometer bias updates by - // setting the coresponding covariance terms to zero. - if (onGround || !useCompass) - { - zeroRows(nextP,15,20); - zeroCols(nextP,15,20); - } - - // If on ground or not using airspeed sensing, inhibit wind velocity - // covariance growth. - if (onGround || !useAirspeed) - { - zeroRows(nextP,13,14); - zeroCols(nextP,13,14); - } - - // If the total position variance exceds 1E6 (1000m), then stop covariance - // growth by setting the predicted to the previous values - // This prevent an ill conditioned matrix from occurring for long periods - // without GPS - if ((P[7][7] + P[8][8]) > 1E6f) - { - for (uint8_t i=7; i<=8; i++) - { - for (unsigned j = 0; j < n_states; j++) - { - nextP[i][j] = P[i][j]; - nextP[j][i] = P[j][i]; - } - } - } - - if (onGround || staticMode) { - // copy the portion of the variances we want to - // propagate - for (unsigned i = 0; i < 14; i++) { - P[i][i] = nextP[i][i]; - - // force symmetry for observable states - // force zero for non-observable states - for (unsigned i = 1; i < n_states; i++) - { - for (uint8_t j = 0; j < i; j++) - { - if ((i > 12) || (j > 12)) { - P[i][j] = 0.0f; - } else { - P[i][j] = 0.5f * (nextP[i][j] + nextP[j][i]); - } - P[j][i] = P[i][j]; - } - } - } - - } else { - - // Copy covariance - for (unsigned i = 0; i < n_states; i++) { - P[i][i] = nextP[i][i]; - } - - // force symmetry for observable states - for (unsigned i = 1; i < n_states; i++) - { - for (uint8_t j = 0; j < i; j++) - { - P[i][j] = 0.5f * (nextP[i][j] + nextP[j][i]); - P[j][i] = P[i][j]; - } - } - - } - - ConstrainVariances(); -} - -void AttPosEKF::FuseVelposNED() -{ - -// declare variables used by fault isolation logic - uint32_t gpsRetryTime = 30000; // time in msec before GPS fusion will be retried following innovation consistency failure - uint32_t gpsRetryTimeNoTAS = 5000; // retry time if no TAS measurement available - uint32_t hgtRetryTime = 5000; // height measurement retry time - uint32_t horizRetryTime; - -// declare variables used to check measurement errors - float velInnov[3] = {0.0f,0.0f,0.0f}; - float posInnov[2] = {0.0f,0.0f}; - float hgtInnov = 0.0f; - -// declare variables used to control access to arrays - bool fuseData[6] = {false,false,false,false,false,false}; - uint8_t stateIndex; - uint8_t obsIndex; - uint8_t indexLimit; - -// declare variables used by state and covariance update calculations - float velErr; - float posErr; - float R_OBS[6]; - float observation[6]; - float SK; - float quatMag; - -// Perform sequential fusion of GPS measurements. This assumes that the -// errors in the different velocity and position components are -// uncorrelated which is not true, however in the absence of covariance -// data from the GPS receiver it is the only assumption we can make -// so we might as well take advantage of the computational efficiencies -// associated with sequential fusion - if (fuseVelData || fusePosData || fuseHgtData) - { - // set the GPS data timeout depending on whether airspeed data is present - if (useAirspeed) horizRetryTime = gpsRetryTime; - else horizRetryTime = gpsRetryTimeNoTAS; - - // Form the observation vector - for (uint8_t i=0; i<=2; i++) observation[i] = velNED[i]; - for (uint8_t i=3; i<=4; i++) observation[i] = posNE[i-3]; - observation[5] = -(hgtMea); - - // Estimate the GPS Velocity, GPS horiz position and height measurement variances. - velErr = 0.2f*accNavMag; // additional error in GPS velocities caused by manoeuvring - posErr = 0.2f*accNavMag; // additional error in GPS position caused by manoeuvring - R_OBS[0] = 0.04f + sq(velErr); - R_OBS[1] = R_OBS[0]; - R_OBS[2] = 0.08f + sq(velErr); - R_OBS[3] = R_OBS[2]; - R_OBS[4] = 4.0f + sq(posErr); - R_OBS[5] = 4.0f; - - // Set innovation variances to zero default - for (uint8_t i = 0; i<=5; i++) - { - varInnovVelPos[i] = 0.0f; - } - // calculate innovations and check GPS data validity using an innovation consistency check - if (fuseVelData) - { - // test velocity measurements - uint8_t imax = 2; - if (fusionModeGPS == 1) imax = 1; - for (uint8_t i = 0; i<=imax; i++) - { - velInnov[i] = statesAtVelTime[i+4] - velNED[i]; - stateIndex = 4 + i; - varInnovVelPos[i] = P[stateIndex][stateIndex] + R_OBS[i]; - } - // apply a 5-sigma threshold - current_ekf_state.velHealth = (sq(velInnov[0]) + sq(velInnov[1]) + sq(velInnov[2])) < 25.0f * (varInnovVelPos[0] + varInnovVelPos[1] + varInnovVelPos[2]); - current_ekf_state.velTimeout = (millis() - current_ekf_state.velFailTime) > horizRetryTime; - if (current_ekf_state.velHealth || current_ekf_state.velTimeout) - { - current_ekf_state.velHealth = true; - current_ekf_state.velFailTime = millis(); - } - else - { - current_ekf_state.velHealth = false; - } - } - if (fusePosData) - { - // test horizontal position measurements - posInnov[0] = statesAtPosTime[7] - posNE[0]; - posInnov[1] = statesAtPosTime[8] - posNE[1]; - varInnovVelPos[3] = P[7][7] + R_OBS[3]; - varInnovVelPos[4] = P[8][8] + R_OBS[4]; - // apply a 10-sigma threshold - current_ekf_state.posHealth = (sq(posInnov[0]) + sq(posInnov[1])) < 100.0f*(varInnovVelPos[3] + varInnovVelPos[4]); - current_ekf_state.posTimeout = (millis() - current_ekf_state.posFailTime) > horizRetryTime; - if (current_ekf_state.posHealth || current_ekf_state.posTimeout) - { - current_ekf_state.posHealth = true; - current_ekf_state.posFailTime = millis(); - } - else - { - current_ekf_state.posHealth = false; - } - } - // test height measurements - if (fuseHgtData) - { - hgtInnov = statesAtHgtTime[9] + hgtMea; - varInnovVelPos[5] = P[9][9] + R_OBS[5]; - // apply a 10-sigma threshold - current_ekf_state.hgtHealth = sq(hgtInnov) < 100.0f*varInnovVelPos[5]; - current_ekf_state.hgtTimeout = (millis() - current_ekf_state.hgtFailTime) > hgtRetryTime; - if (current_ekf_state.hgtHealth || current_ekf_state.hgtTimeout) - { - current_ekf_state.hgtHealth = true; - current_ekf_state.hgtFailTime = millis(); - } - else - { - current_ekf_state.hgtHealth = false; - } - } - // Set range for sequential fusion of velocity and position measurements depending - // on which data is available and its health - if (fuseVelData && fusionModeGPS == 0 && current_ekf_state.velHealth) - { - fuseData[0] = true; - fuseData[1] = true; - fuseData[2] = true; - } - if (fuseVelData && fusionModeGPS == 1 && current_ekf_state.velHealth) - { - fuseData[0] = true; - fuseData[1] = true; - } - if (fusePosData && fusionModeGPS <= 2 && current_ekf_state.posHealth) - { - fuseData[3] = true; - fuseData[4] = true; - } - if (fuseHgtData && current_ekf_state.hgtHealth) - { - fuseData[5] = true; - } - // Limit range of states modified when on ground - if(!onGround) - { - indexLimit = 20; - } - else - { - indexLimit = 12; - } - // Fuse measurements sequentially - for (obsIndex=0; obsIndex<=5; obsIndex++) - { - if (fuseData[obsIndex]) - { - stateIndex = 4 + obsIndex; - // Calculate the measurement innovation, using states from a - // different time coordinate if fusing height data - if (obsIndex >= 0 && obsIndex <= 2) - { - innovVelPos[obsIndex] = statesAtVelTime[stateIndex] - observation[obsIndex]; - } - else if (obsIndex == 3 || obsIndex == 4) - { - innovVelPos[obsIndex] = statesAtPosTime[stateIndex] - observation[obsIndex]; - } - else if (obsIndex == 5) - { - innovVelPos[obsIndex] = statesAtHgtTime[stateIndex] - observation[obsIndex]; - } - // Calculate the Kalman Gain - // Calculate innovation variances - also used for data logging - varInnovVelPos[obsIndex] = P[stateIndex][stateIndex] + R_OBS[obsIndex]; - SK = 1.0/varInnovVelPos[obsIndex]; - for (uint8_t i= 0; i<=indexLimit; i++) - { - Kfusion[i] = P[i][stateIndex]*SK; - } - // Calculate state corrections and re-normalise the quaternions - for (uint8_t i = 0; i<=indexLimit; i++) - { - states[i] = states[i] - Kfusion[i] * innovVelPos[obsIndex]; - } - quatMag = sqrt(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]); - if (quatMag > 1e-12f) // divide by 0 protection - { - for (uint8_t i = 0; i<=3; i++) - { - states[i] = states[i] / quatMag; - } - } - // Update the covariance - take advantage of direct observation of a - // single state at index = stateIndex to reduce computations - // Optimised implementation of standard equation P = (I - K*H)*P; - for (uint8_t i= 0; i<=indexLimit; i++) - { - for (uint8_t j= 0; j<=indexLimit; j++) - { - KHP[i][j] = Kfusion[i] * P[stateIndex][j]; - } - } - for (uint8_t i= 0; i<=indexLimit; i++) - { - for (uint8_t j= 0; j<=indexLimit; j++) - { - P[i][j] = P[i][j] - KHP[i][j]; - } - } - } - } - } - - ForceSymmetry(); - ConstrainVariances(); - - //printf("velh: %s, posh: %s, hgth: %s\n", ((velHealth) ? "OK" : "FAIL"), ((posHealth) ? "OK" : "FAIL"), ((hgtHealth) ? "OK" : "FAIL")); -} - -void AttPosEKF::FuseMagnetometer() -{ - uint8_t obsIndex; - uint8_t indexLimit; - float DCM[3][3] = - { - {1.0f,0.0f,0.0f} , - {0.0f,1.0f,0.0f} , - {0.0f,0.0f,1.0f} - }; - float MagPred[3] = {0.0f,0.0f,0.0f}; - float SK_MX[6]; - float SK_MY[5]; - float SK_MZ[6]; - float SH_MAG[9] = {0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f}; - -// Perform sequential fusion of Magnetometer measurements. -// This assumes that the errors in the different components are -// uncorrelated which is not true, however in the absence of covariance -// data fit is the only assumption we can make -// so we might as well take advantage of the computational efficiencies -// associated with sequential fusion - if (useCompass && (fuseMagData || obsIndex == 1 || obsIndex == 2)) - { - // Limit range of states modified when on ground - if(!onGround) - { - indexLimit = 20; - } - else - { - indexLimit = 12; - } - - static float q0 = 0.0f; - static float q1 = 0.0f; - static float q2 = 0.0f; - static float q3 = 1.0f; - static float magN = 0.4f; - static float magE = 0.0f; - static float magD = 0.3f; - - static float R_MAG = 0.0025f; - - float H_MAG[21] = {0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f}; - - // Sequential fusion of XYZ components to spread processing load across - // three prediction time steps. - - // Calculate observation jacobians and Kalman gains - if (fuseMagData) - { - static float magXbias = 0.0f; - static float magYbias = 0.0f; - static float magZbias = 0.0f; - - // Copy required states to local variable names - q0 = statesAtMagMeasTime[0]; - q1 = statesAtMagMeasTime[1]; - q2 = statesAtMagMeasTime[2]; - q3 = statesAtMagMeasTime[3]; - magN = statesAtMagMeasTime[15]; - magE = statesAtMagMeasTime[16]; - magD = statesAtMagMeasTime[17]; - magXbias = statesAtMagMeasTime[18]; - magYbias = statesAtMagMeasTime[19]; - magZbias = statesAtMagMeasTime[20]; - - // rotate predicted earth components into body axes and calculate - // predicted measurments - DCM[0][0] = q0*q0 + q1*q1 - q2*q2 - q3*q3; - DCM[0][1] = 2*(q1*q2 + q0*q3); - DCM[0][2] = 2*(q1*q3-q0*q2); - DCM[1][0] = 2*(q1*q2 - q0*q3); - DCM[1][1] = q0*q0 - q1*q1 + q2*q2 - q3*q3; - DCM[1][2] = 2*(q2*q3 + q0*q1); - DCM[2][0] = 2*(q1*q3 + q0*q2); - DCM[2][1] = 2*(q2*q3 - q0*q1); - DCM[2][2] = q0*q0 - q1*q1 - q2*q2 + q3*q3; - MagPred[0] = DCM[0][0]*magN + DCM[0][1]*magE + DCM[0][2]*magD + magXbias; - MagPred[1] = DCM[1][0]*magN + DCM[1][1]*magE + DCM[1][2]*magD + magYbias; - MagPred[2] = DCM[2][0]*magN + DCM[2][1]*magE + DCM[2][2]*magD + magZbias; - - // scale magnetometer observation error with total angular rate - R_MAG = 0.0025f + sq(0.05f*dAngIMU.length()/dtIMU); - - // Calculate observation jacobians - SH_MAG[0] = 2*magD*q3 + 2*magE*q2 + 2*magN*q1; - SH_MAG[1] = 2*magD*q0 - 2*magE*q1 + 2*magN*q2; - SH_MAG[2] = 2*magD*q1 + 2*magE*q0 - 2*magN*q3; - SH_MAG[3] = sq(q3); - SH_MAG[4] = sq(q2); - SH_MAG[5] = sq(q1); - SH_MAG[6] = sq(q0); - SH_MAG[7] = 2*magN*q0; - SH_MAG[8] = 2*magE*q3; - - for (uint8_t i=0; i<=20; i++) H_MAG[i] = 0; - H_MAG[0] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2; - H_MAG[1] = SH_MAG[0]; - H_MAG[2] = 2*magE*q1 - 2*magD*q0 - 2*magN*q2; - H_MAG[3] = SH_MAG[2]; - H_MAG[15] = SH_MAG[5] - SH_MAG[4] - SH_MAG[3] + SH_MAG[6]; - H_MAG[16] = 2*q0*q3 + 2*q1*q2; - H_MAG[17] = 2*q1*q3 - 2*q0*q2; - H_MAG[18] = 1.0f; - - // Calculate Kalman gain - SK_MX[0] = 1/(P[18][18] + R_MAG + P[1][18]*SH_MAG[0] + P[3][18]*SH_MAG[2] - P[15][18]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) - (2*magD*q0 - 2*magE*q1 + 2*magN*q2)*(P[18][2] + P[1][2]*SH_MAG[0] + P[3][2]*SH_MAG[2] - P[15][2]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][2]*(2*q0*q3 + 2*q1*q2) - P[17][2]*(2*q0*q2 - 2*q1*q3) - P[2][2]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][2]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (SH_MAG[7] + SH_MAG[8] - 2*magD*q2)*(P[18][0] + P[1][0]*SH_MAG[0] + P[3][0]*SH_MAG[2] - P[15][0]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][0]*(2*q0*q3 + 2*q1*q2) - P[17][0]*(2*q0*q2 - 2*q1*q3) - P[2][0]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][0]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[0]*(P[18][1] + P[1][1]*SH_MAG[0] + P[3][1]*SH_MAG[2] - P[15][1]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][1]*(2*q0*q3 + 2*q1*q2) - P[17][1]*(2*q0*q2 - 2*q1*q3) - P[2][1]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][1]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[2]*(P[18][3] + P[1][3]*SH_MAG[0] + P[3][3]*SH_MAG[2] - P[15][3]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][3]*(2*q0*q3 + 2*q1*q2) - P[17][3]*(2*q0*q2 - 2*q1*q3) - P[2][3]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][3]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6])*(P[18][15] + P[1][15]*SH_MAG[0] + P[3][15]*SH_MAG[2] - P[15][15]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][15]*(2*q0*q3 + 2*q1*q2) - P[17][15]*(2*q0*q2 - 2*q1*q3) - P[2][15]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][15]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[16][18]*(2*q0*q3 + 2*q1*q2) - P[17][18]*(2*q0*q2 - 2*q1*q3) - P[2][18]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + (2*q0*q3 + 2*q1*q2)*(P[18][16] + P[1][16]*SH_MAG[0] + P[3][16]*SH_MAG[2] - P[15][16]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][16]*(2*q0*q3 + 2*q1*q2) - P[17][16]*(2*q0*q2 - 2*q1*q3) - P[2][16]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][16]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (2*q0*q2 - 2*q1*q3)*(P[18][17] + P[1][17]*SH_MAG[0] + P[3][17]*SH_MAG[2] - P[15][17]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][17]*(2*q0*q3 + 2*q1*q2) - P[17][17]*(2*q0*q2 - 2*q1*q3) - P[2][17]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][17]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[0][18]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)); - SK_MX[1] = SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]; - SK_MX[2] = 2*magD*q0 - 2*magE*q1 + 2*magN*q2; - SK_MX[3] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2; - SK_MX[4] = 2*q0*q2 - 2*q1*q3; - SK_MX[5] = 2*q0*q3 + 2*q1*q2; - Kfusion[0] = SK_MX[0]*(P[0][18] + P[0][1]*SH_MAG[0] + P[0][3]*SH_MAG[2] + P[0][0]*SK_MX[3] - P[0][2]*SK_MX[2] - P[0][15]*SK_MX[1] + P[0][16]*SK_MX[5] - P[0][17]*SK_MX[4]); - Kfusion[1] = SK_MX[0]*(P[1][18] + P[1][1]*SH_MAG[0] + P[1][3]*SH_MAG[2] + P[1][0]*SK_MX[3] - P[1][2]*SK_MX[2] - P[1][15]*SK_MX[1] + P[1][16]*SK_MX[5] - P[1][17]*SK_MX[4]); - Kfusion[2] = SK_MX[0]*(P[2][18] + P[2][1]*SH_MAG[0] + P[2][3]*SH_MAG[2] + P[2][0]*SK_MX[3] - P[2][2]*SK_MX[2] - P[2][15]*SK_MX[1] + P[2][16]*SK_MX[5] - P[2][17]*SK_MX[4]); - Kfusion[3] = SK_MX[0]*(P[3][18] + P[3][1]*SH_MAG[0] + P[3][3]*SH_MAG[2] + P[3][0]*SK_MX[3] - P[3][2]*SK_MX[2] - P[3][15]*SK_MX[1] + P[3][16]*SK_MX[5] - P[3][17]*SK_MX[4]); - Kfusion[4] = SK_MX[0]*(P[4][18] + P[4][1]*SH_MAG[0] + P[4][3]*SH_MAG[2] + P[4][0]*SK_MX[3] - P[4][2]*SK_MX[2] - P[4][15]*SK_MX[1] + P[4][16]*SK_MX[5] - P[4][17]*SK_MX[4]); - Kfusion[5] = SK_MX[0]*(P[5][18] + P[5][1]*SH_MAG[0] + P[5][3]*SH_MAG[2] + P[5][0]*SK_MX[3] - P[5][2]*SK_MX[2] - P[5][15]*SK_MX[1] + P[5][16]*SK_MX[5] - P[5][17]*SK_MX[4]); - Kfusion[6] = SK_MX[0]*(P[6][18] + P[6][1]*SH_MAG[0] + P[6][3]*SH_MAG[2] + P[6][0]*SK_MX[3] - P[6][2]*SK_MX[2] - P[6][15]*SK_MX[1] + P[6][16]*SK_MX[5] - P[6][17]*SK_MX[4]); - Kfusion[7] = SK_MX[0]*(P[7][18] + P[7][1]*SH_MAG[0] + P[7][3]*SH_MAG[2] + P[7][0]*SK_MX[3] - P[7][2]*SK_MX[2] - P[7][15]*SK_MX[1] + P[7][16]*SK_MX[5] - P[7][17]*SK_MX[4]); - Kfusion[8] = SK_MX[0]*(P[8][18] + P[8][1]*SH_MAG[0] + P[8][3]*SH_MAG[2] + P[8][0]*SK_MX[3] - P[8][2]*SK_MX[2] - P[8][15]*SK_MX[1] + P[8][16]*SK_MX[5] - P[8][17]*SK_MX[4]); - Kfusion[9] = SK_MX[0]*(P[9][18] + P[9][1]*SH_MAG[0] + P[9][3]*SH_MAG[2] + P[9][0]*SK_MX[3] - P[9][2]*SK_MX[2] - P[9][15]*SK_MX[1] + P[9][16]*SK_MX[5] - P[9][17]*SK_MX[4]); - Kfusion[10] = SK_MX[0]*(P[10][18] + P[10][1]*SH_MAG[0] + P[10][3]*SH_MAG[2] + P[10][0]*SK_MX[3] - P[10][2]*SK_MX[2] - P[10][15]*SK_MX[1] + P[10][16]*SK_MX[5] - P[10][17]*SK_MX[4]); - Kfusion[11] = SK_MX[0]*(P[11][18] + P[11][1]*SH_MAG[0] + P[11][3]*SH_MAG[2] + P[11][0]*SK_MX[3] - P[11][2]*SK_MX[2] - P[11][15]*SK_MX[1] + P[11][16]*SK_MX[5] - P[11][17]*SK_MX[4]); - Kfusion[12] = SK_MX[0]*(P[12][18] + P[12][1]*SH_MAG[0] + P[12][3]*SH_MAG[2] + P[12][0]*SK_MX[3] - P[12][2]*SK_MX[2] - P[12][15]*SK_MX[1] + P[12][16]*SK_MX[5] - P[12][17]*SK_MX[4]); - Kfusion[13] = SK_MX[0]*(P[13][18] + P[13][1]*SH_MAG[0] + P[13][3]*SH_MAG[2] + P[13][0]*SK_MX[3] - P[13][2]*SK_MX[2] - P[13][15]*SK_MX[1] + P[13][16]*SK_MX[5] - P[13][17]*SK_MX[4]); - Kfusion[14] = SK_MX[0]*(P[14][18] + P[14][1]*SH_MAG[0] + P[14][3]*SH_MAG[2] + P[14][0]*SK_MX[3] - P[14][2]*SK_MX[2] - P[14][15]*SK_MX[1] + P[14][16]*SK_MX[5] - P[14][17]*SK_MX[4]); - Kfusion[15] = SK_MX[0]*(P[15][18] + P[15][1]*SH_MAG[0] + P[15][3]*SH_MAG[2] + P[15][0]*SK_MX[3] - P[15][2]*SK_MX[2] - P[15][15]*SK_MX[1] + P[15][16]*SK_MX[5] - P[15][17]*SK_MX[4]); - Kfusion[16] = SK_MX[0]*(P[16][18] + P[16][1]*SH_MAG[0] + P[16][3]*SH_MAG[2] + P[16][0]*SK_MX[3] - P[16][2]*SK_MX[2] - P[16][15]*SK_MX[1] + P[16][16]*SK_MX[5] - P[16][17]*SK_MX[4]); - Kfusion[17] = SK_MX[0]*(P[17][18] + P[17][1]*SH_MAG[0] + P[17][3]*SH_MAG[2] + P[17][0]*SK_MX[3] - P[17][2]*SK_MX[2] - P[17][15]*SK_MX[1] + P[17][16]*SK_MX[5] - P[17][17]*SK_MX[4]); - Kfusion[18] = SK_MX[0]*(P[18][18] + P[18][1]*SH_MAG[0] + P[18][3]*SH_MAG[2] + P[18][0]*SK_MX[3] - P[18][2]*SK_MX[2] - P[18][15]*SK_MX[1] + P[18][16]*SK_MX[5] - P[18][17]*SK_MX[4]); - Kfusion[19] = SK_MX[0]*(P[19][18] + P[19][1]*SH_MAG[0] + P[19][3]*SH_MAG[2] + P[19][0]*SK_MX[3] - P[19][2]*SK_MX[2] - P[19][15]*SK_MX[1] + P[19][16]*SK_MX[5] - P[19][17]*SK_MX[4]); - Kfusion[20] = SK_MX[0]*(P[20][18] + P[20][1]*SH_MAG[0] + P[20][3]*SH_MAG[2] + P[20][0]*SK_MX[3] - P[20][2]*SK_MX[2] - P[20][15]*SK_MX[1] + P[20][16]*SK_MX[5] - P[20][17]*SK_MX[4]); - varInnovMag[0] = 1.0f/SK_MX[0]; - innovMag[0] = MagPred[0] - magData.x; - - // reset the observation index to 0 (we start by fusing the X - // measurement) - obsIndex = 0; - } - else if (obsIndex == 1) // we are now fusing the Y measurement - { - // Calculate observation jacobians - for (unsigned int i=0; i 5Sigma - if ((innovMag[obsIndex]*innovMag[obsIndex]/varInnovMag[obsIndex]) < 25.0) - { - // correct the state vector - for (uint8_t j= 0; j<=indexLimit; j++) - { - states[j] = states[j] - Kfusion[j] * innovMag[obsIndex]; - } - // normalise the quaternion states - float quatMag = sqrt(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]); - if (quatMag > 1e-12) - { - for (uint8_t j= 0; j<=3; j++) - { - float quatMagInv = 1.0f/quatMag; - states[j] = states[j] * quatMagInv; - } - } - // correct the covariance P = (I - K*H)*P - // take advantage of the empty columns in KH to reduce the - // number of operations - for (uint8_t i = 0; i<=indexLimit; i++) - { - for (uint8_t j = 0; j<=3; j++) - { - KH[i][j] = Kfusion[i] * H_MAG[j]; - } - for (uint8_t j = 4; j<=17; j++) KH[i][j] = 0.0f; - if (!onGround) - { - for (uint8_t j = 15; j<=20; j++) - { - KH[i][j] = Kfusion[i] * H_MAG[j]; - } - } - else - { - for (uint8_t j = 15; j<=20; j++) - { - KH[i][j] = 0.0f; - } - } - } - for (uint8_t i = 0; i<=indexLimit; i++) - { - for (uint8_t j = 0; j<=indexLimit; j++) - { - KHP[i][j] = 0.0f; - for (uint8_t k = 0; k<=3; k++) - { - KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j]; - } - if (!onGround) - { - for (uint8_t k = 15; k<=20; k++) - { - KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j]; - } - } - } - } - } - for (uint8_t i = 0; i<=indexLimit; i++) - { - for (uint8_t j = 0; j<=indexLimit; j++) - { - P[i][j] = P[i][j] - KHP[i][j]; - } - } - } - obsIndex = obsIndex + 1; - - ForceSymmetry(); - ConstrainVariances(); -} - -void AttPosEKF::FuseAirspeed() -{ - float vn; - float ve; - float vd; - float vwn; - float vwe; - const float R_TAS = 2.0f; - float SH_TAS[3]; - float Kfusion[21]; - float VtasPred; - - // Copy required states to local variable names - vn = statesAtVtasMeasTime[4]; - ve = statesAtVtasMeasTime[5]; - vd = statesAtVtasMeasTime[6]; - vwn = statesAtVtasMeasTime[13]; - vwe = statesAtVtasMeasTime[14]; - - // Need to check that it is flying before fusing airspeed data - // Calculate the predicted airspeed - VtasPred = sqrtf((ve - vwe)*(ve - vwe) + (vn - vwn)*(vn - vwn) + vd*vd); - // Perform fusion of True Airspeed measurement - if (useAirspeed && fuseVtasData && (VtasPred > 1.0f) && (VtasMeas > 8.0f)) - { - // Calculate observation jacobians - SH_TAS[0] = 1/(sqrt(sq(ve - vwe) + sq(vn - vwn) + sq(vd))); - SH_TAS[1] = (SH_TAS[0]*(2.0f*ve - 2*vwe))/2.0f; - SH_TAS[2] = (SH_TAS[0]*(2.0f*vn - 2*vwn))/2.0f; - - float H_TAS[21]; - for (uint8_t i=0; i<=20; i++) H_TAS[i] = 0.0f; - H_TAS[4] = SH_TAS[2]; - H_TAS[5] = SH_TAS[1]; - H_TAS[6] = vd*SH_TAS[0]; - H_TAS[13] = -SH_TAS[2]; - H_TAS[14] = -SH_TAS[1]; - - // Calculate Kalman gains - float SK_TAS = 1.0f/(R_TAS + SH_TAS[2]*(P[4][4]*SH_TAS[2] + P[5][4]*SH_TAS[1] - P[13][4]*SH_TAS[2] - P[14][4]*SH_TAS[1] + P[6][4]*vd*SH_TAS[0]) + SH_TAS[1]*(P[4][5]*SH_TAS[2] + P[5][5]*SH_TAS[1] - P[13][5]*SH_TAS[2] - P[14][5]*SH_TAS[1] + P[6][5]*vd*SH_TAS[0]) - SH_TAS[2]*(P[4][13]*SH_TAS[2] + P[5][13]*SH_TAS[1] - P[13][13]*SH_TAS[2] - P[14][13]*SH_TAS[1] + P[6][13]*vd*SH_TAS[0]) - SH_TAS[1]*(P[4][14]*SH_TAS[2] + P[5][14]*SH_TAS[1] - P[13][14]*SH_TAS[2] - P[14][14]*SH_TAS[1] + P[6][14]*vd*SH_TAS[0]) + vd*SH_TAS[0]*(P[4][6]*SH_TAS[2] + P[5][6]*SH_TAS[1] - P[13][6]*SH_TAS[2] - P[14][6]*SH_TAS[1] + P[6][6]*vd*SH_TAS[0])); - Kfusion[0] = SK_TAS*(P[0][4]*SH_TAS[2] - P[0][13]*SH_TAS[2] + P[0][5]*SH_TAS[1] - P[0][14]*SH_TAS[1] + P[0][6]*vd*SH_TAS[0]); - Kfusion[1] = SK_TAS*(P[1][4]*SH_TAS[2] - P[1][13]*SH_TAS[2] + P[1][5]*SH_TAS[1] - P[1][14]*SH_TAS[1] + P[1][6]*vd*SH_TAS[0]); - Kfusion[2] = SK_TAS*(P[2][4]*SH_TAS[2] - P[2][13]*SH_TAS[2] + P[2][5]*SH_TAS[1] - P[2][14]*SH_TAS[1] + P[2][6]*vd*SH_TAS[0]); - Kfusion[3] = SK_TAS*(P[3][4]*SH_TAS[2] - P[3][13]*SH_TAS[2] + P[3][5]*SH_TAS[1] - P[3][14]*SH_TAS[1] + P[3][6]*vd*SH_TAS[0]); - Kfusion[4] = SK_TAS*(P[4][4]*SH_TAS[2] - P[4][13]*SH_TAS[2] + P[4][5]*SH_TAS[1] - P[4][14]*SH_TAS[1] + P[4][6]*vd*SH_TAS[0]); - Kfusion[5] = SK_TAS*(P[5][4]*SH_TAS[2] - P[5][13]*SH_TAS[2] + P[5][5]*SH_TAS[1] - P[5][14]*SH_TAS[1] + P[5][6]*vd*SH_TAS[0]); - Kfusion[6] = SK_TAS*(P[6][4]*SH_TAS[2] - P[6][13]*SH_TAS[2] + P[6][5]*SH_TAS[1] - P[6][14]*SH_TAS[1] + P[6][6]*vd*SH_TAS[0]); - Kfusion[7] = SK_TAS*(P[7][4]*SH_TAS[2] - P[7][13]*SH_TAS[2] + P[7][5]*SH_TAS[1] - P[7][14]*SH_TAS[1] + P[7][6]*vd*SH_TAS[0]); - Kfusion[8] = SK_TAS*(P[8][4]*SH_TAS[2] - P[8][13]*SH_TAS[2] + P[8][5]*SH_TAS[1] - P[8][14]*SH_TAS[1] + P[8][6]*vd*SH_TAS[0]); - Kfusion[9] = SK_TAS*(P[9][4]*SH_TAS[2] - P[9][13]*SH_TAS[2] + P[9][5]*SH_TAS[1] - P[9][14]*SH_TAS[1] + P[9][6]*vd*SH_TAS[0]); - Kfusion[10] = SK_TAS*(P[10][4]*SH_TAS[2] - P[10][13]*SH_TAS[2] + P[10][5]*SH_TAS[1] - P[10][14]*SH_TAS[1] + P[10][6]*vd*SH_TAS[0]); - Kfusion[11] = SK_TAS*(P[11][4]*SH_TAS[2] - P[11][13]*SH_TAS[2] + P[11][5]*SH_TAS[1] - P[11][14]*SH_TAS[1] + P[11][6]*vd*SH_TAS[0]); - Kfusion[12] = SK_TAS*(P[12][4]*SH_TAS[2] - P[12][13]*SH_TAS[2] + P[12][5]*SH_TAS[1] - P[12][14]*SH_TAS[1] + P[12][6]*vd*SH_TAS[0]); - Kfusion[13] = SK_TAS*(P[13][4]*SH_TAS[2] - P[13][13]*SH_TAS[2] + P[13][5]*SH_TAS[1] - P[13][14]*SH_TAS[1] + P[13][6]*vd*SH_TAS[0]); - Kfusion[14] = SK_TAS*(P[14][4]*SH_TAS[2] - P[14][13]*SH_TAS[2] + P[14][5]*SH_TAS[1] - P[14][14]*SH_TAS[1] + P[14][6]*vd*SH_TAS[0]); - Kfusion[15] = SK_TAS*(P[15][4]*SH_TAS[2] - P[15][13]*SH_TAS[2] + P[15][5]*SH_TAS[1] - P[15][14]*SH_TAS[1] + P[15][6]*vd*SH_TAS[0]); - Kfusion[16] = SK_TAS*(P[16][4]*SH_TAS[2] - P[16][13]*SH_TAS[2] + P[16][5]*SH_TAS[1] - P[16][14]*SH_TAS[1] + P[16][6]*vd*SH_TAS[0]); - Kfusion[17] = SK_TAS*(P[17][4]*SH_TAS[2] - P[17][13]*SH_TAS[2] + P[17][5]*SH_TAS[1] - P[17][14]*SH_TAS[1] + P[17][6]*vd*SH_TAS[0]); - Kfusion[18] = SK_TAS*(P[18][4]*SH_TAS[2] - P[18][13]*SH_TAS[2] + P[18][5]*SH_TAS[1] - P[18][14]*SH_TAS[1] + P[18][6]*vd*SH_TAS[0]); - Kfusion[19] = SK_TAS*(P[19][4]*SH_TAS[2] - P[19][13]*SH_TAS[2] + P[19][5]*SH_TAS[1] - P[19][14]*SH_TAS[1] + P[19][6]*vd*SH_TAS[0]); - Kfusion[20] = SK_TAS*(P[20][4]*SH_TAS[2] - P[20][13]*SH_TAS[2] + P[20][5]*SH_TAS[1] - P[20][14]*SH_TAS[1] + P[20][6]*vd*SH_TAS[0]); - varInnovVtas = 1.0f/SK_TAS; - - // Calculate the measurement innovation - innovVtas = VtasPred - VtasMeas; - // Check the innovation for consistency and don't fuse if > 5Sigma - if ((innovVtas*innovVtas*SK_TAS) < 25.0) - { - // correct the state vector - for (uint8_t j=0; j<=20; j++) - { - states[j] = states[j] - Kfusion[j] * innovVtas; - } - // normalise the quaternion states - float quatMag = sqrt(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]); - if (quatMag > 1e-12f) - { - for (uint8_t j= 0; j<=3; j++) - { - float quatMagInv = 1.0f/quatMag; - states[j] = states[j] * quatMagInv; - } - } - // correct the covariance P = (I - K*H)*P - // take advantage of the empty columns in H to reduce the - // number of operations - for (uint8_t i = 0; i<=20; i++) - { - for (uint8_t j = 0; j<=3; j++) KH[i][j] = 0.0; - for (uint8_t j = 4; j<=6; j++) - { - KH[i][j] = Kfusion[i] * H_TAS[j]; - } - for (uint8_t j = 7; j<=12; j++) KH[i][j] = 0.0; - for (uint8_t j = 13; j<=14; j++) - { - KH[i][j] = Kfusion[i] * H_TAS[j]; - } - for (uint8_t j = 15; j<=20; j++) KH[i][j] = 0.0; - } - for (uint8_t i = 0; i<=20; i++) - { - for (uint8_t j = 0; j<=20; j++) - { - KHP[i][j] = 0.0; - for (uint8_t k = 4; k<=6; k++) - { - KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j]; - } - for (uint8_t k = 13; k<=14; k++) - { - KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j]; - } - } - } - for (uint8_t i = 0; i<=20; i++) - { - for (uint8_t j = 0; j<=20; j++) - { - P[i][j] = P[i][j] - KHP[i][j]; - } - } - } - } - - ForceSymmetry(); - ConstrainVariances(); -} - -void AttPosEKF::zeroRows(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last) -{ - uint8_t row; - uint8_t col; - for (row=first; row<=last; row++) - { - for (col=0; col GPS_FIX_2D); - } -} - -void AttPosEKF::calcEarthRateNED(Vector3f &omega, float latitude) -{ - //Define Earth rotation vector in the NED navigation frame - omega.x = earthRate*cosf(latitude); - omega.y = 0.0f; - omega.z = -earthRate*sinf(latitude); -} - -void AttPosEKF::CovarianceInit() -{ - // Calculate the initial covariance matrix P - P[0][0] = 0.25f * sq(1.0f*deg2rad); - P[1][1] = 0.25f * sq(1.0f*deg2rad); - P[2][2] = 0.25f * sq(1.0f*deg2rad); - P[3][3] = 0.25f * sq(10.0f*deg2rad); - P[4][4] = sq(0.7); - P[5][5] = P[4][4]; - P[6][6] = sq(0.7); - P[7][7] = sq(15.0); - P[8][8] = P[7][7]; - P[9][9] = sq(5.0); - P[10][10] = sq(0.1*deg2rad*dtIMU); - P[11][11] = P[10][10]; - P[12][12] = P[10][10]; - P[13][13] = sq(8.0f); - P[14][4] = P[13][13]; - P[15][15] = sq(0.02f); - P[16][16] = P[15][15]; - P[17][17] = P[15][15]; - P[18][18] = sq(0.02f); - P[19][19] = P[18][18]; - P[20][20] = P[18][18]; -} - -float AttPosEKF::ConstrainFloat(float val, float min, float max) -{ - return (val > max) ? max : ((val < min) ? min : val); -} - -void AttPosEKF::ConstrainVariances() -{ - if (!numericalProtection) { - return; - } - - // State vector: - // 0-3: quaternions (q0, q1, q2, q3) - // 4-6: Velocity - m/sec (North, East, Down) - // 7-9: Position - m (North, East, Down) - // 10-12: Delta Angle bias - rad (X,Y,Z) - // 13-14: Wind Vector - m/sec (North,East) - // 15-17: Earth Magnetic Field Vector - gauss (North, East, Down) - // 18-20: Body Magnetic Field Vector - gauss (X,Y,Z) - - // Constrain quaternion variances - for (unsigned i = 0; i < 4; i++) { - P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0f); - } - - // Constrain velocitie variances - for (unsigned i = 4; i < 7; i++) { - P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0e3f); - } - - // Constrain position variances - for (unsigned i = 7; i < 10; i++) { - P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0e6f); - } - - // Angle bias variances - for (unsigned i = 10; i < 13; i++) { - P[i][i] = ConstrainFloat(P[i][i], 0.0f, sq(0.175f * dtIMU)); - } - - // Wind velocity variances - for (unsigned i = 13; i < 15; i++) { - P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0e3f); - } - - // Earth magnetic field variances - for (unsigned i = 15; i < 18; i++) { - P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0f); - } - - // Body magnetic field variances - for (unsigned i = 18; i < 21; i++) { - P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0f); - } - -} - -void AttPosEKF::ConstrainStates() -{ - if (!numericalProtection) { - return; - } - - // State vector: - // 0-3: quaternions (q0, q1, q2, q3) - // 4-6: Velocity - m/sec (North, East, Down) - // 7-9: Position - m (North, East, Down) - // 10-12: Delta Angle bias - rad (X,Y,Z) - // 13-14: Wind Vector - m/sec (North,East) - // 15-17: Earth Magnetic Field Vector - gauss (North, East, Down) - // 18-20: Body Magnetic Field Vector - gauss (X,Y,Z) - - - // Constrain quaternion - for (unsigned i = 0; i < 4; i++) { - states[i] = ConstrainFloat(states[i], -1.0f, 1.0f); - } - - // Constrain velocities to what GPS can do for us - for (unsigned i = 4; i < 7; i++) { - states[i] = ConstrainFloat(states[i], -5.0e2f, 5.0e2f); - } - - // Constrain position to a reasonable vehicle range (in meters) - for (unsigned i = 7; i < 9; i++) { - states[i] = ConstrainFloat(states[i], -1.0e6f, 1.0e6f); - } - - // Constrain altitude - states[9] = ConstrainFloat(states[9], -4.0e4f, 1.0e4f); - - // Angle bias limit - set to 8 degrees / sec - for (unsigned i = 10; i < 13; i++) { - states[i] = ConstrainFloat(states[i], -0.12f * dtIMU, 0.12f * dtIMU); - } - - // Wind velocity limits - assume 120 m/s max velocity - for (unsigned i = 13; i < 15; i++) { - states[i] = ConstrainFloat(states[i], -120.0f, 120.0f); - } - - // Earth magnetic field limits (in Gauss) - for (unsigned i = 15; i < 18; i++) { - states[i] = ConstrainFloat(states[i], -1.0f, 1.0f); - } - - // Body magnetic field variances (in Gauss). - // the max offset should be in this range. - for (unsigned i = 18; i < 21; i++) { - states[i] = ConstrainFloat(states[i], -0.5f, 0.5f); - } - -} - -void AttPosEKF::ForceSymmetry() -{ - if (!numericalProtection) { - return; - } - - // Force symmetry on the covariance matrix to prevent ill-conditioning - // of the matrix which would cause the filter to blow-up - for (unsigned i = 1; i < n_states; i++) - { - for (uint8_t j = 0; j < i; j++) - { - P[i][j] = 0.5f * (P[i][j] + P[j][i]); - P[j][i] = P[i][j]; - } - } -} - -bool AttPosEKF::FilterHealthy() -{ - if (!statesInitialised) { - return false; - } - - // XXX Check state vector for NaNs and ill-conditioning - - // Check if any of the major inputs timed out - if (current_ekf_state.posTimeout || current_ekf_state.velTimeout || current_ekf_state.hgtTimeout) { - return false; - } - - // Nothing fired, return ok. - return true; -} - -/** - * Reset the filter position states - * - * This resets the position to the last GPS measurement - * or to zero in case of static position. - */ -void AttPosEKF::ResetPosition(void) -{ - if (staticMode) { - states[7] = 0; - states[8] = 0; - } else if (GPSstatus >= GPS_FIX_3D) { - - // reset the states from the GPS measurements - states[7] = posNE[0]; - states[8] = posNE[1]; - } -} - -/** - * Reset the height state. - * - * This resets the height state with the last altitude measurements - */ -void AttPosEKF::ResetHeight(void) -{ - // write to the state vector - states[9] = -hgtMea; -} - -/** - * Reset the velocity state. - */ -void AttPosEKF::ResetVelocity(void) -{ - if (staticMode) { - states[4] = 0.0f; - states[5] = 0.0f; - states[6] = 0.0f; - } else if (GPSstatus >= GPS_FIX_3D) { - - states[4] = velNED[0]; // north velocity from last reading - states[5] = velNED[1]; // east velocity from last reading - states[6] = velNED[2]; // down velocity from last reading - } -} - - -void AttPosEKF::FillErrorReport(struct ekf_status_report *err) -{ - for (int i = 0; i < n_states; i++) - { - err->states[i] = states[i]; - } - - err->velHealth = current_ekf_state.velHealth; - err->posHealth = current_ekf_state.posHealth; - err->hgtHealth = current_ekf_state.hgtHealth; - err->velTimeout = current_ekf_state.velTimeout; - err->posTimeout = current_ekf_state.posTimeout; - err->hgtTimeout = current_ekf_state.hgtTimeout; -} - -bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) { - bool err = false; - - // check all states and covariance matrices - for (unsigned i = 0; i < n_states; i++) { - for (unsigned j = 0; j < n_states; j++) { - if (!isfinite(KH[i][j])) { - - err_report->covarianceNaN = true; - err = true; - } // intermediate result used for covariance updates - if (!isfinite(KHP[i][j])) { - - err_report->covarianceNaN = true; - err = true; - } // intermediate result used for covariance updates - if (!isfinite(P[i][j])) { - - err_report->covarianceNaN = true; - err = true; - } // covariance matrix - } - - if (!isfinite(Kfusion[i])) { - - err_report->kalmanGainsNaN = true; - err = true; - } // Kalman gains - - if (!isfinite(states[i])) { - - err_report->statesNaN = true; - err = true; - } // state matrix - } - - if (err) { - FillErrorReport(err_report); - } - - return err; - -} - -/** - * Check the filter inputs and bound its operational state - * - * This check will reset the filter states if required - * due to a failure of consistency or timeout checks. - * it should be run after the measurement data has been - * updated, but before any of the fusion steps are - * executed. - */ -int AttPosEKF::CheckAndBound() -{ - - // Store the old filter state - bool currStaticMode = staticMode; - - // Reset the filter if the states went NaN - if (StatesNaN(&last_ekf_error)) { - - InitializeDynamic(velNED, magDeclination); - - return 1; - } - - // Reset the filter if the IMU data is too old - if (dtIMU > 0.2f) { - - ResetVelocity(); - ResetPosition(); - ResetHeight(); - ResetStoredStates(); - - // that's all we can do here, return - return 2; - } - - // Check if we're on ground - this also sets static mode. - OnGroundCheck(); - - // Check if we switched between states - if (currStaticMode != staticMode) { - ResetVelocity(); - ResetPosition(); - ResetHeight(); - ResetStoredStates(); - - return 3; - } - - return 0; -} - -void AttPosEKF::AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float declination, float *initQuat) -{ - float initialRoll, initialPitch; - float cosRoll, sinRoll, cosPitch, sinPitch; - float magX, magY; - float initialHdg, cosHeading, sinHeading; - - initialRoll = atan2(-ay, -az); - initialPitch = atan2(ax, -az); - - cosRoll = cosf(initialRoll); - sinRoll = sinf(initialRoll); - cosPitch = cosf(initialPitch); - sinPitch = sinf(initialPitch); - - magX = mx * cosPitch + my * sinRoll * sinPitch + mz * cosRoll * sinPitch; - - magY = my * cosRoll - mz * sinRoll; - - initialHdg = atan2f(-magY, magX); - /* true heading is the mag heading minus declination */ - initialHdg += declination; - - cosRoll = cosf(initialRoll * 0.5f); - sinRoll = sinf(initialRoll * 0.5f); - - cosPitch = cosf(initialPitch * 0.5f); - sinPitch = sinf(initialPitch * 0.5f); - - cosHeading = cosf(initialHdg * 0.5f); - sinHeading = sinf(initialHdg * 0.5f); - - initQuat[0] = cosRoll * cosPitch * cosHeading + sinRoll * sinPitch * sinHeading; - initQuat[1] = sinRoll * cosPitch * cosHeading - cosRoll * sinPitch * sinHeading; - initQuat[2] = cosRoll * sinPitch * cosHeading + sinRoll * cosPitch * sinHeading; - initQuat[3] = cosRoll * cosPitch * sinHeading - sinRoll * sinPitch * cosHeading; -} - -void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination) -{ - - // Clear the init flag - statesInitialised = false; - - magDeclination = declination; - - ZeroVariables(); - - // Calculate initial filter quaternion states from raw measurements - float initQuat[4]; - Vector3f initMagXYZ; - initMagXYZ = magData - magBias; - AttitudeInit(accel.x, accel.y, accel.z, initMagXYZ.x, initMagXYZ.y, initMagXYZ.z, magDeclination, initQuat); - - // Calculate initial Tbn matrix and rotate Mag measurements into NED - // to set initial NED magnetic field states - Mat3f DCM; - quat2Tbn(DCM, initQuat); - Vector3f initMagNED; - initMagXYZ = magData - magBias; - initMagNED.x = DCM.x.x*initMagXYZ.x + DCM.x.y*initMagXYZ.y + DCM.x.z*initMagXYZ.z; - initMagNED.y = DCM.y.x*initMagXYZ.x + DCM.y.y*initMagXYZ.y + DCM.y.z*initMagXYZ.z; - initMagNED.z = DCM.z.x*initMagXYZ.x + DCM.z.y*initMagXYZ.y + DCM.z.z*initMagXYZ.z; - - - - // write to state vector - for (uint8_t j=0; j<=3; j++) states[j] = initQuat[j]; // quaternions - for (uint8_t j=0; j<=2; j++) states[j+4] = initvelNED[j]; // velocities - for (uint8_t j=0; j<=7; j++) states[j+7] = 0.0f; // positiions, dAngBias, windVel - states[15] = initMagNED.x; // Magnetic Field North - states[16] = initMagNED.y; // Magnetic Field East - states[17] = initMagNED.z; // Magnetic Field Down - states[18] = magBias.x; // Magnetic Field Bias X - states[19] = magBias.y; // Magnetic Field Bias Y - states[20] = magBias.z; // Magnetic Field Bias Z - - statesInitialised = true; - - // initialise the covariance matrix - CovarianceInit(); - - //Define Earth rotation vector in the NED navigation frame - calcEarthRateNED(earthRateNED, latRef); - - //Initialise summed variables used by covariance prediction - summedDelAng.x = 0.0f; - summedDelAng.y = 0.0f; - summedDelAng.z = 0.0f; - summedDelVel.x = 0.0f; - summedDelVel.y = 0.0f; - summedDelVel.z = 0.0f; -} - -void AttPosEKF::InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt, float declination) -{ - //store initial lat,long and height - latRef = referenceLat; - lonRef = referenceLon; - hgtRef = referenceHgt; - - memset(&last_ekf_error, 0, sizeof(last_ekf_error)); - - InitializeDynamic(initvelNED, declination); -} - -void AttPosEKF::ZeroVariables() -{ - // Do the data structure init - for (unsigned i = 0; i < n_states; i++) { - for (unsigned j = 0; j < n_states; j++) { - KH[i][j] = 0.0f; // intermediate result used for covariance updates - KHP[i][j] = 0.0f; // intermediate result used for covariance updates - P[i][j] = 0.0f; // covariance matrix - } - - Kfusion[i] = 0.0f; // Kalman gains - states[i] = 0.0f; // state matrix - } - - for (unsigned i = 0; i < data_buffer_size; i++) { - - for (unsigned j = 0; j < n_states; j++) { - storedStates[j][i] = 0.0f; - } - - statetimeStamp[i] = 0; - } - - memset(¤t_ekf_state, 0, sizeof(current_ekf_state)); -} - -void AttPosEKF::GetFilterState(struct ekf_status_report *state) -{ - memcpy(state, ¤t_ekf_state, sizeof(state)); -} - -void AttPosEKF::GetLastErrorState(struct ekf_status_report *last_error) -{ - memcpy(last_error, &last_ekf_error, sizeof(last_error)); -} diff --git a/src/modules/ekf_att_pos_estimator/estimator_21states.h b/src/modules/ekf_att_pos_estimator/estimator_21states.h deleted file mode 100644 index a19ff19952..0000000000 --- a/src/modules/ekf_att_pos_estimator/estimator_21states.h +++ /dev/null @@ -1,247 +0,0 @@ -#pragma once - -#include "estimator_utilities.h" - -class AttPosEKF { - -public: - - AttPosEKF(); - ~AttPosEKF(); - - /* ############################################## - * - * M A I N F I L T E R P A R A M E T E R S - * - * ########################################### */ - - /* - * parameters are defined here and initialised in - * the InitialiseParameters() (which is just 20 lines down) - */ - - float covTimeStepMax; // maximum time allowed between covariance predictions - float covDelAngMax; // maximum delta angle between covariance predictions - float rngFinderPitch; // pitch angle of laser range finder in radians. Zero is aligned with the Z body axis. Positive is RH rotation about Y body axis. - - float yawVarScale; - float windVelSigma; - float dAngBiasSigma; - float dVelBiasSigma; - float magEarthSigma; - float magBodySigma; - float gndHgtSigma; - - float vneSigma; - float vdSigma; - float posNeSigma; - float posDSigma; - float magMeasurementSigma; - float airspeedMeasurementSigma; - - float gyroProcessNoise; - float accelProcessNoise; - - float EAS2TAS; // ratio f true to equivalent airspeed - - void InitialiseParameters() - { - covTimeStepMax = 0.07f; // maximum time allowed between covariance predictions - covDelAngMax = 0.02f; // maximum delta angle between covariance predictions - rngFinderPitch = 0.0f; // pitch angle of laser range finder in radians. Zero is aligned with the Z body axis. Positive is RH rotation about Y body axis. - EAS2TAS = 1.0f; - - yawVarScale = 1.0f; - windVelSigma = 0.1f; - dAngBiasSigma = 5.0e-7f; - dVelBiasSigma = 1e-4f; - magEarthSigma = 3.0e-4f; - magBodySigma = 3.0e-4f; - gndHgtSigma = 0.02f; // assume 2% terrain gradient 1-sigma - - vneSigma = 0.2f; - vdSigma = 0.3f; - posNeSigma = 2.0f; - posDSigma = 2.0f; - - magMeasurementSigma = 0.05; - airspeedMeasurementSigma = 1.4f; - gyroProcessNoise = 1.4544411e-2f; - accelProcessNoise = 0.5f; - } - - // Global variables - float KH[n_states][n_states]; // intermediate result used for covariance updates - float KHP[n_states][n_states]; // intermediate result used for covariance updates - float P[n_states][n_states]; // covariance matrix - float Kfusion[n_states]; // Kalman gains - float states[n_states]; // state matrix - float storedStates[n_states][data_buffer_size]; // state vectors stored for the last 50 time steps - uint32_t statetimeStamp[data_buffer_size]; // time stamp for each state vector stored - - float statesAtVelTime[n_states]; // States at the effective measurement time for posNE and velNED measurements - float statesAtPosTime[n_states]; // States at the effective measurement time for posNE and velNED measurements - float statesAtHgtTime[n_states]; // States at the effective measurement time for the hgtMea measurement - float statesAtMagMeasTime[n_states]; // filter satates at the effective measurement time - float statesAtVtasMeasTime[n_states]; // filter states at the effective measurement time - - Vector3f correctedDelAng; // delta angles about the xyz body axes corrected for errors (rad) - Vector3f correctedDelVel; // delta velocities along the XYZ body axes corrected for errors (m/s) - Vector3f summedDelAng; // summed delta angles about the xyz body axes corrected for errors (rad) - Vector3f summedDelVel; // summed delta velocities along the XYZ body axes corrected for errors (m/s) - float accNavMag; // magnitude of navigation accel (- used to adjust GPS obs variance (m/s^2) - Vector3f earthRateNED; // earths angular rate vector in NED (rad/s) - Vector3f angRate; // angular rate vector in XYZ body axes measured by the IMU (rad/s) - Vector3f accel; // acceleration vector in XYZ body axes measured by the IMU (m/s^2) - Vector3f dVelIMU; - Vector3f dAngIMU; - float dtIMU; // time lapsed since the last IMU measurement or covariance update (sec) - uint8_t fusionModeGPS; // 0 = GPS outputs 3D velocity, 1 = GPS outputs 2D velocity, 2 = GPS outputs no velocity - float innovVelPos[6]; // innovation output - float varInnovVelPos[6]; // innovation variance output - - float velNED[3]; // North, East, Down velocity obs (m/s) - float posNE[2]; // North, East position obs (m) - float hgtMea; // measured height (m) - float posNED[3]; // North, East Down position (m) - - float innovMag[3]; // innovation output - float varInnovMag[3]; // innovation variance output - Vector3f magData; // magnetometer flux radings in X,Y,Z body axes - float innovVtas; // innovation output - float varInnovVtas; // innovation variance output - float VtasMeas; // true airspeed measurement (m/s) - float magDeclination; - float latRef; // WGS-84 latitude of reference point (rad) - float lonRef; // WGS-84 longitude of reference point (rad) - float hgtRef; // WGS-84 height of reference point (m) - Vector3f magBias; // states representing magnetometer bias vector in XYZ body axes - uint8_t covSkipCount; // Number of state prediction frames (IMU daya updates to skip before doing the covariance prediction - - // GPS input data variables - float gpsCourse; - float gpsVelD; - float gpsLat; - float gpsLon; - float gpsHgt; - uint8_t GPSstatus; - - // Baro input - float baroHgt; - - bool statesInitialised; - - bool fuseVelData; // this boolean causes the posNE and velNED obs to be fused - bool fusePosData; // this boolean causes the posNE and velNED obs to be fused - bool fuseHgtData; // this boolean causes the hgtMea obs to be fused - bool fuseMagData; // boolean true when magnetometer data is to be fused - bool fuseVtasData; // boolean true when airspeed data is to be fused - - bool onGround; ///< boolean true when the flight vehicle is on the ground (not flying) - bool staticMode; ///< boolean true if no position feedback is fused - bool useAirspeed; ///< boolean true if airspeed data is being used - bool useCompass; ///< boolean true if magnetometer data is being used - - struct ekf_status_report current_ekf_state; - struct ekf_status_report last_ekf_error; - - bool numericalProtection; - - unsigned storeIndex; - - -void UpdateStrapdownEquationsNED(); - -void CovariancePrediction(float dt); - -void FuseVelposNED(); - -void FuseMagnetometer(); - -void FuseAirspeed(); - -void zeroRows(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last); - -void zeroCols(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last); - -void quatNorm(float (&quatOut)[4], const float quatIn[4]); - -// store staes along with system time stamp in msces -void StoreStates(uint64_t timestamp_ms); - -/** - * Recall the state vector. - * - * Recalls the vector stored at closest time to the one specified by msec - * - * @return zero on success, integer indicating the number of invalid states on failure. - * Does only copy valid states, if the statesForFusion vector was initialized - * correctly by the caller, the result can be safely used, but is a mixture - * time-wise where valid states were updated and invalid remained at the old - * value. - */ -int RecallStates(float statesForFusion[n_states], uint64_t msec); - -void ResetStoredStates(); - -void quat2Tbn(Mat3f &Tbn, const float (&quat)[4]); - -void calcEarthRateNED(Vector3f &omega, float latitude); - -static void eul2quat(float (&quat)[4], const float (&eul)[3]); - -static void quat2eul(float (&eul)[3], const float (&quat)[4]); - -static void calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, float gpsVelD); - -static void calcposNED(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef); - -static void calcLLH(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef); - -static void quat2Tnb(Mat3f &Tnb, const float (&quat)[4]); - -static float sq(float valIn); - -void OnGroundCheck(); - -void CovarianceInit(); - -void InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt, float declination); - -float ConstrainFloat(float val, float min, float max); - -void ConstrainVariances(); - -void ConstrainStates(); - -void ForceSymmetry(); - -int CheckAndBound(); - -void ResetPosition(); - -void ResetVelocity(); - -void ZeroVariables(); - -void GetFilterState(struct ekf_status_report *state); - -void GetLastErrorState(struct ekf_status_report *last_error); - -bool StatesNaN(struct ekf_status_report *err_report); -void FillErrorReport(struct ekf_status_report *err); - -void InitializeDynamic(float (&initvelNED)[3], float declination); - -protected: - -bool FilterHealthy(); - -void ResetHeight(void); - -void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float declination, float *initQuat); - -}; - -uint32_t millis(); - diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp similarity index 79% rename from src/modules/ekf_att_pos_estimator/estimator_23states.cpp rename to src/modules/ekf_att_pos_estimator/estimator_22states.cpp index c17e034add..b062097fb3 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp @@ -1,4 +1,4 @@ -#include "estimator_23states.h" +#include "estimator_22states.h" #include #include #include @@ -14,9 +14,6 @@ AttPosEKF::AttPosEKF() : covTimeStepMax(0.0f), covDelAngMax(0.0f), rngFinderPitch(0.0f), - a1(0.0f), - a2(0.0f), - a3(0.0f), yawVarScale(0.0f), windVelSigma(0.0f), dAngBiasSigma(0.0f), @@ -71,10 +68,6 @@ AttPosEKF::AttPosEKF() : dtVelPosFilt(0.01f), dtHgtFilt(0.01f), dtGpsFilt(0.1f), - windSpdFiltNorth(0.0f), - windSpdFiltEast(0.0f), - windSpdFiltAltitude(0.0f), - windSpdFiltClimb(0.0f), fusionModeGPS(0), innovVelPos{}, varInnovVelPos{}, @@ -87,7 +80,6 @@ AttPosEKF::AttPosEKF() : innovMag{}, varInnovMag{}, magData{}, - losData{}, innovVtas(0.0f), innovRng(0.0f), innovOptFlow{}, @@ -102,6 +94,8 @@ AttPosEKF::AttPosEKF() : refSet(false), magBias(), covSkipCount(0), + lastFixTime_ms(0), + globalTimeStamp_ms(0), gpsLat(0.0), gpsLon(-M_PI), gpsHgt(0.0f), @@ -123,6 +117,7 @@ AttPosEKF::AttPosEKF() : onGround(true), staticMode(true), + useGPS(false), useAirspeed(true), useCompass(true), useRangeFinder(true), @@ -133,7 +128,7 @@ AttPosEKF::AttPosEKF() : current_ekf_state{}, last_ekf_error{}, numericalProtection(true), - storeIndex(0), + storeIndex(0), storedOmega{}, Popt{}, flowStates{}, @@ -152,6 +147,7 @@ AttPosEKF::AttPosEKF() : minFlowRng(0.0f), moCompR_LOS(0.0f) { + memset(&last_ekf_error, 0, sizeof(last_ekf_error)); memset(¤t_ekf_state, 0, sizeof(current_ekf_state)); ZeroVariables(); @@ -348,11 +344,6 @@ void AttPosEKF::CovariancePrediction(float dt) } else { for (uint8_t i=16; i<=21; i++) processNoise[i] = 0; } - if (!inhibitGndState) { - processNoise[22] = dt * sqrtf(sq(states[4]) + sq(states[5])) * gndHgtSigma; - } else { - processNoise[22] = 0; - } // square all sigmas for (unsigned i = 0; i < n_states; i++) processNoise[i] = sq(processNoise[i]); @@ -451,7 +442,6 @@ void AttPosEKF::CovariancePrediction(float dt) nextP[0][19] = P[0][19] + P[1][19]*SF[7] + P[2][19]*SF[9] + P[3][19]*SF[8] + P[10][19]*SF[11] + P[11][19]*SPP[7] + P[12][19]*SPP[6]; nextP[0][20] = P[0][20] + P[1][20]*SF[7] + P[2][20]*SF[9] + P[3][20]*SF[8] + P[10][20]*SF[11] + P[11][20]*SPP[7] + P[12][20]*SPP[6]; nextP[0][21] = P[0][21] + P[1][21]*SF[7] + P[2][21]*SF[9] + P[3][21]*SF[8] + P[10][21]*SF[11] + P[11][21]*SPP[7] + P[12][21]*SPP[6]; - nextP[0][22] = P[0][22] + P[1][22]*SF[7] + P[2][22]*SF[9] + P[3][22]*SF[8] + P[10][22]*SF[11] + P[11][22]*SPP[7] + P[12][22]*SPP[6]; nextP[1][0] = P[1][0] + SQ[8] + P[0][0]*SF[6] + P[2][0]*SF[5] + P[3][0]*SF[9] + P[11][0]*SPP[6] - P[12][0]*SPP[7] - (P[10][0]*q0)/2 + SF[7]*(P[1][1] + P[0][1]*SF[6] + P[2][1]*SF[5] + P[3][1]*SF[9] + P[11][1]*SPP[6] - P[12][1]*SPP[7] - (P[10][1]*q0)/2) + SF[9]*(P[1][2] + P[0][2]*SF[6] + P[2][2]*SF[5] + P[3][2]*SF[9] + P[11][2]*SPP[6] - P[12][2]*SPP[7] - (P[10][2]*q0)/2) + SF[8]*(P[1][3] + P[0][3]*SF[6] + P[2][3]*SF[5] + P[3][3]*SF[9] + P[11][3]*SPP[6] - P[12][3]*SPP[7] - (P[10][3]*q0)/2) + SF[11]*(P[1][10] + P[0][10]*SF[6] + P[2][10]*SF[5] + P[3][10]*SF[9] + P[11][10]*SPP[6] - P[12][10]*SPP[7] - (P[10][10]*q0)/2) + SPP[7]*(P[1][11] + P[0][11]*SF[6] + P[2][11]*SF[5] + P[3][11]*SF[9] + P[11][11]*SPP[6] - P[12][11]*SPP[7] - (P[10][11]*q0)/2) + SPP[6]*(P[1][12] + P[0][12]*SF[6] + P[2][12]*SF[5] + P[3][12]*SF[9] + P[11][12]*SPP[6] - P[12][12]*SPP[7] - (P[10][12]*q0)/2); nextP[1][1] = P[1][1] + P[0][1]*SF[6] + P[2][1]*SF[5] + P[3][1]*SF[9] + P[11][1]*SPP[6] - P[12][1]*SPP[7] + daxCov*SQ[9] - (P[10][1]*q0)/2 + SF[6]*(P[1][0] + P[0][0]*SF[6] + P[2][0]*SF[5] + P[3][0]*SF[9] + P[11][0]*SPP[6] - P[12][0]*SPP[7] - (P[10][0]*q0)/2) + SF[5]*(P[1][2] + P[0][2]*SF[6] + P[2][2]*SF[5] + P[3][2]*SF[9] + P[11][2]*SPP[6] - P[12][2]*SPP[7] - (P[10][2]*q0)/2) + SF[9]*(P[1][3] + P[0][3]*SF[6] + P[2][3]*SF[5] + P[3][3]*SF[9] + P[11][3]*SPP[6] - P[12][3]*SPP[7] - (P[10][3]*q0)/2) + SPP[6]*(P[1][11] + P[0][11]*SF[6] + P[2][11]*SF[5] + P[3][11]*SF[9] + P[11][11]*SPP[6] - P[12][11]*SPP[7] - (P[10][11]*q0)/2) - SPP[7]*(P[1][12] + P[0][12]*SF[6] + P[2][12]*SF[5] + P[3][12]*SF[9] + P[11][12]*SPP[6] - P[12][12]*SPP[7] - (P[10][12]*q0)/2) + (dayCov*sq(q3))/4 + (dazCov*sq(q2))/4 - (q0*(P[1][10] + P[0][10]*SF[6] + P[2][10]*SF[5] + P[3][10]*SF[9] + P[11][10]*SPP[6] - P[12][10]*SPP[7] - (P[10][10]*q0)/2))/2; nextP[1][2] = P[1][2] + SQ[5] + P[0][2]*SF[6] + P[2][2]*SF[5] + P[3][2]*SF[9] + P[11][2]*SPP[6] - P[12][2]*SPP[7] - (P[10][2]*q0)/2 + SF[4]*(P[1][0] + P[0][0]*SF[6] + P[2][0]*SF[5] + P[3][0]*SF[9] + P[11][0]*SPP[6] - P[12][0]*SPP[7] - (P[10][0]*q0)/2) + SF[8]*(P[1][1] + P[0][1]*SF[6] + P[2][1]*SF[5] + P[3][1]*SF[9] + P[11][1]*SPP[6] - P[12][1]*SPP[7] - (P[10][1]*q0)/2) + SF[6]*(P[1][3] + P[0][3]*SF[6] + P[2][3]*SF[5] + P[3][3]*SF[9] + P[11][3]*SPP[6] - P[12][3]*SPP[7] - (P[10][3]*q0)/2) + SF[11]*(P[1][12] + P[0][12]*SF[6] + P[2][12]*SF[5] + P[3][12]*SF[9] + P[11][12]*SPP[6] - P[12][12]*SPP[7] - (P[10][12]*q0)/2) - SPP[6]*(P[1][10] + P[0][10]*SF[6] + P[2][10]*SF[5] + P[3][10]*SF[9] + P[11][10]*SPP[6] - P[12][10]*SPP[7] - (P[10][10]*q0)/2) - (q0*(P[1][11] + P[0][11]*SF[6] + P[2][11]*SF[5] + P[3][11]*SF[9] + P[11][11]*SPP[6] - P[12][11]*SPP[7] - (P[10][11]*q0)/2))/2; @@ -474,7 +464,6 @@ void AttPosEKF::CovariancePrediction(float dt) nextP[1][19] = P[1][19] + P[0][19]*SF[6] + P[2][19]*SF[5] + P[3][19]*SF[9] + P[11][19]*SPP[6] - P[12][19]*SPP[7] - (P[10][19]*q0)/2; nextP[1][20] = P[1][20] + P[0][20]*SF[6] + P[2][20]*SF[5] + P[3][20]*SF[9] + P[11][20]*SPP[6] - P[12][20]*SPP[7] - (P[10][20]*q0)/2; nextP[1][21] = P[1][21] + P[0][21]*SF[6] + P[2][21]*SF[5] + P[3][21]*SF[9] + P[11][21]*SPP[6] - P[12][21]*SPP[7] - (P[10][21]*q0)/2; - nextP[1][22] = P[1][22] + P[0][22]*SF[6] + P[2][22]*SF[5] + P[3][22]*SF[9] + P[11][22]*SPP[6] - P[12][22]*SPP[7] - (P[10][22]*q0)/2; nextP[2][0] = P[2][0] + SQ[7] + P[0][0]*SF[4] + P[1][0]*SF[8] + P[3][0]*SF[6] + P[12][0]*SF[11] - P[10][0]*SPP[6] - (P[11][0]*q0)/2 + SF[7]*(P[2][1] + P[0][1]*SF[4] + P[1][1]*SF[8] + P[3][1]*SF[6] + P[12][1]*SF[11] - P[10][1]*SPP[6] - (P[11][1]*q0)/2) + SF[9]*(P[2][2] + P[0][2]*SF[4] + P[1][2]*SF[8] + P[3][2]*SF[6] + P[12][2]*SF[11] - P[10][2]*SPP[6] - (P[11][2]*q0)/2) + SF[8]*(P[2][3] + P[0][3]*SF[4] + P[1][3]*SF[8] + P[3][3]*SF[6] + P[12][3]*SF[11] - P[10][3]*SPP[6] - (P[11][3]*q0)/2) + SF[11]*(P[2][10] + P[0][10]*SF[4] + P[1][10]*SF[8] + P[3][10]*SF[6] + P[12][10]*SF[11] - P[10][10]*SPP[6] - (P[11][10]*q0)/2) + SPP[7]*(P[2][11] + P[0][11]*SF[4] + P[1][11]*SF[8] + P[3][11]*SF[6] + P[12][11]*SF[11] - P[10][11]*SPP[6] - (P[11][11]*q0)/2) + SPP[6]*(P[2][12] + P[0][12]*SF[4] + P[1][12]*SF[8] + P[3][12]*SF[6] + P[12][12]*SF[11] - P[10][12]*SPP[6] - (P[11][12]*q0)/2); nextP[2][1] = P[2][1] + SQ[5] + P[0][1]*SF[4] + P[1][1]*SF[8] + P[3][1]*SF[6] + P[12][1]*SF[11] - P[10][1]*SPP[6] - (P[11][1]*q0)/2 + SF[6]*(P[2][0] + P[0][0]*SF[4] + P[1][0]*SF[8] + P[3][0]*SF[6] + P[12][0]*SF[11] - P[10][0]*SPP[6] - (P[11][0]*q0)/2) + SF[5]*(P[2][2] + P[0][2]*SF[4] + P[1][2]*SF[8] + P[3][2]*SF[6] + P[12][2]*SF[11] - P[10][2]*SPP[6] - (P[11][2]*q0)/2) + SF[9]*(P[2][3] + P[0][3]*SF[4] + P[1][3]*SF[8] + P[3][3]*SF[6] + P[12][3]*SF[11] - P[10][3]*SPP[6] - (P[11][3]*q0)/2) + SPP[6]*(P[2][11] + P[0][11]*SF[4] + P[1][11]*SF[8] + P[3][11]*SF[6] + P[12][11]*SF[11] - P[10][11]*SPP[6] - (P[11][11]*q0)/2) - SPP[7]*(P[2][12] + P[0][12]*SF[4] + P[1][12]*SF[8] + P[3][12]*SF[6] + P[12][12]*SF[11] - P[10][12]*SPP[6] - (P[11][12]*q0)/2) - (q0*(P[2][10] + P[0][10]*SF[4] + P[1][10]*SF[8] + P[3][10]*SF[6] + P[12][10]*SF[11] - P[10][10]*SPP[6] - (P[11][10]*q0)/2))/2; nextP[2][2] = P[2][2] + P[0][2]*SF[4] + P[1][2]*SF[8] + P[3][2]*SF[6] + P[12][2]*SF[11] - P[10][2]*SPP[6] + dayCov*SQ[9] + (dazCov*SQ[10])/4 - (P[11][2]*q0)/2 + SF[4]*(P[2][0] + P[0][0]*SF[4] + P[1][0]*SF[8] + P[3][0]*SF[6] + P[12][0]*SF[11] - P[10][0]*SPP[6] - (P[11][0]*q0)/2) + SF[8]*(P[2][1] + P[0][1]*SF[4] + P[1][1]*SF[8] + P[3][1]*SF[6] + P[12][1]*SF[11] - P[10][1]*SPP[6] - (P[11][1]*q0)/2) + SF[6]*(P[2][3] + P[0][3]*SF[4] + P[1][3]*SF[8] + P[3][3]*SF[6] + P[12][3]*SF[11] - P[10][3]*SPP[6] - (P[11][3]*q0)/2) + SF[11]*(P[2][12] + P[0][12]*SF[4] + P[1][12]*SF[8] + P[3][12]*SF[6] + P[12][12]*SF[11] - P[10][12]*SPP[6] - (P[11][12]*q0)/2) - SPP[6]*(P[2][10] + P[0][10]*SF[4] + P[1][10]*SF[8] + P[3][10]*SF[6] + P[12][10]*SF[11] - P[10][10]*SPP[6] - (P[11][10]*q0)/2) + (daxCov*sq(q3))/4 - (q0*(P[2][11] + P[0][11]*SF[4] + P[1][11]*SF[8] + P[3][11]*SF[6] + P[12][11]*SF[11] - P[10][11]*SPP[6] - (P[11][11]*q0)/2))/2; @@ -497,7 +486,6 @@ void AttPosEKF::CovariancePrediction(float dt) nextP[2][19] = P[2][19] + P[0][19]*SF[4] + P[1][19]*SF[8] + P[3][19]*SF[6] + P[12][19]*SF[11] - P[10][19]*SPP[6] - (P[11][19]*q0)/2; nextP[2][20] = P[2][20] + P[0][20]*SF[4] + P[1][20]*SF[8] + P[3][20]*SF[6] + P[12][20]*SF[11] - P[10][20]*SPP[6] - (P[11][20]*q0)/2; nextP[2][21] = P[2][21] + P[0][21]*SF[4] + P[1][21]*SF[8] + P[3][21]*SF[6] + P[12][21]*SF[11] - P[10][21]*SPP[6] - (P[11][21]*q0)/2; - nextP[2][22] = P[2][22] + P[0][22]*SF[4] + P[1][22]*SF[8] + P[3][22]*SF[6] + P[12][22]*SF[11] - P[10][22]*SPP[6] - (P[11][22]*q0)/2; nextP[3][0] = P[3][0] + SQ[6] + P[0][0]*SF[5] + P[1][0]*SF[4] + P[2][0]*SF[7] - P[11][0]*SF[11] + P[10][0]*SPP[7] - (P[12][0]*q0)/2 + SF[7]*(P[3][1] + P[0][1]*SF[5] + P[1][1]*SF[4] + P[2][1]*SF[7] - P[11][1]*SF[11] + P[10][1]*SPP[7] - (P[12][1]*q0)/2) + SF[9]*(P[3][2] + P[0][2]*SF[5] + P[1][2]*SF[4] + P[2][2]*SF[7] - P[11][2]*SF[11] + P[10][2]*SPP[7] - (P[12][2]*q0)/2) + SF[8]*(P[3][3] + P[0][3]*SF[5] + P[1][3]*SF[4] + P[2][3]*SF[7] - P[11][3]*SF[11] + P[10][3]*SPP[7] - (P[12][3]*q0)/2) + SF[11]*(P[3][10] + P[0][10]*SF[5] + P[1][10]*SF[4] + P[2][10]*SF[7] - P[11][10]*SF[11] + P[10][10]*SPP[7] - (P[12][10]*q0)/2) + SPP[7]*(P[3][11] + P[0][11]*SF[5] + P[1][11]*SF[4] + P[2][11]*SF[7] - P[11][11]*SF[11] + P[10][11]*SPP[7] - (P[12][11]*q0)/2) + SPP[6]*(P[3][12] + P[0][12]*SF[5] + P[1][12]*SF[4] + P[2][12]*SF[7] - P[11][12]*SF[11] + P[10][12]*SPP[7] - (P[12][12]*q0)/2); nextP[3][1] = P[3][1] + SQ[4] + P[0][1]*SF[5] + P[1][1]*SF[4] + P[2][1]*SF[7] - P[11][1]*SF[11] + P[10][1]*SPP[7] - (P[12][1]*q0)/2 + SF[6]*(P[3][0] + P[0][0]*SF[5] + P[1][0]*SF[4] + P[2][0]*SF[7] - P[11][0]*SF[11] + P[10][0]*SPP[7] - (P[12][0]*q0)/2) + SF[5]*(P[3][2] + P[0][2]*SF[5] + P[1][2]*SF[4] + P[2][2]*SF[7] - P[11][2]*SF[11] + P[10][2]*SPP[7] - (P[12][2]*q0)/2) + SF[9]*(P[3][3] + P[0][3]*SF[5] + P[1][3]*SF[4] + P[2][3]*SF[7] - P[11][3]*SF[11] + P[10][3]*SPP[7] - (P[12][3]*q0)/2) + SPP[6]*(P[3][11] + P[0][11]*SF[5] + P[1][11]*SF[4] + P[2][11]*SF[7] - P[11][11]*SF[11] + P[10][11]*SPP[7] - (P[12][11]*q0)/2) - SPP[7]*(P[3][12] + P[0][12]*SF[5] + P[1][12]*SF[4] + P[2][12]*SF[7] - P[11][12]*SF[11] + P[10][12]*SPP[7] - (P[12][12]*q0)/2) - (q0*(P[3][10] + P[0][10]*SF[5] + P[1][10]*SF[4] + P[2][10]*SF[7] - P[11][10]*SF[11] + P[10][10]*SPP[7] - (P[12][10]*q0)/2))/2; nextP[3][2] = P[3][2] + SQ[3] + P[0][2]*SF[5] + P[1][2]*SF[4] + P[2][2]*SF[7] - P[11][2]*SF[11] + P[10][2]*SPP[7] - (P[12][2]*q0)/2 + SF[4]*(P[3][0] + P[0][0]*SF[5] + P[1][0]*SF[4] + P[2][0]*SF[7] - P[11][0]*SF[11] + P[10][0]*SPP[7] - (P[12][0]*q0)/2) + SF[8]*(P[3][1] + P[0][1]*SF[5] + P[1][1]*SF[4] + P[2][1]*SF[7] - P[11][1]*SF[11] + P[10][1]*SPP[7] - (P[12][1]*q0)/2) + SF[6]*(P[3][3] + P[0][3]*SF[5] + P[1][3]*SF[4] + P[2][3]*SF[7] - P[11][3]*SF[11] + P[10][3]*SPP[7] - (P[12][3]*q0)/2) + SF[11]*(P[3][12] + P[0][12]*SF[5] + P[1][12]*SF[4] + P[2][12]*SF[7] - P[11][12]*SF[11] + P[10][12]*SPP[7] - (P[12][12]*q0)/2) - SPP[6]*(P[3][10] + P[0][10]*SF[5] + P[1][10]*SF[4] + P[2][10]*SF[7] - P[11][10]*SF[11] + P[10][10]*SPP[7] - (P[12][10]*q0)/2) - (q0*(P[3][11] + P[0][11]*SF[5] + P[1][11]*SF[4] + P[2][11]*SF[7] - P[11][11]*SF[11] + P[10][11]*SPP[7] - (P[12][11]*q0)/2))/2; @@ -520,7 +508,6 @@ void AttPosEKF::CovariancePrediction(float dt) nextP[3][19] = P[3][19] + P[0][19]*SF[5] + P[1][19]*SF[4] + P[2][19]*SF[7] - P[11][19]*SF[11] + P[10][19]*SPP[7] - (P[12][19]*q0)/2; nextP[3][20] = P[3][20] + P[0][20]*SF[5] + P[1][20]*SF[4] + P[2][20]*SF[7] - P[11][20]*SF[11] + P[10][20]*SPP[7] - (P[12][20]*q0)/2; nextP[3][21] = P[3][21] + P[0][21]*SF[5] + P[1][21]*SF[4] + P[2][21]*SF[7] - P[11][21]*SF[11] + P[10][21]*SPP[7] - (P[12][21]*q0)/2; - nextP[3][22] = P[3][22] + P[0][22]*SF[5] + P[1][22]*SF[4] + P[2][22]*SF[7] - P[11][22]*SF[11] + P[10][22]*SPP[7] - (P[12][22]*q0)/2; nextP[4][0] = P[4][0] + P[0][0]*SF[3] + P[1][0]*SF[1] + P[2][0]*SPP[0] - P[3][0]*SPP[2] - P[13][0]*SPP[4] + SF[7]*(P[4][1] + P[0][1]*SF[3] + P[1][1]*SF[1] + P[2][1]*SPP[0] - P[3][1]*SPP[2] - P[13][1]*SPP[4]) + SF[9]*(P[4][2] + P[0][2]*SF[3] + P[1][2]*SF[1] + P[2][2]*SPP[0] - P[3][2]*SPP[2] - P[13][2]*SPP[4]) + SF[8]*(P[4][3] + P[0][3]*SF[3] + P[1][3]*SF[1] + P[2][3]*SPP[0] - P[3][3]*SPP[2] - P[13][3]*SPP[4]) + SF[11]*(P[4][10] + P[0][10]*SF[3] + P[1][10]*SF[1] + P[2][10]*SPP[0] - P[3][10]*SPP[2] - P[13][10]*SPP[4]) + SPP[7]*(P[4][11] + P[0][11]*SF[3] + P[1][11]*SF[1] + P[2][11]*SPP[0] - P[3][11]*SPP[2] - P[13][11]*SPP[4]) + SPP[6]*(P[4][12] + P[0][12]*SF[3] + P[1][12]*SF[1] + P[2][12]*SPP[0] - P[3][12]*SPP[2] - P[13][12]*SPP[4]); nextP[4][1] = P[4][1] + P[0][1]*SF[3] + P[1][1]*SF[1] + P[2][1]*SPP[0] - P[3][1]*SPP[2] - P[13][1]*SPP[4] + SF[6]*(P[4][0] + P[0][0]*SF[3] + P[1][0]*SF[1] + P[2][0]*SPP[0] - P[3][0]*SPP[2] - P[13][0]*SPP[4]) + SF[5]*(P[4][2] + P[0][2]*SF[3] + P[1][2]*SF[1] + P[2][2]*SPP[0] - P[3][2]*SPP[2] - P[13][2]*SPP[4]) + SF[9]*(P[4][3] + P[0][3]*SF[3] + P[1][3]*SF[1] + P[2][3]*SPP[0] - P[3][3]*SPP[2] - P[13][3]*SPP[4]) + SPP[6]*(P[4][11] + P[0][11]*SF[3] + P[1][11]*SF[1] + P[2][11]*SPP[0] - P[3][11]*SPP[2] - P[13][11]*SPP[4]) - SPP[7]*(P[4][12] + P[0][12]*SF[3] + P[1][12]*SF[1] + P[2][12]*SPP[0] - P[3][12]*SPP[2] - P[13][12]*SPP[4]) - (q0*(P[4][10] + P[0][10]*SF[3] + P[1][10]*SF[1] + P[2][10]*SPP[0] - P[3][10]*SPP[2] - P[13][10]*SPP[4]))/2; nextP[4][2] = P[4][2] + P[0][2]*SF[3] + P[1][2]*SF[1] + P[2][2]*SPP[0] - P[3][2]*SPP[2] - P[13][2]*SPP[4] + SF[4]*(P[4][0] + P[0][0]*SF[3] + P[1][0]*SF[1] + P[2][0]*SPP[0] - P[3][0]*SPP[2] - P[13][0]*SPP[4]) + SF[8]*(P[4][1] + P[0][1]*SF[3] + P[1][1]*SF[1] + P[2][1]*SPP[0] - P[3][1]*SPP[2] - P[13][1]*SPP[4]) + SF[6]*(P[4][3] + P[0][3]*SF[3] + P[1][3]*SF[1] + P[2][3]*SPP[0] - P[3][3]*SPP[2] - P[13][3]*SPP[4]) + SF[11]*(P[4][12] + P[0][12]*SF[3] + P[1][12]*SF[1] + P[2][12]*SPP[0] - P[3][12]*SPP[2] - P[13][12]*SPP[4]) - SPP[6]*(P[4][10] + P[0][10]*SF[3] + P[1][10]*SF[1] + P[2][10]*SPP[0] - P[3][10]*SPP[2] - P[13][10]*SPP[4]) - (q0*(P[4][11] + P[0][11]*SF[3] + P[1][11]*SF[1] + P[2][11]*SPP[0] - P[3][11]*SPP[2] - P[13][11]*SPP[4]))/2; @@ -543,7 +530,6 @@ void AttPosEKF::CovariancePrediction(float dt) nextP[4][19] = P[4][19] + P[0][19]*SF[3] + P[1][19]*SF[1] + P[2][19]*SPP[0] - P[3][19]*SPP[2] - P[13][19]*SPP[4]; nextP[4][20] = P[4][20] + P[0][20]*SF[3] + P[1][20]*SF[1] + P[2][20]*SPP[0] - P[3][20]*SPP[2] - P[13][20]*SPP[4]; nextP[4][21] = P[4][21] + P[0][21]*SF[3] + P[1][21]*SF[1] + P[2][21]*SPP[0] - P[3][21]*SPP[2] - P[13][21]*SPP[4]; - nextP[4][22] = P[4][22] + P[0][22]*SF[3] + P[1][22]*SF[1] + P[2][22]*SPP[0] - P[3][22]*SPP[2] - P[13][22]*SPP[4]; nextP[5][0] = P[5][0] + P[0][0]*SF[2] + P[2][0]*SF[1] + P[3][0]*SF[3] - P[1][0]*SPP[0] + P[13][0]*SPP[3] + SF[7]*(P[5][1] + P[0][1]*SF[2] + P[2][1]*SF[1] + P[3][1]*SF[3] - P[1][1]*SPP[0] + P[13][1]*SPP[3]) + SF[9]*(P[5][2] + P[0][2]*SF[2] + P[2][2]*SF[1] + P[3][2]*SF[3] - P[1][2]*SPP[0] + P[13][2]*SPP[3]) + SF[8]*(P[5][3] + P[0][3]*SF[2] + P[2][3]*SF[1] + P[3][3]*SF[3] - P[1][3]*SPP[0] + P[13][3]*SPP[3]) + SF[11]*(P[5][10] + P[0][10]*SF[2] + P[2][10]*SF[1] + P[3][10]*SF[3] - P[1][10]*SPP[0] + P[13][10]*SPP[3]) + SPP[7]*(P[5][11] + P[0][11]*SF[2] + P[2][11]*SF[1] + P[3][11]*SF[3] - P[1][11]*SPP[0] + P[13][11]*SPP[3]) + SPP[6]*(P[5][12] + P[0][12]*SF[2] + P[2][12]*SF[1] + P[3][12]*SF[3] - P[1][12]*SPP[0] + P[13][12]*SPP[3]); nextP[5][1] = P[5][1] + P[0][1]*SF[2] + P[2][1]*SF[1] + P[3][1]*SF[3] - P[1][1]*SPP[0] + P[13][1]*SPP[3] + SF[6]*(P[5][0] + P[0][0]*SF[2] + P[2][0]*SF[1] + P[3][0]*SF[3] - P[1][0]*SPP[0] + P[13][0]*SPP[3]) + SF[5]*(P[5][2] + P[0][2]*SF[2] + P[2][2]*SF[1] + P[3][2]*SF[3] - P[1][2]*SPP[0] + P[13][2]*SPP[3]) + SF[9]*(P[5][3] + P[0][3]*SF[2] + P[2][3]*SF[1] + P[3][3]*SF[3] - P[1][3]*SPP[0] + P[13][3]*SPP[3]) + SPP[6]*(P[5][11] + P[0][11]*SF[2] + P[2][11]*SF[1] + P[3][11]*SF[3] - P[1][11]*SPP[0] + P[13][11]*SPP[3]) - SPP[7]*(P[5][12] + P[0][12]*SF[2] + P[2][12]*SF[1] + P[3][12]*SF[3] - P[1][12]*SPP[0] + P[13][12]*SPP[3]) - (q0*(P[5][10] + P[0][10]*SF[2] + P[2][10]*SF[1] + P[3][10]*SF[3] - P[1][10]*SPP[0] + P[13][10]*SPP[3]))/2; nextP[5][2] = P[5][2] + P[0][2]*SF[2] + P[2][2]*SF[1] + P[3][2]*SF[3] - P[1][2]*SPP[0] + P[13][2]*SPP[3] + SF[4]*(P[5][0] + P[0][0]*SF[2] + P[2][0]*SF[1] + P[3][0]*SF[3] - P[1][0]*SPP[0] + P[13][0]*SPP[3]) + SF[8]*(P[5][1] + P[0][1]*SF[2] + P[2][1]*SF[1] + P[3][1]*SF[3] - P[1][1]*SPP[0] + P[13][1]*SPP[3]) + SF[6]*(P[5][3] + P[0][3]*SF[2] + P[2][3]*SF[1] + P[3][3]*SF[3] - P[1][3]*SPP[0] + P[13][3]*SPP[3]) + SF[11]*(P[5][12] + P[0][12]*SF[2] + P[2][12]*SF[1] + P[3][12]*SF[3] - P[1][12]*SPP[0] + P[13][12]*SPP[3]) - SPP[6]*(P[5][10] + P[0][10]*SF[2] + P[2][10]*SF[1] + P[3][10]*SF[3] - P[1][10]*SPP[0] + P[13][10]*SPP[3]) - (q0*(P[5][11] + P[0][11]*SF[2] + P[2][11]*SF[1] + P[3][11]*SF[3] - P[1][11]*SPP[0] + P[13][11]*SPP[3]))/2; @@ -566,7 +552,6 @@ void AttPosEKF::CovariancePrediction(float dt) nextP[5][19] = P[5][19] + P[0][19]*SF[2] + P[2][19]*SF[1] + P[3][19]*SF[3] - P[1][19]*SPP[0] + P[13][19]*SPP[3]; nextP[5][20] = P[5][20] + P[0][20]*SF[2] + P[2][20]*SF[1] + P[3][20]*SF[3] - P[1][20]*SPP[0] + P[13][20]*SPP[3]; nextP[5][21] = P[5][21] + P[0][21]*SF[2] + P[2][21]*SF[1] + P[3][21]*SF[3] - P[1][21]*SPP[0] + P[13][21]*SPP[3]; - nextP[5][22] = P[5][22] + P[0][22]*SF[2] + P[2][22]*SF[1] + P[3][22]*SF[3] - P[1][22]*SPP[0] + P[13][22]*SPP[3]; nextP[6][0] = P[6][0] + P[1][0]*SF[2] + P[3][0]*SF[1] + P[0][0]*SPP[0] - P[2][0]*SPP[1] - P[13][0]*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + SF[7]*(P[6][1] + P[1][1]*SF[2] + P[3][1]*SF[1] + P[0][1]*SPP[0] - P[2][1]*SPP[1] - P[13][1]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[9]*(P[6][2] + P[1][2]*SF[2] + P[3][2]*SF[1] + P[0][2]*SPP[0] - P[2][2]*SPP[1] - P[13][2]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[8]*(P[6][3] + P[1][3]*SF[2] + P[3][3]*SF[1] + P[0][3]*SPP[0] - P[2][3]*SPP[1] - P[13][3]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[11]*(P[6][10] + P[1][10]*SF[2] + P[3][10]*SF[1] + P[0][10]*SPP[0] - P[2][10]*SPP[1] - P[13][10]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SPP[7]*(P[6][11] + P[1][11]*SF[2] + P[3][11]*SF[1] + P[0][11]*SPP[0] - P[2][11]*SPP[1] - P[13][11]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SPP[6]*(P[6][12] + P[1][12]*SF[2] + P[3][12]*SF[1] + P[0][12]*SPP[0] - P[2][12]*SPP[1] - P[13][12]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))); nextP[6][1] = P[6][1] + P[1][1]*SF[2] + P[3][1]*SF[1] + P[0][1]*SPP[0] - P[2][1]*SPP[1] - P[13][1]*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + SF[6]*(P[6][0] + P[1][0]*SF[2] + P[3][0]*SF[1] + P[0][0]*SPP[0] - P[2][0]*SPP[1] - P[13][0]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[5]*(P[6][2] + P[1][2]*SF[2] + P[3][2]*SF[1] + P[0][2]*SPP[0] - P[2][2]*SPP[1] - P[13][2]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[9]*(P[6][3] + P[1][3]*SF[2] + P[3][3]*SF[1] + P[0][3]*SPP[0] - P[2][3]*SPP[1] - P[13][3]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SPP[6]*(P[6][11] + P[1][11]*SF[2] + P[3][11]*SF[1] + P[0][11]*SPP[0] - P[2][11]*SPP[1] - P[13][11]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) - SPP[7]*(P[6][12] + P[1][12]*SF[2] + P[3][12]*SF[1] + P[0][12]*SPP[0] - P[2][12]*SPP[1] - P[13][12]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) - (q0*(P[6][10] + P[1][10]*SF[2] + P[3][10]*SF[1] + P[0][10]*SPP[0] - P[2][10]*SPP[1] - P[13][10]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))))/2; nextP[6][2] = P[6][2] + P[1][2]*SF[2] + P[3][2]*SF[1] + P[0][2]*SPP[0] - P[2][2]*SPP[1] - P[13][2]*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + SF[4]*(P[6][0] + P[1][0]*SF[2] + P[3][0]*SF[1] + P[0][0]*SPP[0] - P[2][0]*SPP[1] - P[13][0]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[8]*(P[6][1] + P[1][1]*SF[2] + P[3][1]*SF[1] + P[0][1]*SPP[0] - P[2][1]*SPP[1] - P[13][1]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[6]*(P[6][3] + P[1][3]*SF[2] + P[3][3]*SF[1] + P[0][3]*SPP[0] - P[2][3]*SPP[1] - P[13][3]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[11]*(P[6][12] + P[1][12]*SF[2] + P[3][12]*SF[1] + P[0][12]*SPP[0] - P[2][12]*SPP[1] - P[13][12]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) - SPP[6]*(P[6][10] + P[1][10]*SF[2] + P[3][10]*SF[1] + P[0][10]*SPP[0] - P[2][10]*SPP[1] - P[13][10]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) - (q0*(P[6][11] + P[1][11]*SF[2] + P[3][11]*SF[1] + P[0][11]*SPP[0] - P[2][11]*SPP[1] - P[13][11]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))))/2; @@ -589,7 +574,6 @@ void AttPosEKF::CovariancePrediction(float dt) nextP[6][19] = P[6][19] + P[1][19]*SF[2] + P[3][19]*SF[1] + P[0][19]*SPP[0] - P[2][19]*SPP[1] - P[13][19]*SPP[5]; nextP[6][20] = P[6][20] + P[1][20]*SF[2] + P[3][20]*SF[1] + P[0][20]*SPP[0] - P[2][20]*SPP[1] - P[13][20]*SPP[5]; nextP[6][21] = P[6][21] + P[1][21]*SF[2] + P[3][21]*SF[1] + P[0][21]*SPP[0] - P[2][21]*SPP[1] - P[13][21]*SPP[5]; - nextP[6][22] = P[6][22] + P[1][22]*SF[2] + P[3][22]*SF[1] + P[0][22]*SPP[0] - P[2][22]*SPP[1] - P[13][22]*SPP[5]; nextP[7][0] = P[7][0] + P[4][0]*dt + SF[7]*(P[7][1] + P[4][1]*dt) + SF[9]*(P[7][2] + P[4][2]*dt) + SF[8]*(P[7][3] + P[4][3]*dt) + SF[11]*(P[7][10] + P[4][10]*dt) + SPP[7]*(P[7][11] + P[4][11]*dt) + SPP[6]*(P[7][12] + P[4][12]*dt); nextP[7][1] = P[7][1] + P[4][1]*dt + SF[6]*(P[7][0] + P[4][0]*dt) + SF[5]*(P[7][2] + P[4][2]*dt) + SF[9]*(P[7][3] + P[4][3]*dt) + SPP[6]*(P[7][11] + P[4][11]*dt) - SPP[7]*(P[7][12] + P[4][12]*dt) - (q0*(P[7][10] + P[4][10]*dt))/2; nextP[7][2] = P[7][2] + P[4][2]*dt + SF[4]*(P[7][0] + P[4][0]*dt) + SF[8]*(P[7][1] + P[4][1]*dt) + SF[6]*(P[7][3] + P[4][3]*dt) + SF[11]*(P[7][12] + P[4][12]*dt) - SPP[6]*(P[7][10] + P[4][10]*dt) - (q0*(P[7][11] + P[4][11]*dt))/2; @@ -612,7 +596,6 @@ void AttPosEKF::CovariancePrediction(float dt) nextP[7][19] = P[7][19] + P[4][19]*dt; nextP[7][20] = P[7][20] + P[4][20]*dt; nextP[7][21] = P[7][21] + P[4][21]*dt; - nextP[7][22] = P[7][22] + P[4][22]*dt; nextP[8][0] = P[8][0] + P[5][0]*dt + SF[7]*(P[8][1] + P[5][1]*dt) + SF[9]*(P[8][2] + P[5][2]*dt) + SF[8]*(P[8][3] + P[5][3]*dt) + SF[11]*(P[8][10] + P[5][10]*dt) + SPP[7]*(P[8][11] + P[5][11]*dt) + SPP[6]*(P[8][12] + P[5][12]*dt); nextP[8][1] = P[8][1] + P[5][1]*dt + SF[6]*(P[8][0] + P[5][0]*dt) + SF[5]*(P[8][2] + P[5][2]*dt) + SF[9]*(P[8][3] + P[5][3]*dt) + SPP[6]*(P[8][11] + P[5][11]*dt) - SPP[7]*(P[8][12] + P[5][12]*dt) - (q0*(P[8][10] + P[5][10]*dt))/2; nextP[8][2] = P[8][2] + P[5][2]*dt + SF[4]*(P[8][0] + P[5][0]*dt) + SF[8]*(P[8][1] + P[5][1]*dt) + SF[6]*(P[8][3] + P[5][3]*dt) + SF[11]*(P[8][12] + P[5][12]*dt) - SPP[6]*(P[8][10] + P[5][10]*dt) - (q0*(P[8][11] + P[5][11]*dt))/2; @@ -635,7 +618,6 @@ void AttPosEKF::CovariancePrediction(float dt) nextP[8][19] = P[8][19] + P[5][19]*dt; nextP[8][20] = P[8][20] + P[5][20]*dt; nextP[8][21] = P[8][21] + P[5][21]*dt; - nextP[8][22] = P[8][22] + P[5][22]*dt; nextP[9][0] = P[9][0] + P[6][0]*dt + SF[7]*(P[9][1] + P[6][1]*dt) + SF[9]*(P[9][2] + P[6][2]*dt) + SF[8]*(P[9][3] + P[6][3]*dt) + SF[11]*(P[9][10] + P[6][10]*dt) + SPP[7]*(P[9][11] + P[6][11]*dt) + SPP[6]*(P[9][12] + P[6][12]*dt); nextP[9][1] = P[9][1] + P[6][1]*dt + SF[6]*(P[9][0] + P[6][0]*dt) + SF[5]*(P[9][2] + P[6][2]*dt) + SF[9]*(P[9][3] + P[6][3]*dt) + SPP[6]*(P[9][11] + P[6][11]*dt) - SPP[7]*(P[9][12] + P[6][12]*dt) - (q0*(P[9][10] + P[6][10]*dt))/2; nextP[9][2] = P[9][2] + P[6][2]*dt + SF[4]*(P[9][0] + P[6][0]*dt) + SF[8]*(P[9][1] + P[6][1]*dt) + SF[6]*(P[9][3] + P[6][3]*dt) + SF[11]*(P[9][12] + P[6][12]*dt) - SPP[6]*(P[9][10] + P[6][10]*dt) - (q0*(P[9][11] + P[6][11]*dt))/2; @@ -658,7 +640,6 @@ void AttPosEKF::CovariancePrediction(float dt) nextP[9][19] = P[9][19] + P[6][19]*dt; nextP[9][20] = P[9][20] + P[6][20]*dt; nextP[9][21] = P[9][21] + P[6][21]*dt; - nextP[9][22] = P[9][22] + P[6][22]*dt; nextP[10][0] = P[10][0] + P[10][1]*SF[7] + P[10][2]*SF[9] + P[10][3]*SF[8] + P[10][10]*SF[11] + P[10][11]*SPP[7] + P[10][12]*SPP[6]; nextP[10][1] = P[10][1] + P[10][0]*SF[6] + P[10][2]*SF[5] + P[10][3]*SF[9] + P[10][11]*SPP[6] - P[10][12]*SPP[7] - (P[10][10]*q0)/2; nextP[10][2] = P[10][2] + P[10][0]*SF[4] + P[10][1]*SF[8] + P[10][3]*SF[6] + P[10][12]*SF[11] - P[10][10]*SPP[6] - (P[10][11]*q0)/2; @@ -681,7 +662,6 @@ void AttPosEKF::CovariancePrediction(float dt) nextP[10][19] = P[10][19]; nextP[10][20] = P[10][20]; nextP[10][21] = P[10][21]; - nextP[10][22] = P[10][22]; nextP[11][0] = P[11][0] + P[11][1]*SF[7] + P[11][2]*SF[9] + P[11][3]*SF[8] + P[11][10]*SF[11] + P[11][11]*SPP[7] + P[11][12]*SPP[6]; nextP[11][1] = P[11][1] + P[11][0]*SF[6] + P[11][2]*SF[5] + P[11][3]*SF[9] + P[11][11]*SPP[6] - P[11][12]*SPP[7] - (P[11][10]*q0)/2; nextP[11][2] = P[11][2] + P[11][0]*SF[4] + P[11][1]*SF[8] + P[11][3]*SF[6] + P[11][12]*SF[11] - P[11][10]*SPP[6] - (P[11][11]*q0)/2; @@ -704,7 +684,6 @@ void AttPosEKF::CovariancePrediction(float dt) nextP[11][19] = P[11][19]; nextP[11][20] = P[11][20]; nextP[11][21] = P[11][21]; - nextP[11][22] = P[11][22]; nextP[12][0] = P[12][0] + P[12][1]*SF[7] + P[12][2]*SF[9] + P[12][3]*SF[8] + P[12][10]*SF[11] + P[12][11]*SPP[7] + P[12][12]*SPP[6]; nextP[12][1] = P[12][1] + P[12][0]*SF[6] + P[12][2]*SF[5] + P[12][3]*SF[9] + P[12][11]*SPP[6] - P[12][12]*SPP[7] - (P[12][10]*q0)/2; nextP[12][2] = P[12][2] + P[12][0]*SF[4] + P[12][1]*SF[8] + P[12][3]*SF[6] + P[12][12]*SF[11] - P[12][10]*SPP[6] - (P[12][11]*q0)/2; @@ -727,7 +706,6 @@ void AttPosEKF::CovariancePrediction(float dt) nextP[12][19] = P[12][19]; nextP[12][20] = P[12][20]; nextP[12][21] = P[12][21]; - nextP[12][22] = P[12][22]; nextP[13][0] = P[13][0] + P[13][1]*SF[7] + P[13][2]*SF[9] + P[13][3]*SF[8] + P[13][10]*SF[11] + P[13][11]*SPP[7] + P[13][12]*SPP[6]; nextP[13][1] = P[13][1] + P[13][0]*SF[6] + P[13][2]*SF[5] + P[13][3]*SF[9] + P[13][11]*SPP[6] - P[13][12]*SPP[7] - (P[13][10]*q0)/2; nextP[13][2] = P[13][2] + P[13][0]*SF[4] + P[13][1]*SF[8] + P[13][3]*SF[6] + P[13][12]*SF[11] - P[13][10]*SPP[6] - (P[13][11]*q0)/2; @@ -750,7 +728,6 @@ void AttPosEKF::CovariancePrediction(float dt) nextP[13][19] = P[13][19]; nextP[13][20] = P[13][20]; nextP[13][21] = P[13][21]; - nextP[13][22] = P[13][22]; nextP[14][0] = P[14][0] + P[14][1]*SF[7] + P[14][2]*SF[9] + P[14][3]*SF[8] + P[14][10]*SF[11] + P[14][11]*SPP[7] + P[14][12]*SPP[6]; nextP[14][1] = P[14][1] + P[14][0]*SF[6] + P[14][2]*SF[5] + P[14][3]*SF[9] + P[14][11]*SPP[6] - P[14][12]*SPP[7] - (P[14][10]*q0)/2; nextP[14][2] = P[14][2] + P[14][0]*SF[4] + P[14][1]*SF[8] + P[14][3]*SF[6] + P[14][12]*SF[11] - P[14][10]*SPP[6] - (P[14][11]*q0)/2; @@ -773,7 +750,6 @@ void AttPosEKF::CovariancePrediction(float dt) nextP[14][19] = P[14][19]; nextP[14][20] = P[14][20]; nextP[14][21] = P[14][21]; - nextP[14][22] = P[14][22]; nextP[15][0] = P[15][0] + P[15][1]*SF[7] + P[15][2]*SF[9] + P[15][3]*SF[8] + P[15][10]*SF[11] + P[15][11]*SPP[7] + P[15][12]*SPP[6]; nextP[15][1] = P[15][1] + P[15][0]*SF[6] + P[15][2]*SF[5] + P[15][3]*SF[9] + P[15][11]*SPP[6] - P[15][12]*SPP[7] - (P[15][10]*q0)/2; nextP[15][2] = P[15][2] + P[15][0]*SF[4] + P[15][1]*SF[8] + P[15][3]*SF[6] + P[15][12]*SF[11] - P[15][10]*SPP[6] - (P[15][11]*q0)/2; @@ -796,7 +772,6 @@ void AttPosEKF::CovariancePrediction(float dt) nextP[15][19] = P[15][19]; nextP[15][20] = P[15][20]; nextP[15][21] = P[15][21]; - nextP[15][22] = P[15][22]; nextP[16][0] = P[16][0] + P[16][1]*SF[7] + P[16][2]*SF[9] + P[16][3]*SF[8] + P[16][10]*SF[11] + P[16][11]*SPP[7] + P[16][12]*SPP[6]; nextP[16][1] = P[16][1] + P[16][0]*SF[6] + P[16][2]*SF[5] + P[16][3]*SF[9] + P[16][11]*SPP[6] - P[16][12]*SPP[7] - (P[16][10]*q0)/2; nextP[16][2] = P[16][2] + P[16][0]*SF[4] + P[16][1]*SF[8] + P[16][3]*SF[6] + P[16][12]*SF[11] - P[16][10]*SPP[6] - (P[16][11]*q0)/2; @@ -819,7 +794,6 @@ void AttPosEKF::CovariancePrediction(float dt) nextP[16][19] = P[16][19]; nextP[16][20] = P[16][20]; nextP[16][21] = P[16][21]; - nextP[16][22] = P[16][22]; nextP[17][0] = P[17][0] + P[17][1]*SF[7] + P[17][2]*SF[9] + P[17][3]*SF[8] + P[17][10]*SF[11] + P[17][11]*SPP[7] + P[17][12]*SPP[6]; nextP[17][1] = P[17][1] + P[17][0]*SF[6] + P[17][2]*SF[5] + P[17][3]*SF[9] + P[17][11]*SPP[6] - P[17][12]*SPP[7] - (P[17][10]*q0)/2; nextP[17][2] = P[17][2] + P[17][0]*SF[4] + P[17][1]*SF[8] + P[17][3]*SF[6] + P[17][12]*SF[11] - P[17][10]*SPP[6] - (P[17][11]*q0)/2; @@ -842,7 +816,6 @@ void AttPosEKF::CovariancePrediction(float dt) nextP[17][19] = P[17][19]; nextP[17][20] = P[17][20]; nextP[17][21] = P[17][21]; - nextP[17][22] = P[17][22]; nextP[18][0] = P[18][0] + P[18][1]*SF[7] + P[18][2]*SF[9] + P[18][3]*SF[8] + P[18][10]*SF[11] + P[18][11]*SPP[7] + P[18][12]*SPP[6]; nextP[18][1] = P[18][1] + P[18][0]*SF[6] + P[18][2]*SF[5] + P[18][3]*SF[9] + P[18][11]*SPP[6] - P[18][12]*SPP[7] - (P[18][10]*q0)/2; nextP[18][2] = P[18][2] + P[18][0]*SF[4] + P[18][1]*SF[8] + P[18][3]*SF[6] + P[18][12]*SF[11] - P[18][10]*SPP[6] - (P[18][11]*q0)/2; @@ -865,7 +838,6 @@ void AttPosEKF::CovariancePrediction(float dt) nextP[18][19] = P[18][19]; nextP[18][20] = P[18][20]; nextP[18][21] = P[18][21]; - nextP[18][22] = P[18][22]; nextP[19][0] = P[19][0] + P[19][1]*SF[7] + P[19][2]*SF[9] + P[19][3]*SF[8] + P[19][10]*SF[11] + P[19][11]*SPP[7] + P[19][12]*SPP[6]; nextP[19][1] = P[19][1] + P[19][0]*SF[6] + P[19][2]*SF[5] + P[19][3]*SF[9] + P[19][11]*SPP[6] - P[19][12]*SPP[7] - (P[19][10]*q0)/2; nextP[19][2] = P[19][2] + P[19][0]*SF[4] + P[19][1]*SF[8] + P[19][3]*SF[6] + P[19][12]*SF[11] - P[19][10]*SPP[6] - (P[19][11]*q0)/2; @@ -888,7 +860,6 @@ void AttPosEKF::CovariancePrediction(float dt) nextP[19][19] = P[19][19]; nextP[19][20] = P[19][20]; nextP[19][21] = P[19][21]; - nextP[19][22] = P[19][22]; nextP[20][0] = P[20][0] + P[20][1]*SF[7] + P[20][2]*SF[9] + P[20][3]*SF[8] + P[20][10]*SF[11] + P[20][11]*SPP[7] + P[20][12]*SPP[6]; nextP[20][1] = P[20][1] + P[20][0]*SF[6] + P[20][2]*SF[5] + P[20][3]*SF[9] + P[20][11]*SPP[6] - P[20][12]*SPP[7] - (P[20][10]*q0)/2; nextP[20][2] = P[20][2] + P[20][0]*SF[4] + P[20][1]*SF[8] + P[20][3]*SF[6] + P[20][12]*SF[11] - P[20][10]*SPP[6] - (P[20][11]*q0)/2; @@ -911,7 +882,6 @@ void AttPosEKF::CovariancePrediction(float dt) nextP[20][19] = P[20][19]; nextP[20][20] = P[20][20]; nextP[20][21] = P[20][21]; - nextP[20][22] = P[20][22]; nextP[21][0] = P[21][0] + P[21][1]*SF[7] + P[21][2]*SF[9] + P[21][3]*SF[8] + P[21][10]*SF[11] + P[21][11]*SPP[7] + P[21][12]*SPP[6]; nextP[21][1] = P[21][1] + P[21][0]*SF[6] + P[21][2]*SF[5] + P[21][3]*SF[9] + P[21][11]*SPP[6] - P[21][12]*SPP[7] - (P[21][10]*q0)/2; nextP[21][2] = P[21][2] + P[21][0]*SF[4] + P[21][1]*SF[8] + P[21][3]*SF[6] + P[21][12]*SF[11] - P[21][10]*SPP[6] - (P[21][11]*q0)/2; @@ -934,30 +904,6 @@ void AttPosEKF::CovariancePrediction(float dt) nextP[21][19] = P[21][19]; nextP[21][20] = P[21][20]; nextP[21][21] = P[21][21]; - nextP[21][22] = P[21][22]; - nextP[22][0] = P[22][0] + P[22][1]*SF[7] + P[22][2]*SF[9] + P[22][3]*SF[8] + P[22][10]*SF[11] + P[22][11]*SPP[7] + P[22][12]*SPP[6]; - nextP[22][1] = P[22][1] + P[22][0]*SF[6] + P[22][2]*SF[5] + P[22][3]*SF[9] + P[22][11]*SPP[6] - P[22][12]*SPP[7] - (P[22][10]*q0)/2; - nextP[22][2] = P[22][2] + P[22][0]*SF[4] + P[22][1]*SF[8] + P[22][3]*SF[6] + P[22][12]*SF[11] - P[22][10]*SPP[6] - (P[22][11]*q0)/2; - nextP[22][3] = P[22][3] + P[22][0]*SF[5] + P[22][1]*SF[4] + P[22][2]*SF[7] - P[22][11]*SF[11] + P[22][10]*SPP[7] - (P[22][12]*q0)/2; - nextP[22][4] = P[22][4] + P[22][1]*SF[1] + P[22][0]*SF[3] + P[22][2]*SPP[0] - P[22][3]*SPP[2] - P[22][13]*SPP[4]; - nextP[22][5] = P[22][5] + P[22][0]*SF[2] + P[22][2]*SF[1] + P[22][3]*SF[3] - P[22][1]*SPP[0] + P[22][13]*SPP[3]; - nextP[22][6] = P[22][6] + P[22][1]*SF[2] + P[22][3]*SF[1] + P[22][0]*SPP[0] - P[22][2]*SPP[1] - P[22][13]*SPP[5]; - nextP[22][7] = P[22][7] + P[22][4]*dt; - nextP[22][8] = P[22][8] + P[22][5]*dt; - nextP[22][9] = P[22][9] + P[22][6]*dt; - nextP[22][10] = P[22][10]; - nextP[22][11] = P[22][11]; - nextP[22][12] = P[22][12]; - nextP[22][13] = P[22][13]; - nextP[22][14] = P[22][14]; - nextP[22][15] = P[22][15]; - nextP[22][16] = P[22][16]; - nextP[22][17] = P[22][17]; - nextP[22][18] = P[22][18]; - nextP[22][19] = P[22][19]; - nextP[22][20] = P[22][20]; - nextP[22][21] = P[22][21]; - nextP[22][22] = P[22][22]; for (unsigned i = 0; i < n_states; i++) { @@ -1031,7 +977,7 @@ void AttPosEKF::FuseVelposNED() bool fuseData[6] = {false,false,false,false,false,false}; uint8_t stateIndex; uint8_t obsIndex; - uint8_t indexLimit = 22; + uint8_t indexLimit = 21; // declare variables used by state and covariance update calculations float velErr; @@ -1235,10 +1181,6 @@ void AttPosEKF::FuseVelposNED() Kfusion[i] = 0; } } - // Don't update terrain state if inhibited - if (inhibitGndState) { - Kfusion[22] = 0; - } // Calculate state corrections and re-normalise the quaternions for (uint8_t i = 0; i<=indexLimit; i++) @@ -1419,11 +1361,6 @@ void AttPosEKF::FuseMagnetometer() Kfusion[i] = 0; } } - if (!inhibitGndState) { - Kfusion[22] = SK_MX[0]*(P[22][19] + P[22][1]*SH_MAG[0] + P[22][3]*SH_MAG[2] + P[22][0]*SK_MX[3] - P[22][2]*SK_MX[2] - P[22][16]*SK_MX[1] + P[22][17]*SK_MX[5] - P[22][18]*SK_MX[4]); - } else { - Kfusion[22] = 0; - } varInnovMag[0] = 1.0f/SK_MX[0]; innovMag[0] = MagPred[0] - magData.x; } @@ -1669,32 +1606,6 @@ void AttPosEKF::FuseAirspeed() // Perform fusion of True Airspeed measurement if (useAirspeed && fuseVtasData && (VtasPred > 1.0f) && (VtasMeas > 8.0f)) { - - float altDiff = fabsf(windSpdFiltAltitude - hgtMea); - - if (isfinite(windSpdFiltClimb)) { - windSpdFiltClimb = ((1.0f - 0.0002f) * windSpdFiltClimb) + (0.0002f * states[6]); - } else { - windSpdFiltClimb = states[6]; - } - - if (altDiff < 20.0f) { - // Lowpass the output of the wind estimate - we want a long-term - // stable estimate, but not start to load into the overall dynamics - // of the system (which adjusting covariances would do) - - // Change filter coefficient based on altitude change rate - float windFiltCoeff = ConstrainFloat(fabsf(windSpdFiltClimb) / 1000.0f, 0.00005f, 0.2f); - - windSpdFiltNorth = ((1.0f - windFiltCoeff) * windSpdFiltNorth) + (windFiltCoeff * vwn); - windSpdFiltEast = ((1.0f - windFiltCoeff) * windSpdFiltEast) + (windFiltCoeff * vwe); - } else { - windSpdFiltNorth = vwn; - windSpdFiltEast = vwe; - } - - windSpdFiltAltitude = hgtMea; - // Calculate observation jacobians SH_TAS[0] = 1/(sqrt(sq(ve - vwe) + sq(vn - vwn) + sq(vd))); SH_TAS[1] = (SH_TAS[0]*(2.0f*ve - 2*vwe))/2.0f; @@ -1835,356 +1746,269 @@ void AttPosEKF::zeroRows(float (&covMat)[n_states][n_states], uint8_t first, uin } } -void AttPosEKF::FuseRangeFinder() -{ - - // Local variables - float rngPred; - float SH_RNG[5]; - float H_RNG[23]; - float SK_RNG[6]; - float cosRngTilt; - const float R_RNG = 0.25f; // 0.5 m2 rangefinder measurement variance - - // Copy required states to local variable names - float q0 = statesAtRngTime[0]; - float q1 = statesAtRngTime[1]; - float q2 = statesAtRngTime[2]; - float q3 = statesAtRngTime[3]; - float pd = statesAtRngTime[9]; - float ptd = statesAtRngTime[22]; - - // Need to check that our range finder tilt angle is less than 30 degrees and we are using range finder data - SH_RNG[4] = sinf(rngFinderPitch); - cosRngTilt = - Tbn.z.x * SH_RNG[4] + Tbn.z.z * cosf(rngFinderPitch); - if (useRangeFinder && fuseRngData && cosRngTilt > 0.87f) - { - // Calculate observation jacobian and Kalman gain ignoring all states other than the terrain offset - // This prevents the range finder measurement modifying any of the other filter states and significantly reduces computations - SH_RNG[0] = SH_RNG[4]*(2*q0*q2 - 2*q1*q3) - sq(q0) + sq(q1) + sq(q2) - sq(q3); - SH_RNG[1] = pd - ptd; - SH_RNG[2] = 1/sq(SH_RNG[0]); - SH_RNG[3] = 1/SH_RNG[0]; - for (uint8_t i = 0; i < n_states; i++) { - H_RNG[i] = 0.0f; - Kfusion[i] = 0.0f; - } - H_RNG[22] = -SH_RNG[3]; - SK_RNG[0] = 1/(R_RNG + SH_RNG[3]*(P[9][9]*SH_RNG[3] - P[22][9]*SH_RNG[3] + P[0][9]*SH_RNG[1]*SH_RNG[2]*(2*q0 - 2*q2*SH_RNG[4]) - P[1][9]*SH_RNG[1]*SH_RNG[2]*(2*q1 - 2*q3*SH_RNG[4]) - P[2][9]*SH_RNG[1]*SH_RNG[2]*(2*q2 + 2*q0*SH_RNG[4]) + P[3][9]*SH_RNG[1]*SH_RNG[2]*(2*q3 + 2*q1*SH_RNG[4])) - SH_RNG[3]*(P[9][22]*SH_RNG[3] - P[22][22]*SH_RNG[3] + P[0][22]*SH_RNG[1]*SH_RNG[2]*(2*q0 - 2*q2*SH_RNG[4]) - P[1][22]*SH_RNG[1]*SH_RNG[2]*(2*q1 - 2*q3*SH_RNG[4]) - P[2][22]*SH_RNG[1]*SH_RNG[2]*(2*q2 + 2*q0*SH_RNG[4]) + P[3][22]*SH_RNG[1]*SH_RNG[2]*(2*q3 + 2*q1*SH_RNG[4])) + SH_RNG[1]*SH_RNG[2]*(2*q0 - 2*q2*SH_RNG[4])*(P[9][0]*SH_RNG[3] - P[22][0]*SH_RNG[3] + P[0][0]*SH_RNG[1]*SH_RNG[2]*(2*q0 - 2*q2*SH_RNG[4]) - P[1][0]*SH_RNG[1]*SH_RNG[2]*(2*q1 - 2*q3*SH_RNG[4]) - P[2][0]*SH_RNG[1]*SH_RNG[2]*(2*q2 + 2*q0*SH_RNG[4]) + P[3][0]*SH_RNG[1]*SH_RNG[2]*(2*q3 + 2*q1*SH_RNG[4])) - SH_RNG[1]*SH_RNG[2]*(2*q1 - 2*q3*SH_RNG[4])*(P[9][1]*SH_RNG[3] - P[22][1]*SH_RNG[3] + P[0][1]*SH_RNG[1]*SH_RNG[2]*(2*q0 - 2*q2*SH_RNG[4]) - P[1][1]*SH_RNG[1]*SH_RNG[2]*(2*q1 - 2*q3*SH_RNG[4]) - P[2][1]*SH_RNG[1]*SH_RNG[2]*(2*q2 + 2*q0*SH_RNG[4]) + P[3][1]*SH_RNG[1]*SH_RNG[2]*(2*q3 + 2*q1*SH_RNG[4])) - SH_RNG[1]*SH_RNG[2]*(2*q2 + 2*q0*SH_RNG[4])*(P[9][2]*SH_RNG[3] - P[22][2]*SH_RNG[3] + P[0][2]*SH_RNG[1]*SH_RNG[2]*(2*q0 - 2*q2*SH_RNG[4]) - P[1][2]*SH_RNG[1]*SH_RNG[2]*(2*q1 - 2*q3*SH_RNG[4]) - P[2][2]*SH_RNG[1]*SH_RNG[2]*(2*q2 + 2*q0*SH_RNG[4]) + P[3][2]*SH_RNG[1]*SH_RNG[2]*(2*q3 + 2*q1*SH_RNG[4])) + SH_RNG[1]*SH_RNG[2]*(2*q3 + 2*q1*SH_RNG[4])*(P[9][3]*SH_RNG[3] - P[22][3]*SH_RNG[3] + P[0][3]*SH_RNG[1]*SH_RNG[2]*(2*q0 - 2*q2*SH_RNG[4]) - P[1][3]*SH_RNG[1]*SH_RNG[2]*(2*q1 - 2*q3*SH_RNG[4]) - P[2][3]*SH_RNG[1]*SH_RNG[2]*(2*q2 + 2*q0*SH_RNG[4]) + P[3][3]*SH_RNG[1]*SH_RNG[2]*(2*q3 + 2*q1*SH_RNG[4]))); - SK_RNG[1] = 2*q1 - 2*q3*SH_RNG[4]; - SK_RNG[2] = 2*q0 - 2*q2*SH_RNG[4]; - SK_RNG[3] = 2*q3 + 2*q1*SH_RNG[4]; - SK_RNG[4] = 2*q2 + 2*q0*SH_RNG[4]; - SK_RNG[5] = SH_RNG[2]; - Kfusion[22] = SK_RNG[0]*(P[22][9]*SH_RNG[3] - P[22][22]*SH_RNG[3] + P[22][0]*SH_RNG[1]*SK_RNG[2]*SK_RNG[5] - P[22][1]*SH_RNG[1]*SK_RNG[1]*SK_RNG[5] - P[22][2]*SH_RNG[1]*SK_RNG[4]*SK_RNG[5] + P[22][3]*SH_RNG[1]*SK_RNG[3]*SK_RNG[5]); - - // Calculate the innovation variance for data logging - varInnovRng = 1.0f/SK_RNG[0]; - - // Calculate the measurement innovation - rngPred = (ptd - pd)/cosRngTilt; - innovRng = rngPred - rngMea; - - // calculate the innovation consistency test ratio - auxRngTestRatio = sq(innovRng) / (sq(rngInnovGate) * varInnovRng); - - // Check the innovation for consistency and don't fuse if out of bounds - if (auxRngTestRatio < 1.0f) - { - // correct the state vector - states[22] = states[22] - Kfusion[22] * innovRng; - - // correct the covariance P = (I - K*H)*P - P[22][22] = P[22][22] - Kfusion[22] * H_RNG[22] * P[22][22]; - P[22][22] = ConstrainFloat(P[22][22], 0.0f, 10000.0f); - } - } - -} - void AttPosEKF::FuseOptFlow() { -// static uint8_t obsIndex; -// static float SH_LOS[13]; -// static float SKK_LOS[15]; -// static float SK_LOS[2]; -// static float q0 = 0.0f; -// static float q1 = 0.0f; -// static float q2 = 0.0f; -// static float q3 = 1.0f; -// static float vn = 0.0f; -// static float ve = 0.0f; -// static float vd = 0.0f; -// static float pd = 0.0f; -// static float ptd = 0.0f; -// static float R_LOS = 0.01f; -// static float losPred[2]; + static float SH_LOS[13]; + static float SK_LOS[9]; + static float q0 = 0.0f; + static float q1 = 0.0f; + static float q2 = 0.0f; + static float q3 = 1.0f; + static float vn = 0.0f; + static float ve = 0.0f; + static float vd = 0.0f; + static float pd = 0.0f; + static float ptd = 0.0f; + static float losPred[2]; -// // Transformation matrix from nav to body axes -// Mat3f Tnb_local; -// // Transformation matrix from body to sensor axes -// // assume camera is aligned with Z body axis plus a misalignment -// // defined by 3 small angles about X, Y and Z body axis -// Mat3f Tbs; -// Tbs.x.y = a3; -// Tbs.y.x = -a3; -// Tbs.x.z = -a2; -// Tbs.z.x = a2; -// Tbs.y.z = a1; -// Tbs.z.y = -a1; -// // Transformation matrix from navigation to sensor axes -// Mat3f Tns; -// float H_LOS[n_states]; -// for (uint8_t i = 0; i < n_states; i++) { -// H_LOS[i] = 0.0f; -// } -// Vector3f velNED_local; -// Vector3f relVelSensor; + // Transformation matrix from nav to body axes + float H_LOS[2][n_states]; + float K_LOS[2][n_states]; + Vector3f velNED_local; + Vector3f relVelSensor; -// // Perform sequential fusion of optical flow measurements only when in the air and tilt is less than 30 deg. -// if (useOpticalFlow && (fuseOptFlowData || obsIndex == 1) && !onGround && Tbs.z.z > 0.866f && rngMea > 5.0f && rngMea < 39.0f) -// { -// // Sequential fusion of XY components to spread processing load across -// // two prediction time steps. + // Perform sequential fusion of optical flow measurements only with valid tilt and height + flowStates[1] = maxf(flowStates[1], statesAtFlowTime[9] + minFlowRng); + float heightAboveGndEst = flowStates[1] - statesAtFlowTime[9]; + bool validTilt = Tnb.z.z > 0.71f; + if (validTilt) + { + // Sequential fusion of XY components. -// // Calculate observation jacobians and Kalman gains -// if (fuseOptFlowData) -// { -// // Copy required states to local variable names -// q0 = statesAtOptFlowTime[0]; -// q1 = statesAtOptFlowTime[1]; -// q2 = statesAtOptFlowTime[2]; -// q3 = statesAtOptFlowTime[3]; -// vn = statesAtOptFlowTime[4]; -// ve = statesAtOptFlowTime[5]; -// vd = statesAtOptFlowTime[6]; -// pd = statesAtOptFlowTime[9]; -// ptd = statesAtOptFlowTime[22]; -// velNED_local.x = vn; -// velNED_local.y = ve; -// velNED_local.z = vd; + // Calculate observation jacobians and Kalman gains + if (fuseOptFlowData) + { + // Copy required states to local variable names + q0 = statesAtFlowTime[0]; + q1 = statesAtFlowTime[1]; + q2 = statesAtFlowTime[2]; + q3 = statesAtFlowTime[3]; + vn = statesAtFlowTime[4]; + ve = statesAtFlowTime[5]; + vd = statesAtFlowTime[6]; + pd = statesAtFlowTime[9]; + ptd = flowStates[1]; + velNED_local.x = vn; + velNED_local.y = ve; + velNED_local.z = vd; -// // calculate rotation from NED to body axes -// float q00 = sq(q0); -// float q11 = sq(q1); -// float q22 = sq(q2); -// float q33 = sq(q3); -// float q01 = q0 * q1; -// float q02 = q0 * q2; -// float q03 = q0 * q3; -// float q12 = q1 * q2; -// float q13 = q1 * q3; -// float q23 = q2 * q3; -// Tnb_local.x.x = q00 + q11 - q22 - q33; -// Tnb_local.y.y = q00 - q11 + q22 - q33; -// Tnb_local.z.z = q00 - q11 - q22 + q33; -// Tnb_local.y.x = 2*(q12 - q03); -// Tnb_local.z.x = 2*(q13 + q02); -// Tnb_local.x.y = 2*(q12 + q03); -// Tnb_local.z.y = 2*(q23 - q01); -// Tnb_local.x.z = 2*(q13 - q02); -// Tnb_local.y.z = 2*(q23 + q01); + // calculate range from ground plain to centre of sensor fov assuming flat earth + float range = heightAboveGndEst/Tnb_flow.z.z; -// // calculate transformation from NED to sensor axes -// Tns = Tbs*Tnb_local; + // calculate relative velocity in sensor frame + relVelSensor = Tnb_flow*velNED_local; -// // calculate range from ground plain to centre of sensor fov assuming flat earth -// float range = ConstrainFloat(((ptd - pd)/Tns.z.z),0.5f,100.0f); + // divide velocity by range and include angular rate effects to get predicted angular LOS rates relative to X and Y axes + losPred[0] = relVelSensor.y/range; + losPred[1] = -relVelSensor.x/range; -// // calculate relative velocity in sensor frame -// relVelSensor = Tns*velNED_local; + // Calculate common expressions for observation jacobians + SH_LOS[0] = sq(q0) - sq(q1) - sq(q2) + sq(q3); + SH_LOS[1] = vn*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) - vd*(2*q0*q2 - 2*q1*q3) + ve*(2*q0*q3 + 2*q1*q2); + SH_LOS[2] = ve*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + vd*(2*q0*q1 + 2*q2*q3) - vn*(2*q0*q3 - 2*q1*q2); + SH_LOS[3] = 1/(pd - ptd); + SH_LOS[4] = 1/sq(pd - ptd); -// // divide velocity by range and include angular rate effects to get predicted angular LOS rates relative to X and Y axes -// losPred[0] = relVelSensor.y/range; -// losPred[1] = -relVelSensor.x/range; + // Calculate common expressions for Kalman gains + SK_LOS[0] = 1.0f/((R_LOS + sq(omegaAcrossFlowTime[0] * moCompR_LOS)) + (SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) + 2*q0*SH_LOS[1]*SH_LOS[3])*(P[0][0]*(SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) + 2*q0*SH_LOS[1]*SH_LOS[3]) + P[1][0]*(SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q1*SH_LOS[1]*SH_LOS[3]) - P[2][0]*(SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) + 2*q2*SH_LOS[1]*SH_LOS[3]) + P[3][0]*(SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) + 2*q3*SH_LOS[1]*SH_LOS[3]) + P[5][0]*SH_LOS[0]*SH_LOS[3]*(2*q0*q3 + 2*q1*q2) - P[6][0]*SH_LOS[0]*SH_LOS[3]*(2*q0*q2 - 2*q1*q3) - P[9][0]*SH_LOS[0]*SH_LOS[1]*SH_LOS[4] + P[4][0]*SH_LOS[0]*SH_LOS[3]*(sq(q0) + sq(q1) - sq(q2) - sq(q3))) + (SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q1*SH_LOS[1]*SH_LOS[3])*(P[0][1]*(SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) + 2*q0*SH_LOS[1]*SH_LOS[3]) + P[1][1]*(SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q1*SH_LOS[1]*SH_LOS[3]) - P[2][1]*(SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) + 2*q2*SH_LOS[1]*SH_LOS[3]) + P[3][1]*(SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) + 2*q3*SH_LOS[1]*SH_LOS[3]) + P[5][1]*SH_LOS[0]*SH_LOS[3]*(2*q0*q3 + 2*q1*q2) - P[6][1]*SH_LOS[0]*SH_LOS[3]*(2*q0*q2 - 2*q1*q3) - P[9][1]*SH_LOS[0]*SH_LOS[1]*SH_LOS[4] + P[4][1]*SH_LOS[0]*SH_LOS[3]*(sq(q0) + sq(q1) - sq(q2) - sq(q3))) - (SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) + 2*q2*SH_LOS[1]*SH_LOS[3])*(P[0][2]*(SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) + 2*q0*SH_LOS[1]*SH_LOS[3]) + P[1][2]*(SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q1*SH_LOS[1]*SH_LOS[3]) - P[2][2]*(SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) + 2*q2*SH_LOS[1]*SH_LOS[3]) + P[3][2]*(SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) + 2*q3*SH_LOS[1]*SH_LOS[3]) + P[5][2]*SH_LOS[0]*SH_LOS[3]*(2*q0*q3 + 2*q1*q2) - P[6][2]*SH_LOS[0]*SH_LOS[3]*(2*q0*q2 - 2*q1*q3) - P[9][2]*SH_LOS[0]*SH_LOS[1]*SH_LOS[4] + P[4][2]*SH_LOS[0]*SH_LOS[3]*(sq(q0) + sq(q1) - sq(q2) - sq(q3))) + (SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) + 2*q3*SH_LOS[1]*SH_LOS[3])*(P[0][3]*(SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) + 2*q0*SH_LOS[1]*SH_LOS[3]) + P[1][3]*(SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q1*SH_LOS[1]*SH_LOS[3]) - P[2][3]*(SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) + 2*q2*SH_LOS[1]*SH_LOS[3]) + P[3][3]*(SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) + 2*q3*SH_LOS[1]*SH_LOS[3]) + P[5][3]*SH_LOS[0]*SH_LOS[3]*(2*q0*q3 + 2*q1*q2) - P[6][3]*SH_LOS[0]*SH_LOS[3]*(2*q0*q2 - 2*q1*q3) - P[9][3]*SH_LOS[0]*SH_LOS[1]*SH_LOS[4] + P[4][3]*SH_LOS[0]*SH_LOS[3]*(sq(q0) + sq(q1) - sq(q2) - sq(q3))) - SH_LOS[0]*SH_LOS[1]*SH_LOS[4]*(P[0][9]*(SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) + 2*q0*SH_LOS[1]*SH_LOS[3]) + P[1][9]*(SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q1*SH_LOS[1]*SH_LOS[3]) - P[2][9]*(SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) + 2*q2*SH_LOS[1]*SH_LOS[3]) + P[3][9]*(SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) + 2*q3*SH_LOS[1]*SH_LOS[3]) + P[5][9]*SH_LOS[0]*SH_LOS[3]*(2*q0*q3 + 2*q1*q2) - P[6][9]*SH_LOS[0]*SH_LOS[3]*(2*q0*q2 - 2*q1*q3) - P[9][9]*SH_LOS[0]*SH_LOS[1]*SH_LOS[4] + P[4][9]*SH_LOS[0]*SH_LOS[3]*(sq(q0) + sq(q1) - sq(q2) - sq(q3))) + SH_LOS[0]*SH_LOS[3]*(sq(q0) + sq(q1) - sq(q2) - sq(q3))*(P[0][4]*(SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) + 2*q0*SH_LOS[1]*SH_LOS[3]) + P[1][4]*(SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q1*SH_LOS[1]*SH_LOS[3]) - P[2][4]*(SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) + 2*q2*SH_LOS[1]*SH_LOS[3]) + P[3][4]*(SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) + 2*q3*SH_LOS[1]*SH_LOS[3]) + P[5][4]*SH_LOS[0]*SH_LOS[3]*(2*q0*q3 + 2*q1*q2) - P[6][4]*SH_LOS[0]*SH_LOS[3]*(2*q0*q2 - 2*q1*q3) - P[9][4]*SH_LOS[0]*SH_LOS[1]*SH_LOS[4] + P[4][4]*SH_LOS[0]*SH_LOS[3]*(sq(q0) + sq(q1) - sq(q2) - sq(q3))) + SH_LOS[0]*SH_LOS[3]*(2*q0*q3 + 2*q1*q2)*(P[0][5]*(SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) + 2*q0*SH_LOS[1]*SH_LOS[3]) + P[1][5]*(SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q1*SH_LOS[1]*SH_LOS[3]) - P[2][5]*(SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) + 2*q2*SH_LOS[1]*SH_LOS[3]) + P[3][5]*(SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) + 2*q3*SH_LOS[1]*SH_LOS[3]) + P[5][5]*SH_LOS[0]*SH_LOS[3]*(2*q0*q3 + 2*q1*q2) - P[6][5]*SH_LOS[0]*SH_LOS[3]*(2*q0*q2 - 2*q1*q3) - P[9][5]*SH_LOS[0]*SH_LOS[1]*SH_LOS[4] + P[4][5]*SH_LOS[0]*SH_LOS[3]*(sq(q0) + sq(q1) - sq(q2) - sq(q3))) - SH_LOS[0]*SH_LOS[3]*(2*q0*q2 - 2*q1*q3)*(P[0][6]*(SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) + 2*q0*SH_LOS[1]*SH_LOS[3]) + P[1][6]*(SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q1*SH_LOS[1]*SH_LOS[3]) - P[2][6]*(SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) + 2*q2*SH_LOS[1]*SH_LOS[3]) + P[3][6]*(SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) + 2*q3*SH_LOS[1]*SH_LOS[3]) + P[5][6]*SH_LOS[0]*SH_LOS[3]*(2*q0*q3 + 2*q1*q2) - P[6][6]*SH_LOS[0]*SH_LOS[3]*(2*q0*q2 - 2*q1*q3) - P[9][6]*SH_LOS[0]*SH_LOS[1]*SH_LOS[4] + P[4][6]*SH_LOS[0]*SH_LOS[3]*(sq(q0) + sq(q1) - sq(q2) - sq(q3)))); + SK_LOS[1] = 1.0f/((R_LOS + sq(omegaAcrossFlowTime[1] * moCompR_LOS))+ (SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) + 2*q0*SH_LOS[2]*SH_LOS[3])*(P[0][0]*(SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) + 2*q0*SH_LOS[2]*SH_LOS[3]) + P[1][0]*(SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) - 2*q1*SH_LOS[2]*SH_LOS[3]) + P[2][0]*(SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q2*SH_LOS[2]*SH_LOS[3]) - P[3][0]*(SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) - 2*q3*SH_LOS[2]*SH_LOS[3]) - P[4][0]*SH_LOS[0]*SH_LOS[3]*(2*q0*q3 - 2*q1*q2) + P[6][0]*SH_LOS[0]*SH_LOS[3]*(2*q0*q1 + 2*q2*q3) - P[9][0]*SH_LOS[0]*SH_LOS[2]*SH_LOS[4] + P[5][0]*SH_LOS[0]*SH_LOS[3]*(sq(q0) - sq(q1) + sq(q2) - sq(q3))) + (SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) - 2*q1*SH_LOS[2]*SH_LOS[3])*(P[0][1]*(SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) + 2*q0*SH_LOS[2]*SH_LOS[3]) + P[1][1]*(SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) - 2*q1*SH_LOS[2]*SH_LOS[3]) + P[2][1]*(SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q2*SH_LOS[2]*SH_LOS[3]) - P[3][1]*(SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) - 2*q3*SH_LOS[2]*SH_LOS[3]) - P[4][1]*SH_LOS[0]*SH_LOS[3]*(2*q0*q3 - 2*q1*q2) + P[6][1]*SH_LOS[0]*SH_LOS[3]*(2*q0*q1 + 2*q2*q3) - P[9][1]*SH_LOS[0]*SH_LOS[2]*SH_LOS[4] + P[5][1]*SH_LOS[0]*SH_LOS[3]*(sq(q0) - sq(q1) + sq(q2) - sq(q3))) + (SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q2*SH_LOS[2]*SH_LOS[3])*(P[0][2]*(SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) + 2*q0*SH_LOS[2]*SH_LOS[3]) + P[1][2]*(SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) - 2*q1*SH_LOS[2]*SH_LOS[3]) + P[2][2]*(SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q2*SH_LOS[2]*SH_LOS[3]) - P[3][2]*(SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) - 2*q3*SH_LOS[2]*SH_LOS[3]) - P[4][2]*SH_LOS[0]*SH_LOS[3]*(2*q0*q3 - 2*q1*q2) + P[6][2]*SH_LOS[0]*SH_LOS[3]*(2*q0*q1 + 2*q2*q3) - P[9][2]*SH_LOS[0]*SH_LOS[2]*SH_LOS[4] + P[5][2]*SH_LOS[0]*SH_LOS[3]*(sq(q0) - sq(q1) + sq(q2) - sq(q3))) - (SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) - 2*q3*SH_LOS[2]*SH_LOS[3])*(P[0][3]*(SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) + 2*q0*SH_LOS[2]*SH_LOS[3]) + P[1][3]*(SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) - 2*q1*SH_LOS[2]*SH_LOS[3]) + P[2][3]*(SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q2*SH_LOS[2]*SH_LOS[3]) - P[3][3]*(SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) - 2*q3*SH_LOS[2]*SH_LOS[3]) - P[4][3]*SH_LOS[0]*SH_LOS[3]*(2*q0*q3 - 2*q1*q2) + P[6][3]*SH_LOS[0]*SH_LOS[3]*(2*q0*q1 + 2*q2*q3) - P[9][3]*SH_LOS[0]*SH_LOS[2]*SH_LOS[4] + P[5][3]*SH_LOS[0]*SH_LOS[3]*(sq(q0) - sq(q1) + sq(q2) - sq(q3))) - SH_LOS[0]*SH_LOS[2]*SH_LOS[4]*(P[0][9]*(SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) + 2*q0*SH_LOS[2]*SH_LOS[3]) + P[1][9]*(SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) - 2*q1*SH_LOS[2]*SH_LOS[3]) + P[2][9]*(SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q2*SH_LOS[2]*SH_LOS[3]) - P[3][9]*(SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) - 2*q3*SH_LOS[2]*SH_LOS[3]) - P[4][9]*SH_LOS[0]*SH_LOS[3]*(2*q0*q3 - 2*q1*q2) + P[6][9]*SH_LOS[0]*SH_LOS[3]*(2*q0*q1 + 2*q2*q3) - P[9][9]*SH_LOS[0]*SH_LOS[2]*SH_LOS[4] + P[5][9]*SH_LOS[0]*SH_LOS[3]*(sq(q0) - sq(q1) + sq(q2) - sq(q3))) + SH_LOS[0]*SH_LOS[3]*(sq(q0) - sq(q1) + sq(q2) - sq(q3))*(P[0][5]*(SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) + 2*q0*SH_LOS[2]*SH_LOS[3]) + P[1][5]*(SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) - 2*q1*SH_LOS[2]*SH_LOS[3]) + P[2][5]*(SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q2*SH_LOS[2]*SH_LOS[3]) - P[3][5]*(SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) - 2*q3*SH_LOS[2]*SH_LOS[3]) - P[4][5]*SH_LOS[0]*SH_LOS[3]*(2*q0*q3 - 2*q1*q2) + P[6][5]*SH_LOS[0]*SH_LOS[3]*(2*q0*q1 + 2*q2*q3) - P[9][5]*SH_LOS[0]*SH_LOS[2]*SH_LOS[4] + P[5][5]*SH_LOS[0]*SH_LOS[3]*(sq(q0) - sq(q1) + sq(q2) - sq(q3))) - SH_LOS[0]*SH_LOS[3]*(2*q0*q3 - 2*q1*q2)*(P[0][4]*(SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) + 2*q0*SH_LOS[2]*SH_LOS[3]) + P[1][4]*(SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) - 2*q1*SH_LOS[2]*SH_LOS[3]) + P[2][4]*(SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q2*SH_LOS[2]*SH_LOS[3]) - P[3][4]*(SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) - 2*q3*SH_LOS[2]*SH_LOS[3]) - P[4][4]*SH_LOS[0]*SH_LOS[3]*(2*q0*q3 - 2*q1*q2) + P[6][4]*SH_LOS[0]*SH_LOS[3]*(2*q0*q1 + 2*q2*q3) - P[9][4]*SH_LOS[0]*SH_LOS[2]*SH_LOS[4] + P[5][4]*SH_LOS[0]*SH_LOS[3]*(sq(q0) - sq(q1) + sq(q2) - sq(q3))) + SH_LOS[0]*SH_LOS[3]*(2*q0*q1 + 2*q2*q3)*(P[0][6]*(SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) + 2*q0*SH_LOS[2]*SH_LOS[3]) + P[1][6]*(SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) - 2*q1*SH_LOS[2]*SH_LOS[3]) + P[2][6]*(SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q2*SH_LOS[2]*SH_LOS[3]) - P[3][6]*(SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) - 2*q3*SH_LOS[2]*SH_LOS[3]) - P[4][6]*SH_LOS[0]*SH_LOS[3]*(2*q0*q3 - 2*q1*q2) + P[6][6]*SH_LOS[0]*SH_LOS[3]*(2*q0*q1 + 2*q2*q3) - P[9][6]*SH_LOS[0]*SH_LOS[2]*SH_LOS[4] + P[5][6]*SH_LOS[0]*SH_LOS[3]*(sq(q0) - sq(q1) + sq(q2) - sq(q3)))); + SK_LOS[2] = SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn); + SK_LOS[3] = SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn); + SK_LOS[4] = SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn); + SK_LOS[5] = SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn); + SK_LOS[6] = sq(q0) - sq(q1) + sq(q2) - sq(q3); + SK_LOS[7] = sq(q0) + sq(q1) - sq(q2) - sq(q3); + SK_LOS[8] = SH_LOS[3]; -// //printf("relVelSensor.x=%5.1f, relVelSensor.y=%5.1f\n", relVelSensor.x, relVelSensor.y); -// //printf("Xpred=%5.2f, Xmea=%5.2f, Ypred=%5.2f, Ymea=%5.2f, delAng.x=%4.4f, delAng.y=%4.4f\n", losPred[0], losData[0], losPred[1], losData[1], delAng.x, delAng.y); -// //printf("omegaX=%5.2f, omegaY=%5.2f, velY=%5.1f velX=%5.1f\n, range=%5.1f\n", delAngRel.x/dt, delAngRel.y/dt, relVelSensor.y, relVelSensor.x, range); + // Calculate common intermediate terms + float tempVar[9]; + tempVar[0] = SH_LOS[0]*SK_LOS[6]*SK_LOS[8]; + tempVar[1] = SH_LOS[0]*SH_LOS[2]*SH_LOS[4]; + tempVar[2] = 2.0f*SH_LOS[2]*SK_LOS[8]; + tempVar[3] = SH_LOS[0]*SK_LOS[8]*(2.0f*q0*q1 + 2.0f*q2*q3); + tempVar[4] = SH_LOS[0]*SK_LOS[8]*(2.0f*q0*q3 - 2.0f*q1*q2); + tempVar[5] = (SK_LOS[5] - q2*tempVar[2]); + tempVar[6] = (SK_LOS[2] - q3*tempVar[2]); + tempVar[7] = (SK_LOS[3] - q1*tempVar[2]); + tempVar[8] = (SK_LOS[4] + q0*tempVar[2]); -// // Calculate observation jacobians -// SH_LOS[0] = a1*(2*q0*q1 + 2*q2*q3) + a2*(2*q0*q2 - 2*q1*q3) - sq(q0) + sq(q1) + sq(q2) - sq(q3); -// SH_LOS[1] = vd*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3)) - ve*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)) + vn*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3)); -// SH_LOS[2] = ve*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)) - vd*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3)) + vn*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3)); -// SH_LOS[3] = 1/(pd - ptd); -// SH_LOS[4] = 2*q1 - 2*a2*q3 + 2*a3*q2; -// SH_LOS[5] = 2*a2*q2 - 2*q0 + 2*a3*q3; -// SH_LOS[6] = 2*q2 + 2*a2*q0 - 2*a3*q1; -// SH_LOS[7] = 1/sq(pd - ptd); -// SH_LOS[8] = 2*q2 + 2*a1*q3 - 2*a3*q1; -// SH_LOS[9] = 2*q3 - 2*a1*q2 + 2*a3*q0; -// SH_LOS[10] = 2*a1*q1 - 2*q0 + 2*a3*q3; -// SH_LOS[11] = 2*q3 + 2*a2*q1 + 2*a3*q0; -// SH_LOS[12] = 2*q1 + 2*a1*q0 + 2*a3*q2; + // calculate observation jacobians for X LOS rate + for (uint8_t i = 0; i < 23; i++) H_LOS[0][i] = 0; + H_LOS[0][0] = - SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) - 2*q0*SH_LOS[2]*SH_LOS[3]; + H_LOS[0][1] = 2*q1*SH_LOS[2]*SH_LOS[3] - SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn); + H_LOS[0][2] = 2*q2*SH_LOS[2]*SH_LOS[3] - SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn); + H_LOS[0][3] = SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) - 2*q3*SH_LOS[2]*SH_LOS[3]; + H_LOS[0][4] = SH_LOS[0]*SH_LOS[3]*(2*q0*q3 - 2*q1*q2); + H_LOS[0][5] = -SH_LOS[0]*SH_LOS[3]*(sq(q0) - sq(q1) + sq(q2) - sq(q3)); + H_LOS[0][6] = -SH_LOS[0]*SH_LOS[3]*(2*q0*q1 + 2*q2*q3); + H_LOS[0][9] = tempVar[1]; -// for (uint8_t i = 0; i < n_states; i++) H_LOS[i] = 0; -// H_LOS[0] = - SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9]); -// H_LOS[1] = - SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8]); -// H_LOS[2] = SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12]) - SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3); -// H_LOS[3] = SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10]); -// H_LOS[4] = -SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3)); -// H_LOS[5] = -SH_LOS[0]*SH_LOS[3]*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)); -// H_LOS[6] = SH_LOS[0]*SH_LOS[3]*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3)); -// H_LOS[9] = SH_LOS[0]*SH_LOS[2]*SH_LOS[7]; -// H_LOS[22] = -SH_LOS[0]*SH_LOS[2]*SH_LOS[7]; + // calculate Kalman gains for X LOS rate + K_LOS[0][0] = -(P[0][0]*tempVar[8] + P[0][1]*tempVar[7] - P[0][3]*tempVar[6] + P[0][2]*tempVar[5] - P[0][4]*tempVar[4] + P[0][6]*tempVar[3] - P[0][9]*tempVar[1] + P[0][5]*tempVar[0])/(R_LOS + tempVar[8]*(P[0][0]*tempVar[8] + P[1][0]*tempVar[7] + P[2][0]*tempVar[5] - P[3][0]*tempVar[6] - P[4][0]*tempVar[4] + P[6][0]*tempVar[3] - P[9][0]*tempVar[1] + P[5][0]*tempVar[0]) + tempVar[7]*(P[0][1]*tempVar[8] + P[1][1]*tempVar[7] + P[2][1]*tempVar[5] - P[3][1]*tempVar[6] - P[4][1]*tempVar[4] + P[6][1]*tempVar[3] - P[9][1]*tempVar[1] + P[5][1]*tempVar[0]) + tempVar[5]*(P[0][2]*tempVar[8] + P[1][2]*tempVar[7] + P[2][2]*tempVar[5] - P[3][2]*tempVar[6] - P[4][2]*tempVar[4] + P[6][2]*tempVar[3] - P[9][2]*tempVar[1] + P[5][2]*tempVar[0]) - tempVar[6]*(P[0][3]*tempVar[8] + P[1][3]*tempVar[7] + P[2][3]*tempVar[5] - P[3][3]*tempVar[6] - P[4][3]*tempVar[4] + P[6][3]*tempVar[3] - P[9][3]*tempVar[1] + P[5][3]*tempVar[0]) - tempVar[4]*(P[0][4]*tempVar[8] + P[1][4]*tempVar[7] + P[2][4]*tempVar[5] - P[3][4]*tempVar[6] - P[4][4]*tempVar[4] + P[6][4]*tempVar[3] - P[9][4]*tempVar[1] + P[5][4]*tempVar[0]) + tempVar[3]*(P[0][6]*tempVar[8] + P[1][6]*tempVar[7] + P[2][6]*tempVar[5] - P[3][6]*tempVar[6] - P[4][6]*tempVar[4] + P[6][6]*tempVar[3] - P[9][6]*tempVar[1] + P[5][6]*tempVar[0]) - tempVar[1]*(P[0][9]*tempVar[8] + P[1][9]*tempVar[7] + P[2][9]*tempVar[5] - P[3][9]*tempVar[6] - P[4][9]*tempVar[4] + P[6][9]*tempVar[3] - P[9][9]*tempVar[1] + P[5][9]*tempVar[0]) + tempVar[0]*(P[0][5]*tempVar[8] + P[1][5]*tempVar[7] + P[2][5]*tempVar[5] - P[3][5]*tempVar[6] - P[4][5]*tempVar[4] + P[6][5]*tempVar[3] - P[9][5]*tempVar[1] + P[5][5]*tempVar[0])); + K_LOS[0][1] = -SK_LOS[1]*(P[1][0]*tempVar[8] + P[1][1]*tempVar[7] - P[1][3]*tempVar[6] + P[1][2]*tempVar[5] - P[1][4]*tempVar[4] + P[1][6]*tempVar[3] - P[1][9]*tempVar[1] + P[1][5]*tempVar[0]); + K_LOS[0][2] = -SK_LOS[1]*(P[2][0]*tempVar[8] + P[2][1]*tempVar[7] - P[2][3]*tempVar[6] + P[2][2]*tempVar[5] - P[2][4]*tempVar[4] + P[2][6]*tempVar[3] - P[2][9]*tempVar[1] + P[2][5]*tempVar[0]); + K_LOS[0][3] = -SK_LOS[1]*(P[3][0]*tempVar[8] + P[3][1]*tempVar[7] - P[3][3]*tempVar[6] + P[3][2]*tempVar[5] - P[3][4]*tempVar[4] + P[3][6]*tempVar[3] - P[3][9]*tempVar[1] + P[3][5]*tempVar[0]); + K_LOS[0][4] = -SK_LOS[1]*(P[4][0]*tempVar[8] + P[4][1]*tempVar[7] - P[4][3]*tempVar[6] + P[4][2]*tempVar[5] - P[4][4]*tempVar[4] + P[4][6]*tempVar[3] - P[4][9]*tempVar[1] + P[4][5]*tempVar[0]); + K_LOS[0][5] = -SK_LOS[1]*(P[5][0]*tempVar[8] + P[5][1]*tempVar[7] - P[5][3]*tempVar[6] + P[5][2]*tempVar[5] - P[5][4]*tempVar[4] + P[5][6]*tempVar[3] - P[5][9]*tempVar[1] + P[5][5]*tempVar[0]); + K_LOS[0][6] = -SK_LOS[1]*(P[6][0]*tempVar[8] + P[6][1]*tempVar[7] - P[6][3]*tempVar[6] + P[6][2]*tempVar[5] - P[6][4]*tempVar[4] + P[6][6]*tempVar[3] - P[6][9]*tempVar[1] + P[6][5]*tempVar[0]); + K_LOS[0][7] = -SK_LOS[1]*(P[7][0]*tempVar[8] + P[7][1]*tempVar[7] - P[7][3]*tempVar[6] + P[7][2]*tempVar[5] - P[7][4]*tempVar[4] + P[7][6]*tempVar[3] - P[7][9]*tempVar[1] + P[7][5]*tempVar[0]); + K_LOS[0][8] = -SK_LOS[1]*(P[8][0]*tempVar[8] + P[8][1]*tempVar[7] - P[8][3]*tempVar[6] + P[8][2]*tempVar[5] - P[8][4]*tempVar[4] + P[8][6]*tempVar[3] - P[8][9]*tempVar[1] + P[8][5]*tempVar[0]); + K_LOS[0][9] = -SK_LOS[1]*(P[9][0]*tempVar[8] + P[9][1]*tempVar[7] - P[9][3]*tempVar[6] + P[9][2]*tempVar[5] - P[9][4]*tempVar[4] + P[9][6]*tempVar[3] - P[9][9]*tempVar[1] + P[9][5]*tempVar[0]); + K_LOS[0][10] = -SK_LOS[1]*(P[10][0]*tempVar[8] + P[10][1]*tempVar[7] - P[10][3]*tempVar[6] + P[10][2]*tempVar[5] - P[10][4]*tempVar[4] + P[10][6]*tempVar[3] - P[10][9]*tempVar[1] + P[10][5]*tempVar[0]); + K_LOS[0][11] = -SK_LOS[1]*(P[11][0]*tempVar[8] + P[11][1]*tempVar[7] - P[11][3]*tempVar[6] + P[11][2]*tempVar[5] - P[11][4]*tempVar[4] + P[11][6]*tempVar[3] - P[11][9]*tempVar[1] + P[11][5]*tempVar[0]); + K_LOS[0][12] = -SK_LOS[1]*(P[12][0]*tempVar[8] + P[12][1]*tempVar[7] - P[12][3]*tempVar[6] + P[12][2]*tempVar[5] - P[12][4]*tempVar[4] + P[12][6]*tempVar[3] - P[12][9]*tempVar[1] + P[12][5]*tempVar[0]); + // only height measurements are allowed to modify the Z bias state to improve the stability of the estimate + K_LOS[0][13] = 0.0f;//-SK_LOS[1]*(P[13][0]*tempVar[8] + P[13][1]*tempVar[7] - P[13][3]*tempVar[6] + P[13][2]*tempVar[5] - P[13][4]*tempVar[4] + P[13][6]*tempVar[3] - P[13][9]*tempVar[1] + P[13][5]*tempVar[0]); + if (inhibitWindStates) { + K_LOS[0][14] = -SK_LOS[1]*(P[14][0]*tempVar[8] + P[14][1]*tempVar[7] - P[14][3]*tempVar[6] + P[14][2]*tempVar[5] - P[14][4]*tempVar[4] + P[14][6]*tempVar[3] - P[14][9]*tempVar[1] + P[14][5]*tempVar[0]); + K_LOS[0][15] = -SK_LOS[1]*(P[15][0]*tempVar[8] + P[15][1]*tempVar[7] - P[15][3]*tempVar[6] + P[15][2]*tempVar[5] - P[15][4]*tempVar[4] + P[15][6]*tempVar[3] - P[15][9]*tempVar[1] + P[15][5]*tempVar[0]); + } else { + K_LOS[0][14] = 0.0f; + K_LOS[0][15] = 0.0f; + } + if (inhibitMagStates) { + K_LOS[0][16] = -SK_LOS[1]*(P[16][0]*tempVar[8] + P[16][1]*tempVar[7] - P[16][3]*tempVar[6] + P[16][2]*tempVar[5] - P[16][4]*tempVar[4] + P[16][6]*tempVar[3] - P[16][9]*tempVar[1] + P[16][5]*tempVar[0]); + K_LOS[0][17] = -SK_LOS[1]*(P[17][0]*tempVar[8] + P[17][1]*tempVar[7] - P[17][3]*tempVar[6] + P[17][2]*tempVar[5] - P[17][4]*tempVar[4] + P[17][6]*tempVar[3] - P[17][9]*tempVar[1] + P[17][5]*tempVar[0]); + K_LOS[0][18] = -SK_LOS[1]*(P[18][0]*tempVar[8] + P[18][1]*tempVar[7] - P[18][3]*tempVar[6] + P[18][2]*tempVar[5] - P[18][4]*tempVar[4] + P[18][6]*tempVar[3] - P[18][9]*tempVar[1] + P[18][5]*tempVar[0]); + K_LOS[0][19] = -SK_LOS[1]*(P[19][0]*tempVar[8] + P[19][1]*tempVar[7] - P[19][3]*tempVar[6] + P[19][2]*tempVar[5] - P[19][4]*tempVar[4] + P[19][6]*tempVar[3] - P[19][9]*tempVar[1] + P[19][5]*tempVar[0]); + K_LOS[0][20] = -SK_LOS[1]*(P[20][0]*tempVar[8] + P[20][1]*tempVar[7] - P[20][3]*tempVar[6] + P[20][2]*tempVar[5] - P[20][4]*tempVar[4] + P[20][6]*tempVar[3] - P[20][9]*tempVar[1] + P[20][5]*tempVar[0]); + K_LOS[0][21] = -SK_LOS[1]*(P[21][0]*tempVar[8] + P[21][1]*tempVar[7] - P[21][3]*tempVar[6] + P[21][2]*tempVar[5] - P[21][4]*tempVar[4] + P[21][6]*tempVar[3] - P[21][9]*tempVar[1] + P[21][5]*tempVar[0]); + } else { + for (uint8_t i = 16; i <= 21; i++) { + K_LOS[0][i] = 0.0f; + } + } -// // Calculate Kalman gain -// SKK_LOS[0] = a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3); -// SKK_LOS[1] = a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3); -// SKK_LOS[2] = a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3); -// SKK_LOS[3] = a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3); -// SKK_LOS[4] = a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3); -// SKK_LOS[5] = a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3); -// SKK_LOS[6] = SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10]); -// SKK_LOS[7] = SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8]); -// SKK_LOS[8] = SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9]); -// SKK_LOS[9] = SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12]); -// SKK_LOS[10] = SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5]); -// SKK_LOS[11] = SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4]); -// SKK_LOS[12] = SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6]); -// SKK_LOS[13] = SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11]); -// SKK_LOS[14] = SH_LOS[0]; + // calculate innovation variance and innovation for X axis observation + varInnovOptFlow[0] = 1.0f/SK_LOS[0]; + innovOptFlow[0] = losPred[0] - flowRadXYcomp[0]; -// SK_LOS[0] = 1/(R_LOS + SKK_LOS[8]*(P[0][0]*SKK_LOS[8] + P[1][0]*SKK_LOS[7] + P[2][0]*SKK_LOS[9] - P[3][0]*SKK_LOS[6] - P[9][0]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][0]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][0]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][0]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][0]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) + SKK_LOS[7]*(P[0][1]*SKK_LOS[8] + P[1][1]*SKK_LOS[7] + P[2][1]*SKK_LOS[9] - P[3][1]*SKK_LOS[6] - P[9][1]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][1]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][1]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][1]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][1]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) + SKK_LOS[9]*(P[0][2]*SKK_LOS[8] + P[1][2]*SKK_LOS[7] + P[2][2]*SKK_LOS[9] - P[3][2]*SKK_LOS[6] - P[9][2]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][2]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][2]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][2]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][2]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) - SKK_LOS[6]*(P[0][3]*SKK_LOS[8] + P[1][3]*SKK_LOS[7] + P[2][3]*SKK_LOS[9] - P[3][3]*SKK_LOS[6] - P[9][3]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][3]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][3]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][3]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][3]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) - SH_LOS[2]*SH_LOS[7]*SKK_LOS[14]*(P[0][9]*SKK_LOS[8] + P[1][9]*SKK_LOS[7] + P[2][9]*SKK_LOS[9] - P[3][9]*SKK_LOS[6] - P[9][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][9]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][9]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][9]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) + SH_LOS[2]*SH_LOS[7]*SKK_LOS[14]*(P[0][22]*SKK_LOS[8] + P[1][22]*SKK_LOS[7] + P[2][22]*SKK_LOS[9] - P[3][22]*SKK_LOS[6] - P[9][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][22]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][22]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][22]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) + SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14]*(P[0][4]*SKK_LOS[8] + P[1][4]*SKK_LOS[7] + P[2][4]*SKK_LOS[9] - P[3][4]*SKK_LOS[6] - P[9][4]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][4]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][4]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][4]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) + SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14]*(P[0][5]*SKK_LOS[8] + P[1][5]*SKK_LOS[7] + P[2][5]*SKK_LOS[9] - P[3][5]*SKK_LOS[6] - P[9][5]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][5]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][5]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][5]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) - SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]*(P[0][6]*SKK_LOS[8] + P[1][6]*SKK_LOS[7] + P[2][6]*SKK_LOS[9] - P[3][6]*SKK_LOS[6] - P[9][6]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][6]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][6]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][6]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14])); -// Kfusion[0] = -SK_LOS[0]*(P[0][0]*SKK_LOS[8] + P[0][1]*SKK_LOS[7] - P[0][3]*SKK_LOS[6] + P[0][2]*SKK_LOS[9] - P[0][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[0][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[0][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[0][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[0][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); -// Kfusion[1] = -SK_LOS[0]*(P[1][0]*SKK_LOS[8] + P[1][1]*SKK_LOS[7] - P[1][3]*SKK_LOS[6] + P[1][2]*SKK_LOS[9] - P[1][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[1][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[1][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[1][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[1][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); -// Kfusion[2] = -SK_LOS[0]*(P[2][0]*SKK_LOS[8] + P[2][1]*SKK_LOS[7] - P[2][3]*SKK_LOS[6] + P[2][2]*SKK_LOS[9] - P[2][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[2][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[2][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[2][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[2][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); -// Kfusion[3] = -SK_LOS[0]*(P[3][0]*SKK_LOS[8] + P[3][1]*SKK_LOS[7] - P[3][3]*SKK_LOS[6] + P[3][2]*SKK_LOS[9] - P[3][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[3][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[3][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[3][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[3][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); -// Kfusion[4] = -SK_LOS[0]*(P[4][0]*SKK_LOS[8] + P[4][1]*SKK_LOS[7] - P[4][3]*SKK_LOS[6] + P[4][2]*SKK_LOS[9] - P[4][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[4][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[4][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); -// Kfusion[5] = -SK_LOS[0]*(P[5][0]*SKK_LOS[8] + P[5][1]*SKK_LOS[7] - P[5][3]*SKK_LOS[6] + P[5][2]*SKK_LOS[9] - P[5][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[5][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[5][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[5][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); -// Kfusion[6] = -SK_LOS[0]*(P[6][0]*SKK_LOS[8] + P[6][1]*SKK_LOS[7] - P[6][3]*SKK_LOS[6] + P[6][2]*SKK_LOS[9] - P[6][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[6][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[6][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[6][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); -// Kfusion[7] = -SK_LOS[0]*(P[7][0]*SKK_LOS[8] + P[7][1]*SKK_LOS[7] - P[7][3]*SKK_LOS[6] + P[7][2]*SKK_LOS[9] - P[7][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[7][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[7][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[7][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[7][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); -// Kfusion[8] = -SK_LOS[0]*(P[8][0]*SKK_LOS[8] + P[8][1]*SKK_LOS[7] - P[8][3]*SKK_LOS[6] + P[8][2]*SKK_LOS[9] - P[8][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[8][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[8][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[8][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[8][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); -// Kfusion[9] = -SK_LOS[0]*(P[9][0]*SKK_LOS[8] + P[9][1]*SKK_LOS[7] - P[9][3]*SKK_LOS[6] + P[9][2]*SKK_LOS[9] - P[9][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[9][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[9][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[9][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[9][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); -// Kfusion[10] = -SK_LOS[0]*(P[10][0]*SKK_LOS[8] + P[10][1]*SKK_LOS[7] - P[10][3]*SKK_LOS[6] + P[10][2]*SKK_LOS[9] - P[10][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[10][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[10][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[10][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[10][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); -// Kfusion[11] = -SK_LOS[0]*(P[11][0]*SKK_LOS[8] + P[11][1]*SKK_LOS[7] - P[11][3]*SKK_LOS[6] + P[11][2]*SKK_LOS[9] - P[11][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[11][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[11][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[11][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[11][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); -// Kfusion[12] = -SK_LOS[0]*(P[12][0]*SKK_LOS[8] + P[12][1]*SKK_LOS[7] - P[12][3]*SKK_LOS[6] + P[12][2]*SKK_LOS[9] - P[12][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[12][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[12][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[12][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[12][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); -// Kfusion[13] = 0.0f;//-SK_LOS[0]*(P[13][0]*SKK_LOS[8] + P[13][1]*SKK_LOS[7] - P[13][3]*SKK_LOS[6] + P[13][2]*SKK_LOS[9] - P[13][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[13][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[13][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[13][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[13][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); -// Kfusion[14] = -SK_LOS[0]*(P[14][0]*SKK_LOS[8] + P[14][1]*SKK_LOS[7] - P[14][3]*SKK_LOS[6] + P[14][2]*SKK_LOS[9] - P[14][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[14][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[14][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[14][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[14][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); -// Kfusion[15] = -SK_LOS[0]*(P[15][0]*SKK_LOS[8] + P[15][1]*SKK_LOS[7] - P[15][3]*SKK_LOS[6] + P[15][2]*SKK_LOS[9] - P[15][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[15][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[15][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[15][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[15][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); -// Kfusion[16] = -SK_LOS[0]*(P[16][0]*SKK_LOS[8] + P[16][1]*SKK_LOS[7] - P[16][3]*SKK_LOS[6] + P[16][2]*SKK_LOS[9] - P[16][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[16][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[16][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[16][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[16][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); -// Kfusion[17] = -SK_LOS[0]*(P[17][0]*SKK_LOS[8] + P[17][1]*SKK_LOS[7] - P[17][3]*SKK_LOS[6] + P[17][2]*SKK_LOS[9] - P[17][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[17][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[17][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[17][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[17][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); -// Kfusion[18] = -SK_LOS[0]*(P[18][0]*SKK_LOS[8] + P[18][1]*SKK_LOS[7] - P[18][3]*SKK_LOS[6] + P[18][2]*SKK_LOS[9] - P[18][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[18][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[18][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[18][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[18][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); -// Kfusion[19] = -SK_LOS[0]*(P[19][0]*SKK_LOS[8] + P[19][1]*SKK_LOS[7] - P[19][3]*SKK_LOS[6] + P[19][2]*SKK_LOS[9] - P[19][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[19][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[19][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[19][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[19][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); -// Kfusion[20] = -SK_LOS[0]*(P[20][0]*SKK_LOS[8] + P[20][1]*SKK_LOS[7] - P[20][3]*SKK_LOS[6] + P[20][2]*SKK_LOS[9] - P[20][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[20][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[20][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[20][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[20][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); -// Kfusion[21] = -SK_LOS[0]*(P[21][0]*SKK_LOS[8] + P[21][1]*SKK_LOS[7] - P[21][3]*SKK_LOS[6] + P[21][2]*SKK_LOS[9] - P[21][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[21][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[21][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[21][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[21][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); -// Kfusion[22] = -SK_LOS[0]*(P[22][0]*SKK_LOS[8] + P[22][1]*SKK_LOS[7] - P[22][3]*SKK_LOS[6] + P[22][2]*SKK_LOS[9] - P[22][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[22][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[22][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]); -// varInnovOptFlow[0] = 1.0f/SK_LOS[0]; -// innovOptFlow[0] = losPred[0] - losData[0]; + // calculate intermediate common variables + tempVar[0] = 2.0f*SH_LOS[1]*SK_LOS[8]; + tempVar[1] = (SK_LOS[2] + q0*tempVar[0]); + tempVar[2] = (SK_LOS[5] - q1*tempVar[0]); + tempVar[3] = (SK_LOS[3] + q2*tempVar[0]); + tempVar[4] = (SK_LOS[4] + q3*tempVar[0]); + tempVar[5] = SH_LOS[0]*SK_LOS[8]*(2*q0*q3 + 2*q1*q2); + tempVar[6] = SH_LOS[0]*SK_LOS[8]*(2*q0*q2 - 2*q1*q3); + tempVar[7] = SH_LOS[0]*SH_LOS[1]*SH_LOS[4]; + tempVar[8] = SH_LOS[0]*SK_LOS[7]*SK_LOS[8]; -// // set the observation index to 1 to fuse the y component next time round and reset the commence fusion flag -// obsIndex = 1; -// fuseOptFlowData = false; -// } -// else if (obsIndex == 1) // we are now fusing the Y measurement -// { -// // Calculate observation jacobians -// for (uint8_t i = 0; i < n_states; i++) H_LOS[i] = 0; -// H_LOS[0] = SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5]); -// H_LOS[1] = SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4]); -// H_LOS[2] = SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6]); -// H_LOS[3] = SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11]) - SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1); -// H_LOS[4] = SH_LOS[0]*SH_LOS[3]*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3)); -// H_LOS[5] = -SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)); -// H_LOS[6] = SH_LOS[0]*SH_LOS[3]*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3)); -// H_LOS[9] = -SH_LOS[0]*SH_LOS[1]*SH_LOS[7]; -// H_LOS[22] = SH_LOS[0]*SH_LOS[1]*SH_LOS[7]; + // Calculate observation jacobians for Y LOS rate + for (uint8_t i = 0; i < 23; i++) H_LOS[1][i] = 0; + H_LOS[1][0] = SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) + 2*q0*SH_LOS[1]*SH_LOS[3]; + H_LOS[1][1] = SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q1*SH_LOS[1]*SH_LOS[3]; + H_LOS[1][2] = - SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) - 2*q2*SH_LOS[1]*SH_LOS[3]; + H_LOS[1][3] = SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) + 2*q3*SH_LOS[1]*SH_LOS[3]; + H_LOS[1][4] = SH_LOS[0]*SH_LOS[3]*(sq(q0) + sq(q1) - sq(q2) - sq(q3)); + H_LOS[1][5] = SH_LOS[0]*SH_LOS[3]*(2*q0*q3 + 2*q1*q2); + H_LOS[1][6] = -SH_LOS[0]*SH_LOS[3]*(2*q0*q2 - 2*q1*q3); + H_LOS[1][9] = -tempVar[7]; -// // Calculate Kalman gains -// SK_LOS[1] = 1/(R_LOS + SKK_LOS[12]*(P[0][2]*SKK_LOS[10] + P[1][2]*SKK_LOS[11] + P[2][2]*SKK_LOS[12] - P[3][2]*SKK_LOS[13] - P[9][2]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][2]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][2]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][2]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][2]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) - SKK_LOS[13]*(P[0][3]*SKK_LOS[10] + P[1][3]*SKK_LOS[11] + P[2][3]*SKK_LOS[12] - P[3][3]*SKK_LOS[13] - P[9][3]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][3]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][3]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][3]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][3]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) + SKK_LOS[10]*(P[0][0]*SKK_LOS[10] + P[1][0]*SKK_LOS[11] + P[2][0]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[3]*SKK_LOS[14]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6])) - P[3][0]*(SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[3]*SKK_LOS[14]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][0]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][0]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][0]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][0]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][0]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) + SKK_LOS[11]*(P[0][1]*SKK_LOS[10] + P[1][1]*SKK_LOS[11] + P[2][1]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[3]*SKK_LOS[14]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6])) - P[3][1]*(SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[3]*SKK_LOS[14]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][1]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][1]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][1]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][1]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][1]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) - SH_LOS[1]*SH_LOS[7]*SKK_LOS[14]*(P[0][9]*SKK_LOS[10] + P[1][9]*SKK_LOS[11] + P[2][9]*SKK_LOS[12] - P[3][9]*SKK_LOS[13] - P[9][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][9]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][9]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][9]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) + SH_LOS[1]*SH_LOS[7]*SKK_LOS[14]*(P[0][22]*SKK_LOS[10] + P[1][22]*SKK_LOS[11] + P[2][22]*SKK_LOS[12] - P[3][22]*SKK_LOS[13] - P[9][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][22]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][22]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][22]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) + SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14]*(P[0][4]*SKK_LOS[10] + P[1][4]*SKK_LOS[11] + P[2][4]*SKK_LOS[12] - P[3][4]*SKK_LOS[13] - P[9][4]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][4]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][4]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][4]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) - SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]*(P[0][5]*SKK_LOS[10] + P[1][5]*SKK_LOS[11] + P[2][5]*SKK_LOS[12] - P[3][5]*SKK_LOS[13] - P[9][5]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][5]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][5]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][5]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) + SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]*(P[0][6]*SKK_LOS[10] + P[1][6]*SKK_LOS[11] + P[2][6]*SKK_LOS[12] - P[3][6]*SKK_LOS[13] - P[9][6]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][6]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][6]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][6]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14])); -// Kfusion[0] = SK_LOS[1]*(P[0][0]*SKK_LOS[10] - P[0][3]*SKK_LOS[13] + P[0][1]*SKK_LOS[11] + P[0][2]*SKK_LOS[12] - P[0][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[0][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[0][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[0][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[0][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); -// Kfusion[1] = SK_LOS[1]*(P[1][0]*SKK_LOS[10] - P[1][3]*SKK_LOS[13] + P[1][1]*SKK_LOS[11] + P[1][2]*SKK_LOS[12] - P[1][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[1][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[1][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[1][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[1][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); -// Kfusion[2] = SK_LOS[1]*(P[2][0]*SKK_LOS[10] - P[2][3]*SKK_LOS[13] + P[2][1]*SKK_LOS[11] + P[2][2]*SKK_LOS[12] - P[2][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[2][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[2][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[2][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[2][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); -// Kfusion[3] = SK_LOS[1]*(P[3][0]*SKK_LOS[10] - P[3][3]*SKK_LOS[13] + P[3][1]*SKK_LOS[11] + P[3][2]*SKK_LOS[12] - P[3][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[3][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[3][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[3][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[3][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); -// Kfusion[4] = SK_LOS[1]*(P[4][0]*SKK_LOS[10] - P[4][3]*SKK_LOS[13] + P[4][1]*SKK_LOS[11] + P[4][2]*SKK_LOS[12] - P[4][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[4][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[4][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); -// Kfusion[5] = SK_LOS[1]*(P[5][0]*SKK_LOS[10] - P[5][3]*SKK_LOS[13] + P[5][1]*SKK_LOS[11] + P[5][2]*SKK_LOS[12] - P[5][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[5][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[5][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[5][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); -// Kfusion[6] = SK_LOS[1]*(P[6][0]*SKK_LOS[10] - P[6][3]*SKK_LOS[13] + P[6][1]*SKK_LOS[11] + P[6][2]*SKK_LOS[12] - P[6][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[6][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[6][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[6][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[6][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); -// Kfusion[7] = SK_LOS[1]*(P[7][0]*SKK_LOS[10] - P[7][3]*SKK_LOS[13] + P[7][1]*SKK_LOS[11] + P[7][2]*SKK_LOS[12] - P[7][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[7][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[7][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[7][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[7][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); -// Kfusion[8] = SK_LOS[1]*(P[8][0]*SKK_LOS[10] - P[8][3]*SKK_LOS[13] + P[8][1]*SKK_LOS[11] + P[8][2]*SKK_LOS[12] - P[8][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[8][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[8][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[8][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[8][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); -// Kfusion[9] = SK_LOS[1]*(P[9][0]*SKK_LOS[10] - P[9][3]*SKK_LOS[13] + P[9][1]*SKK_LOS[11] + P[9][2]*SKK_LOS[12] - P[9][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[9][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[9][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[9][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[9][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); -// Kfusion[10] = SK_LOS[1]*(P[10][0]*SKK_LOS[10] - P[10][3]*SKK_LOS[13] + P[10][1]*SKK_LOS[11] + P[10][2]*SKK_LOS[12] - P[10][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[10][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[10][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[10][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[10][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); -// Kfusion[11] = SK_LOS[1]*(P[11][0]*SKK_LOS[10] - P[11][3]*SKK_LOS[13] + P[11][1]*SKK_LOS[11] + P[11][2]*SKK_LOS[12] - P[11][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[11][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[11][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[11][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[11][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); -// Kfusion[12] = SK_LOS[1]*(P[12][0]*SKK_LOS[10] - P[12][3]*SKK_LOS[13] + P[12][1]*SKK_LOS[11] + P[12][2]*SKK_LOS[12] - P[12][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[12][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[12][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[12][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[12][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); -// Kfusion[13] = 0.0f;//SK_LOS[1]*(P[13][0]*SKK_LOS[10] - P[13][3]*SKK_LOS[13] + P[13][1]*SKK_LOS[11] + P[13][2]*SKK_LOS[12] - P[13][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[13][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[13][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[13][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[13][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); -// Kfusion[14] = SK_LOS[1]*(P[14][0]*SKK_LOS[10] - P[14][3]*SKK_LOS[13] + P[14][1]*SKK_LOS[11] + P[14][2]*SKK_LOS[12] - P[14][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[14][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[14][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[14][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[14][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); -// Kfusion[15] = SK_LOS[1]*(P[15][0]*SKK_LOS[10] - P[15][3]*SKK_LOS[13] + P[15][1]*SKK_LOS[11] + P[15][2]*SKK_LOS[12] - P[15][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[15][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[15][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[15][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[15][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); -// Kfusion[16] = SK_LOS[1]*(P[16][0]*SKK_LOS[10] - P[16][3]*SKK_LOS[13] + P[16][1]*SKK_LOS[11] + P[16][2]*SKK_LOS[12] - P[16][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[16][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[16][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[16][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[16][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); -// Kfusion[17] = SK_LOS[1]*(P[17][0]*SKK_LOS[10] - P[17][3]*SKK_LOS[13] + P[17][1]*SKK_LOS[11] + P[17][2]*SKK_LOS[12] - P[17][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[17][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[17][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[17][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[17][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); -// Kfusion[18] = SK_LOS[1]*(P[18][0]*SKK_LOS[10] - P[18][3]*SKK_LOS[13] + P[18][1]*SKK_LOS[11] + P[18][2]*SKK_LOS[12] - P[18][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[18][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[18][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[18][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[18][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); -// Kfusion[19] = SK_LOS[1]*(P[19][0]*SKK_LOS[10] - P[19][3]*SKK_LOS[13] + P[19][1]*SKK_LOS[11] + P[19][2]*SKK_LOS[12] - P[19][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[19][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[19][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[19][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[19][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); -// Kfusion[20] = SK_LOS[1]*(P[20][0]*SKK_LOS[10] - P[20][3]*SKK_LOS[13] + P[20][1]*SKK_LOS[11] + P[20][2]*SKK_LOS[12] - P[20][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[20][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[20][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[20][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[20][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); -// Kfusion[21] = SK_LOS[1]*(P[21][0]*SKK_LOS[10] - P[21][3]*SKK_LOS[13] + P[21][1]*SKK_LOS[11] + P[21][2]*SKK_LOS[12] - P[21][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[21][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[21][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[21][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[21][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); -// Kfusion[22] = SK_LOS[1]*(P[22][0]*SKK_LOS[10] - P[22][3]*SKK_LOS[13] + P[22][1]*SKK_LOS[11] + P[22][2]*SKK_LOS[12] - P[22][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[22][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[22][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]); -// varInnovOptFlow[1] = 1.0f/SK_LOS[1]; -// innovOptFlow[1] = losPred[1] - losData[1]; + // Calculate Kalman gains for Y LOS rate + K_LOS[1][0] = SK_LOS[0]*(P[0][0]*tempVar[1] + P[0][1]*tempVar[2] - P[0][2]*tempVar[3] + P[0][3]*tempVar[4] + P[0][5]*tempVar[5] - P[0][6]*tempVar[6] - P[0][9]*tempVar[7] + P[0][4]*tempVar[8]); + K_LOS[1][1] = SK_LOS[0]*(P[1][0]*tempVar[1] + P[1][1]*tempVar[2] - P[1][2]*tempVar[3] + P[1][3]*tempVar[4] + P[1][5]*tempVar[5] - P[1][6]*tempVar[6] - P[1][9]*tempVar[7] + P[1][4]*tempVar[8]); + K_LOS[1][2] = SK_LOS[0]*(P[2][0]*tempVar[1] + P[2][1]*tempVar[2] - P[2][2]*tempVar[3] + P[2][3]*tempVar[4] + P[2][5]*tempVar[5] - P[2][6]*tempVar[6] - P[2][9]*tempVar[7] + P[2][4]*tempVar[8]); + K_LOS[1][3] = SK_LOS[0]*(P[3][0]*tempVar[1] + P[3][1]*tempVar[2] - P[3][2]*tempVar[3] + P[3][3]*tempVar[4] + P[3][5]*tempVar[5] - P[3][6]*tempVar[6] - P[3][9]*tempVar[7] + P[3][4]*tempVar[8]); + K_LOS[1][4] = SK_LOS[0]*(P[4][0]*tempVar[1] + P[4][1]*tempVar[2] - P[4][2]*tempVar[3] + P[4][3]*tempVar[4] + P[4][5]*tempVar[5] - P[4][6]*tempVar[6] - P[4][9]*tempVar[7] + P[4][4]*tempVar[8]); + K_LOS[1][5] = SK_LOS[0]*(P[5][0]*tempVar[1] + P[5][1]*tempVar[2] - P[5][2]*tempVar[3] + P[5][3]*tempVar[4] + P[5][5]*tempVar[5] - P[5][6]*tempVar[6] - P[5][9]*tempVar[7] + P[5][4]*tempVar[8]); + K_LOS[1][6] = SK_LOS[0]*(P[6][0]*tempVar[1] + P[6][1]*tempVar[2] - P[6][2]*tempVar[3] + P[6][3]*tempVar[4] + P[6][5]*tempVar[5] - P[6][6]*tempVar[6] - P[6][9]*tempVar[7] + P[6][4]*tempVar[8]); + K_LOS[1][7] = SK_LOS[0]*(P[7][0]*tempVar[1] + P[7][1]*tempVar[2] - P[7][2]*tempVar[3] + P[7][3]*tempVar[4] + P[7][5]*tempVar[5] - P[7][6]*tempVar[6] - P[7][9]*tempVar[7] + P[7][4]*tempVar[8]); + K_LOS[1][8] = SK_LOS[0]*(P[8][0]*tempVar[1] + P[8][1]*tempVar[2] - P[8][2]*tempVar[3] + P[8][3]*tempVar[4] + P[8][5]*tempVar[5] - P[8][6]*tempVar[6] - P[8][9]*tempVar[7] + P[8][4]*tempVar[8]); + K_LOS[1][9] = SK_LOS[0]*(P[9][0]*tempVar[1] + P[9][1]*tempVar[2] - P[9][2]*tempVar[3] + P[9][3]*tempVar[4] + P[9][5]*tempVar[5] - P[9][6]*tempVar[6] - P[9][9]*tempVar[7] + P[9][4]*tempVar[8]); + K_LOS[1][10] = SK_LOS[0]*(P[10][0]*tempVar[1] + P[10][1]*tempVar[2] - P[10][2]*tempVar[3] + P[10][3]*tempVar[4] + P[10][5]*tempVar[5] - P[10][6]*tempVar[6] - P[10][9]*tempVar[7] + P[10][4]*tempVar[8]); + K_LOS[1][11] = SK_LOS[0]*(P[11][0]*tempVar[1] + P[11][1]*tempVar[2] - P[11][2]*tempVar[3] + P[11][3]*tempVar[4] + P[11][5]*tempVar[5] - P[11][6]*tempVar[6] - P[11][9]*tempVar[7] + P[11][4]*tempVar[8]); + K_LOS[1][12] = SK_LOS[0]*(P[12][0]*tempVar[1] + P[12][1]*tempVar[2] - P[12][2]*tempVar[3] + P[12][3]*tempVar[4] + P[12][5]*tempVar[5] - P[12][6]*tempVar[6] - P[12][9]*tempVar[7] + P[12][4]*tempVar[8]); + // only height measurements are allowed to modify the Z bias state to improve the stability of the estimate + K_LOS[1][13] = 0.0f;//SK_LOS[0]*(P[13][0]*tempVar[1] + P[13][1]*tempVar[2] - P[13][2]*tempVar[3] + P[13][3]*tempVar[4] + P[13][5]*tempVar[5] - P[13][6]*tempVar[6] - P[13][9]*tempVar[7] + P[13][4]*tempVar[8]); + if (inhibitWindStates) { + K_LOS[1][14] = SK_LOS[0]*(P[14][0]*tempVar[1] + P[14][1]*tempVar[2] - P[14][2]*tempVar[3] + P[14][3]*tempVar[4] + P[14][5]*tempVar[5] - P[14][6]*tempVar[6] - P[14][9]*tempVar[7] + P[14][4]*tempVar[8]); + K_LOS[1][15] = SK_LOS[0]*(P[15][0]*tempVar[1] + P[15][1]*tempVar[2] - P[15][2]*tempVar[3] + P[15][3]*tempVar[4] + P[15][5]*tempVar[5] - P[15][6]*tempVar[6] - P[15][9]*tempVar[7] + P[15][4]*tempVar[8]); + } else { + K_LOS[1][14] = 0.0f; + K_LOS[1][15] = 0.0f; + } + if (inhibitMagStates) { + K_LOS[1][16] = SK_LOS[0]*(P[16][0]*tempVar[1] + P[16][1]*tempVar[2] - P[16][2]*tempVar[3] + P[16][3]*tempVar[4] + P[16][5]*tempVar[5] - P[16][6]*tempVar[6] - P[16][9]*tempVar[7] + P[16][4]*tempVar[8]); + K_LOS[1][17] = SK_LOS[0]*(P[17][0]*tempVar[1] + P[17][1]*tempVar[2] - P[17][2]*tempVar[3] + P[17][3]*tempVar[4] + P[17][5]*tempVar[5] - P[17][6]*tempVar[6] - P[17][9]*tempVar[7] + P[17][4]*tempVar[8]); + K_LOS[1][18] = SK_LOS[0]*(P[18][0]*tempVar[1] + P[18][1]*tempVar[2] - P[18][2]*tempVar[3] + P[18][3]*tempVar[4] + P[18][5]*tempVar[5] - P[18][6]*tempVar[6] - P[18][9]*tempVar[7] + P[18][4]*tempVar[8]); + K_LOS[1][19] = SK_LOS[0]*(P[19][0]*tempVar[1] + P[19][1]*tempVar[2] - P[19][2]*tempVar[3] + P[19][3]*tempVar[4] + P[19][5]*tempVar[5] - P[19][6]*tempVar[6] - P[19][9]*tempVar[7] + P[19][4]*tempVar[8]); + K_LOS[1][20] = SK_LOS[0]*(P[20][0]*tempVar[1] + P[20][1]*tempVar[2] - P[20][2]*tempVar[3] + P[20][3]*tempVar[4] + P[20][5]*tempVar[5] - P[20][6]*tempVar[6] - P[20][9]*tempVar[7] + P[20][4]*tempVar[8]); + K_LOS[1][21] = SK_LOS[0]*(P[21][0]*tempVar[1] + P[21][1]*tempVar[2] - P[21][2]*tempVar[3] + P[21][3]*tempVar[4] + P[21][5]*tempVar[5] - P[21][6]*tempVar[6] - P[21][9]*tempVar[7] + P[21][4]*tempVar[8]); + } else { + for (uint8_t i = 16; i <= 21; i++) { + K_LOS[1][i] = 0.0f; + } + } -// // reset the observation index -// obsIndex = 0; -// fuseOptFlowData = false; -// } + // calculate variance and innovation for Y observation + varInnovOptFlow[1] = 1.0f/SK_LOS[1]; + innovOptFlow[1] = losPred[1] - flowRadXYcomp[1]; -// // Check the innovation for consistency and don't fuse if > 3Sigma -// if ((innovOptFlow[obsIndex]*innovOptFlow[obsIndex]/varInnovOptFlow[obsIndex]) < 9.0f) -// { -// // correct the state vector -// for (uint8_t j = 0; j < n_states; j++) -// { -// states[j] = states[j] - Kfusion[j] * innovOptFlow[obsIndex]; -// } -// // normalise the quaternion states -// float quatMag = sqrt(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]); -// if (quatMag > 1e-12f) -// { -// for (uint8_t j= 0; j<=3; j++) -// { -// float quatMagInv = 1.0f/quatMag; -// states[j] = states[j] * quatMagInv; -// } -// } -// // correct the covariance P = (I - K*H)*P -// // take advantage of the empty columns in KH to reduce the -// // number of operations -// for (uint8_t i = 0; i < n_states; i++) -// { -// for (uint8_t j = 0; j <= 6; j++) -// { -// KH[i][j] = Kfusion[i] * H_LOS[j]; -// } -// for (uint8_t j = 7; j <= 8; j++) -// { -// KH[i][j] = 0.0f; -// } -// KH[i][9] = Kfusion[i] * H_LOS[9]; -// for (uint8_t j = 10; j <= 21; j++) -// { -// KH[i][j] = 0.0f; -// } -// KH[i][22] = Kfusion[i] * H_LOS[22]; -// } -// for (uint8_t i = 0; i < n_states; i++) -// { -// for (uint8_t j = 0; j < n_states; j++) -// { -// KHP[i][j] = 0.0f; -// for (uint8_t k = 0; k <= 6; k++) -// { -// KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j]; -// } -// KHP[i][j] = KHP[i][j] + KH[i][9] * P[9][j]; -// KHP[i][j] = KHP[i][j] + KH[i][22] * P[2][j]; -// } -// } -// } -// for (uint8_t i = 0; i < n_states; i++) -// { -// for (uint8_t j = 0; j < n_states; j++) -// { -// P[i][j] = P[i][j] - KHP[i][j]; -// } -// } -// ForceSymmetry(); -// ConstrainVariances(); -// } + } + + // loop through the X and Y observations and fuse them sequentially + for (uint8_t obsIndex = 0; obsIndex < 2; obsIndex++) { + // Check the innovation for consistency and don't fuse if > 5Sigma + if ((innovOptFlow[obsIndex]*innovOptFlow[obsIndex]/varInnovOptFlow[obsIndex]) < 25.0f) { + // correct the state vector + for (uint8_t j = 0; j < n_states; j++) + { + states[j] = states[j] - K_LOS[obsIndex][j] * innovOptFlow[obsIndex]; + } + // normalise the quaternion states + float quatMag = sqrt(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]); + if (quatMag > 1e-12f) + { + for (uint8_t j= 0; j<=3; j++) + { + float quatMagInv = 1.0f/quatMag; + states[j] = states[j] * quatMagInv; + } + } + // correct the covariance P = (I - K*H)*P + // take advantage of the empty columns in KH to reduce the + // number of operations + for (uint8_t i = 0; i < n_states; i++) + { + for (uint8_t j = 0; j <= 6; j++) + { + KH[i][j] = K_LOS[obsIndex][i] * H_LOS[obsIndex][j]; + } + for (uint8_t j = 7; j <= 8; j++) + { + KH[i][j] = 0.0f; + } + KH[i][9] = K_LOS[obsIndex][i] * H_LOS[obsIndex][9]; + for (uint8_t j = 10; j <= 21; j++) + { + KH[i][j] = 0.0f; + } + } + for (uint8_t i = 0; i < n_states; i++) + { + for (uint8_t j = 0; j < n_states; j++) + { + KHP[i][j] = 0.0f; + for (uint8_t k = 0; k <= 6; k++) + { + KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j]; + } + KHP[i][j] = KHP[i][j] + KH[i][9] * P[9][j]; + } + } + for (uint8_t i = 0; i < n_states; i++) + { + for (uint8_t j = 0; j < n_states; j++) + { + P[i][j] = P[i][j] - KHP[i][j]; + } + } + } + } + ForceSymmetry(); + ConstrainVariances(); + } } /* @@ -2192,27 +2016,29 @@ Estimation of optical flow sensor focal length scale factor and terrain height u This fiter requires optical flow rates that are not motion compensated Range to ground measurement is assumed to be via a narrow beam type sensor - eg laser */ -void AttPosEKF::GroundEKF() +void AttPosEKF::OpticalFlowEKF() { // propagate ground position state noise each time this is called using the difference in position since the last observations and an RMS gradient assumption // limit distance to prevent intialisation afer bad gps causing bad numerical conditioning if (!inhibitGndState) { float distanceTravelledSq; - distanceTravelledSq = sq(statesAtRngTime[7] - prevPosN) + sq(statesAtRngTime[8] - prevPosE); - prevPosN = statesAtRngTime[7]; - prevPosE = statesAtRngTime[8]; + if (fuseRngData) { + distanceTravelledSq = sq(statesAtRngTime[7] - prevPosN) + sq(statesAtRngTime[8] - prevPosE); + prevPosN = statesAtRngTime[7]; + prevPosE = statesAtRngTime[8]; + } else if (fuseOptFlowData) { + distanceTravelledSq = sq(statesAtFlowTime[7] - prevPosN) + sq(statesAtFlowTime[8] - prevPosE); + prevPosN = statesAtFlowTime[7]; + prevPosE = statesAtFlowTime[8]; + } else { + return; + } distanceTravelledSq = min(distanceTravelledSq, 100.0f); Popt[1][1] += (distanceTravelledSq * sq(gndHgtSigma)); } - // we aren't using optical flow measurements in this hacked implementation so set the covariances for this state to zero to avoid numerical problems - Popt[0][0] = 0.0f; - Popt[0][1] = 0.0f; - Popt[1][0] = 0.0f; - // Fuse range finder data - // Need to check that our range finder tilt angle is less than 30 degrees - float cosRngTilt = - Tbn.z.x * sinf(rngFinderPitch) + Tbn.z.z * cosf(rngFinderPitch); - if (useRangeFinder && fuseRngData && cosRngTilt > 0.87f) { + // fuse range finder data + if (fuseRngData) { float range; // range from camera to centre of image float q0; // quaternion at optical flow measurement time float q1; // quaternion at optical flow measurement time @@ -2266,10 +2092,7 @@ void AttPosEKF::GroundEKF() flowStates[i] -= K_RNG[i] * innovRng; } // constrain the states - - // constrain focal length to 0.1 to 10 mm flowStates[0] = ConstrainFloat(flowStates[0], 0.1f, 10.0f); - // constrain altitude flowStates[1] = maxf(flowStates[1], statesAtRngTime[9] + minFlowRng); // correct the covariance matrix @@ -2285,6 +2108,132 @@ void AttPosEKF::GroundEKF() Popt[1][0] = Popt[0][1]; } } + + if (fuseOptFlowData) { + Vector3f vel; // velocity of sensor relative to ground in NED axes + Vector3f relVelSensor; // velocity of sensor relative to ground in sensor axes + float losPred[2]; // predicted optical flow angular rate measurements + float range; // range from camera to centre of image + float q0; // quaternion at optical flow measurement time + float q1; // quaternion at optical flow measurement time + float q2; // quaternion at optical flow measurement time + float q3; // quaternion at optical flow measurement time + float HP[2]; + float SH_OPT[6]; + float SK_OPT[3]; + float K_OPT[2][2]; + float H_OPT[2][2]; + float nextPopt[2][2]; + + // propagate scale factor state noise + if (!inhibitScaleState) { + Popt[0][0] += 1e-8f; + } else { + Popt[0][0] = 0.0f; + } + + // Copy required states to local variable names + q0 = statesAtFlowTime[0]; + q1 = statesAtFlowTime[1]; + q2 = statesAtFlowTime[2]; + q3 = statesAtFlowTime[3]; + vel.x = statesAtFlowTime[4]; + vel.y = statesAtFlowTime[5]; + vel.z = statesAtFlowTime[6]; + + // constrain terrain height to be below the vehicle + flowStates[1] = maxf(flowStates[1], statesAtFlowTime[9] + minFlowRng); + + // estimate range to centre of image + range = (flowStates[1] - statesAtFlowTime[9]) / Tnb_flow.z.z; + + // calculate relative velocity in sensor frame + relVelSensor = Tnb_flow * vel; + + // divide velocity by range, subtract body rates and apply scale factor to + // get predicted sensed angular optical rates relative to X and Y sensor axes + losPred[0] = flowStates[0]*( relVelSensor.y / range) - omegaAcrossFlowTime[0]; + losPred[1] = flowStates[0]*(-relVelSensor.x / range) - omegaAcrossFlowTime[1]; + + // calculate innovations + auxFlowObsInnov[0] = losPred[0] - flowRadXY[0]; + auxFlowObsInnov[1] = losPred[1] - flowRadXY[1]; + + // calculate Kalman gains + SH_OPT[0] = sq(q0) - sq(q1) - sq(q2) + sq(q3); + SH_OPT[1] = vel.x*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + vel.y*(2*q0*q3 + 2*q1*q2) - vel.z*(2*q0*q2 - 2*q1*q3); + SH_OPT[2] = vel.y*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) - vel.x*(2*q0*q3 - 2*q1*q2) + vel.z*(2*q0*q1 + 2*q2*q3); + SH_OPT[3] = statesAtFlowTime[9] - flowStates[1]; + SH_OPT[4] = 1.0f/sq(SH_OPT[3]); + SH_OPT[5] = 1.0f/SH_OPT[3]; + float SH015 = SH_OPT[0]*SH_OPT[1]*SH_OPT[5]; + float SH025 = SH_OPT[0]*SH_OPT[2]*SH_OPT[5]; + float SH014 = SH_OPT[0]*SH_OPT[1]*SH_OPT[4]; + float SH024 = SH_OPT[0]*SH_OPT[2]*SH_OPT[4]; + SK_OPT[0] = 1.0f/(R_LOS + SH015*(Popt[0][0]*SH015 + Popt[1][0]*flowStates[0]*SH014) + flowStates[0]*SH014*(Popt[0][1]*SH015 + Popt[1][1]*flowStates[0]*SH014)); + SK_OPT[1] = 1.0f/(R_LOS + SH025*(Popt[0][0]*SH025 + Popt[1][0]*flowStates[0]*SH024) + flowStates[0]*SH024*(Popt[0][1]*SH025 + Popt[1][1]*flowStates[0]*SH024)); + SK_OPT[2] = SH_OPT[0]; + if (!inhibitScaleState) { + K_OPT[0][0] = -SK_OPT[1]*(Popt[0][0]*SH_OPT[2]*SH_OPT[5]*SK_OPT[2] + Popt[0][1]*flowStates[0]*SH_OPT[2]*SH_OPT[4]*SK_OPT[2]); + K_OPT[0][1] = SK_OPT[0]*(Popt[0][0]*SH_OPT[1]*SH_OPT[5]*SK_OPT[2] + Popt[0][1]*flowStates[0]*SH_OPT[1]*SH_OPT[4]*SK_OPT[2]); + } else { + K_OPT[0][0] = 0.0f; + K_OPT[0][1] = 0.0f; + } + if (!inhibitGndState) { + K_OPT[1][0] = -SK_OPT[1]*(Popt[1][0]*SH_OPT[2]*SH_OPT[5]*SK_OPT[2] + Popt[1][1]*flowStates[0]*SH_OPT[2]*SH_OPT[4]*SK_OPT[2]); + K_OPT[1][1] = SK_OPT[0]*(Popt[1][0]*SH_OPT[1]*SH_OPT[5]*SK_OPT[2] + Popt[1][1]*flowStates[0]*SH_OPT[1]*SH_OPT[4]*SK_OPT[2]); + } else { + K_OPT[1][0] = 0.0f; + K_OPT[1][1] = 0.0f; + } + + // calculate innovation variances + auxFlowObsInnovVar[0] = 1.0f/SK_OPT[1]; + auxFlowObsInnovVar[1] = 1.0f/SK_OPT[0]; + + // calculate observations jacobians + H_OPT[0][0] = -SH025; + H_OPT[0][1] = -flowStates[0]*SH024; + H_OPT[1][0] = SH015; + H_OPT[1][1] = flowStates[0]*SH014; + + // Check the innovation for consistency and don't fuse if > threshold + for (uint8_t obsIndex = 0; obsIndex < 2; obsIndex++) { + + // calculate the innovation consistency test ratio + auxFlowTestRatio[obsIndex] = sq(auxFlowObsInnov[obsIndex]) / (sq(auxFlowInnovGate) * auxFlowObsInnovVar[obsIndex]); + if (auxFlowTestRatio[obsIndex] < 1.0f) { + // correct the state + for (uint8_t i = 0; i < 2 ; i++) { + flowStates[i] -= K_OPT[i][obsIndex] * auxFlowObsInnov[obsIndex]; + } + // constrain the states + flowStates[0] = ConstrainFloat(flowStates[0], 0.1f, 10.0f); + flowStates[1] = maxf(flowStates[1], statesAtFlowTime[9] + minFlowRng); + + // correct the covariance matrix + for (uint8_t i = 0; i < 2 ; i++) { + HP[i] = 0.0f; + for (uint8_t j = 0; j < 2 ; j++) { + HP[i] += H_OPT[obsIndex][j] * P[j][i]; + } + } + for (uint8_t i = 0; i < 2 ; i++) { + for (uint8_t j = 0; j < 2 ; j++) { + nextPopt[i][j] = P[i][j] - K_OPT[i][obsIndex] * HP[j]; + } + } + + // prevent the state variances from becoming negative and maintain symmetry + Popt[0][0] = maxf(nextPopt[0][0],0.0f); + Popt[1][1] = maxf(nextPopt[1][1],0.0f); + Popt[0][1] = 0.5f * (nextPopt[0][1] + nextPopt[1][0]); + Popt[1][0] = Popt[0][1]; + } + } + } + } void AttPosEKF::zeroCols(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last) @@ -2328,6 +2277,9 @@ void AttPosEKF::StoreStates(uint64_t timestamp_ms) { for (unsigned i=0; i 0) + { + for (unsigned i=0; i < 3; i++) { + omegaForFusion[i] += storedOmega[i][storeIndexLocal]; + } + sumIndex += 1; + } + } + if (sumIndex >= 1) { + for (unsigned i=0; i < 3; i++) { + omegaForFusion[i] = omegaForFusion[i] / float(sumIndex); + } + } else { + omegaForFusion[0] = angRate.x; + omegaForFusion[1] = angRate.y; + omegaForFusion[2] = angRate.z; + } +} + void AttPosEKF::quat2Tnb(Mat3f &Tnb, const float (&quat)[4]) { // Calculate the nav to body cosine matrix @@ -2502,6 +2481,7 @@ void AttPosEKF::calcLLH(float posNED[3], double &lat, double &lon, float &hgt, d void AttPosEKF::OnGroundCheck() { onGround = (((sq(velNED[0]) + sq(velNED[1]) + sq(velNED[2])) < 4.0f) && (VtasMeas < 8.0f)); + if (staticMode) { staticMode = (!refSet || (GPSstatus < GPS_FIX_3D)); } @@ -2517,12 +2497,25 @@ void AttPosEKF::OnGroundCheck() } else { inhibitMagStates = false; } - // don't update terrain offset state if on ground - if (onGround) { + // don't update terrain offset state if there is no range finder and flying at low velocity or without GPS + if ((onGround || !useGPS) && !useRangeFinder) { inhibitGndState = true; } else { inhibitGndState = false; } + // don't update terrain offset state if there is no range finder and flying at low velocity, or without GPS, as it is poorly observable + if ((onGround || (globalTimeStamp_ms - lastFixTime_ms) > 1000) && !useRangeFinder) { + inhibitGndState = true; + } else { + inhibitGndState = false; + } + // Don't update focal length offset state if there is no range finder or optical flow sensor + // we need both sensors to do this estimation + if (!useRangeFinder || !useOpticalFlow) { + inhibitScaleState = true; + } else { + inhibitScaleState = false; + } } void AttPosEKF::calcEarthRateNED(Vector3f &omega, float latitude) @@ -2558,7 +2551,9 @@ void AttPosEKF::CovarianceInit() P[19][19] = sq(0.02f); P[20][20] = P[19][19]; P[21][21] = P[19][19]; - P[22][22] = sq(0.5f); + + fScaleFactorVar = 0.001f; // focal length scale factor variance + Popt[0][0] = 0.001f; } float AttPosEKF::ConstrainFloat(float val, float min_val, float max_val) @@ -2596,7 +2591,6 @@ void AttPosEKF::ConstrainVariances() // 14-15: Wind Vector - m/sec (North,East) // 16-18: Earth Magnetic Field Vector - gauss (North, East, Down) // 19-21: Body Magnetic Field Vector - gauss (X,Y,Z) - // 22: Terrain offset - m // Constrain quaternion variances for (unsigned i = 0; i <= 3; i++) { @@ -2636,8 +2630,6 @@ void AttPosEKF::ConstrainVariances() P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0f); } - // Constrain terrain offset variance - P[22][22] = ConstrainFloat(P[22][22], 0.0f, 10000.0f); } void AttPosEKF::ConstrainStates() @@ -2655,7 +2647,6 @@ void AttPosEKF::ConstrainStates() // 14-15: Wind Vector - m/sec (North,East) // 16-18: Earth Magnetic Field Vector - gauss (North, East, Down) // 19-21: Body Magnetic Field Vector - gauss (X,Y,Z) - // 22: Terrain offset - m // Constrain quaternion for (unsigned i = 0; i <= 3; i++) { @@ -2673,7 +2664,8 @@ void AttPosEKF::ConstrainStates() } // Constrain altitude - states[9] = ConstrainFloat(states[9], -4.0e4f, 1.0e4f); + // NOT FOR FLIGHT : Upper value of 0.0 is a temporary fix to get around lack of range finder data during development testing + states[9] = ConstrainFloat(states[9], -4.0e4f, 4.0e4f); // Angle bias limit - set to 8 degrees / sec for (unsigned i = 10; i <= 12; i++) { @@ -2699,9 +2691,6 @@ void AttPosEKF::ConstrainStates() states[i] = ConstrainFloat(states[i], -0.5f, 0.5f); } - // Constrain terrain offset - states[22] = ConstrainFloat(states[22], -1000.0f, 1000.0f); - } void AttPosEKF::ForceSymmetry() @@ -3162,12 +3151,14 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination) states[19] = magBias.x; // Magnetic Field Bias X states[20] = magBias.y; // Magnetic Field Bias Y states[21] = magBias.z; // Magnetic Field Bias Z - states[22] = 0.0f; // terrain height ResetVelocity(); ResetPosition(); ResetHeight(); + // initialise focal length scale factor estimator states + flowStates[0] = 1.0f; + statesInitialised = true; // initialise the covariance matrix @@ -3223,6 +3214,9 @@ void AttPosEKF::ZeroVariables() states[i] = 0.0f; // state matrix } + // initialise the variables for the focal length scale factor to unity + flowStates[0] = 1.0f; + correctedDelAng.zero(); summedDelAng.zero(); summedDelVel.zero(); @@ -3230,12 +3224,9 @@ void AttPosEKF::ZeroVariables() dVelIMU.zero(); lastGyroOffset.zero(); - windSpdFiltNorth = 0.0f; - windSpdFiltEast = 0.0f; - // setting the altitude to zero will give us a higher - // gain to adjust faster in the first step - windSpdFiltAltitude = 0.0f; - windSpdFiltClimb = 0.0f; + // initialise states used by OpticalFlowEKF + flowStates[0] = 1.0f; + flowStates[1] = 0.0f; for (unsigned i = 0; i < data_buffer_size; i++) { diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.h b/src/modules/ekf_att_pos_estimator/estimator_22states.h similarity index 91% rename from src/modules/ekf_att_pos_estimator/estimator_23states.h rename to src/modules/ekf_att_pos_estimator/estimator_22states.h index a607955a89..b1d71bd748 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.h +++ b/src/modules/ekf_att_pos_estimator/estimator_22states.h @@ -2,7 +2,7 @@ #include "estimator_utilities.h" -const unsigned int n_states = 23; +const unsigned int n_states = 22; const unsigned int data_buffer_size = 50; class AttPosEKF { @@ -29,10 +29,6 @@ public: float covDelAngMax; // maximum delta angle between covariance predictions float rngFinderPitch; // pitch angle of laser range finder in radians. Zero is aligned with the Z body axis. Positive is RH rotation about Y body axis. - float a1; // optical flow sensor misalgnment angle about X axis (rad) - float a2; // optical flow sensor misalgnment angle about Y axis (rad) - float a3; // optical flow sensor misalgnment angle about Z axis (rad) - float yawVarScale; float windVelSigma; float dAngBiasSigma; @@ -59,9 +55,6 @@ public: covDelAngMax = 0.02f; // maximum delta angle between covariance predictions rngFinderPitch = 0.0f; // pitch angle of laser range finder in radians. Zero is aligned with the Z body axis. Positive is RH rotation about Y body axis. EAS2TAS = 1.0f; - a1 = 0.0f; // optical flow sensor misalgnment angle about X axis (rad) - a2 = 0.0f; // optical flow sensor misalgnment angle about Y axis (rad) - a3 = 0.0f; // optical flow sensor misalgnment angle about Z axis (rad) yawVarScale = 1.0f; windVelSigma = 0.1f; @@ -69,7 +62,6 @@ public: dVelBiasSigma = 1e-4f; magEarthSigma = 3.0e-4f; magBodySigma = 3.0e-4f; - gndHgtSigma = 0.02f; // assume 2% terrain gradient 1-sigma vneSigma = 0.2f; vdSigma = 0.3f; @@ -82,12 +74,13 @@ public: accelProcessNoise = 0.5f; gndHgtSigma = 0.1f; // terrain gradient 1-sigma - R_LOS = 0.03f; // optical flow measurement noise variance (rad/sec)^2 + R_LOS = 0.3f; // optical flow measurement noise variance (rad/sec)^2 flowInnovGate = 3.0f; // number of standard deviations applied to the optical flow innovation consistency check auxFlowInnovGate = 10.0f; // number of standard deviations applied to the optical flow innovation consistency check used by the auxiliary filter - rngInnovGate = 10.0f; // number of standard deviations applied to the rnage finder innovation consistency check - minFlowRng = 0.01f; //minimum range between ground and flow sensor - moCompR_LOS = 0.2; // scaler from sensor gyro rate to uncertainty in LOS rate + rngInnovGate = 5.0f; // number of standard deviations applied to the range finder innovation consistency check + minFlowRng = 0.3f; //minimum range between ground and flow sensor + moCompR_LOS = 0.0; // scaler from sensor gyro rate to uncertainty in LOS rate + } struct mag_state_struct { @@ -134,6 +127,7 @@ public: float statesAtVtasMeasTime[n_states]; // filter states at the effective measurement time float statesAtRngTime[n_states]; // filter states at the effective measurement time float statesAtFlowTime[n_states]; // States at the effective optical flow measurement time + float omegaAcrossFlowTime[3]; // angular rates at the effective optical flow measurement time Vector3f correctedDelAng; // delta angles about the xyz body axes corrected for errors (rad) Vector3f correctedDelVel; // delta velocities along the XYZ body axes corrected for errors (m/s) @@ -145,7 +139,7 @@ public: Vector3f lastGyroOffset; // Last gyro offset Vector3f delAngTotal; - Mat3f Tbn; // transformation matrix from body to NED coordinates + Mat3f Tbn; // transformation matrix from body to NED coordinatesFuseOptFlow Mat3f Tnb; // transformation amtrix from NED to body coordinates Vector3f accel; // acceleration vector in XYZ body axes measured by the IMU (m/s^2) @@ -157,10 +151,6 @@ public: float dtVelPosFilt; // average time between position / velocity fusion steps float dtHgtFilt; // average time between height measurement updates float dtGpsFilt; // average time between gps measurement updates - float windSpdFiltNorth; // average wind speed north component - float windSpdFiltEast; // average wind speed east component - float windSpdFiltAltitude; // the last altitude used to filter wind speed - float windSpdFiltClimb; // filtered climb rate uint8_t fusionModeGPS; // 0 = GPS outputs 3D velocity, 1 = GPS outputs 2D velocity, 2 = GPS outputs no velocity float innovVelPos[6]; // innovation output float varInnovVelPos[6]; // innovation variance output @@ -175,7 +165,8 @@ public: float innovMag[3]; // innovation output float varInnovMag[3]; // innovation variance output Vector3f magData; // magnetometer flux radings in X,Y,Z body axes - float losData[2]; // optical flow LOS rate measurements (rad/sec) + float flowRadXYcomp[2]; // motion compensated optical flow angular rates(rad/sec) + float flowRadXY[2]; // raw (non motion compensated) optical flow angular rates (rad/sec) float innovVtas; // innovation output float innovRng; ///< Range finder innovation float innovOptFlow[2]; // optical flow LOS innovations (rad/sec) @@ -190,6 +181,8 @@ public: bool refSet; ///< flag to indicate if the reference position has been set Vector3f magBias; // states representing magnetometer bias vector in XYZ body axes unsigned covSkipCount; // Number of state prediction frames (IMU daya updates to skip before doing the covariance prediction + uint32_t lastFixTime_ms; // Number of msec since last GPS Fix that was used + uint32_t globalTimeStamp_ms; // time in msec of current prediction cycle // GPS input data variables double gpsLat; @@ -217,6 +210,7 @@ public: bool onGround; ///< boolean true when the flight vehicle is on the ground (not flying) bool staticMode; ///< boolean true if no position feedback is fused + bool useGPS; // boolean true if GPS data is being used bool useAirspeed; ///< boolean true if airspeed data is being used bool useCompass; ///< boolean true if magnetometer data is being used bool useRangeFinder; ///< true when rangefinder is being used @@ -267,11 +261,9 @@ void FuseMagnetometer(); void FuseAirspeed(); -void FuseRangeFinder(); - void FuseOptFlow(); -void GroundEKF(); +void OpticalFlowEKF(); void zeroRows(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last); @@ -286,7 +278,7 @@ void StoreStates(uint64_t timestamp_ms); * Recall the state vector. * * Recalls the vector stored at closest time to the one specified by msec - * + *FuseOptFlow * @return zero on success, integer indicating the number of invalid states on failure. * Does only copy valid states, if the statesForFusion vector was initialized * correctly by the caller, the result can be safely used, but is a mixture @@ -295,6 +287,8 @@ void StoreStates(uint64_t timestamp_ms); */ int RecallStates(float *statesForFusion, uint64_t msec); +void RecallOmega(float *omegaForFusion, uint64_t msec); + void ResetStoredStates(); void quat2Tbn(Mat3f &TBodyNed, const float (&quat)[4]); @@ -325,7 +319,7 @@ void CovarianceInit(); void InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt, float declination); -float ConstrainFloat(float val, float min, float max); +float ConstrainFloat(float val, float min, float maxf); void ConstrainVariances(); diff --git a/src/modules/ekf_att_pos_estimator/module.mk b/src/modules/ekf_att_pos_estimator/module.mk index 843dde9789..f519621137 100644 --- a/src/modules/ekf_att_pos_estimator/module.mk +++ b/src/modules/ekf_att_pos_estimator/module.mk @@ -39,7 +39,7 @@ MODULE_COMMAND = ekf_att_pos_estimator SRCS = ekf_att_pos_estimator_main.cpp \ ekf_att_pos_estimator_params.c \ - estimator_23states.cpp \ + estimator_22states.cpp \ estimator_utilities.cpp EXTRACXXFLAGS = -Weffc++ -Wframe-larger-than=6100 From 84d724503f5fe687cad526cb46a142314bed02eb Mon Sep 17 00:00:00 2001 From: Mohammed Kabir Date: Tue, 30 Dec 2014 18:06:48 +0530 Subject: [PATCH 288/652] Remove invalid params --- src/modules/sensors/sensor_params.c | 21 ++------------------- 1 file changed, 2 insertions(+), 19 deletions(-) diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index f5f6094d1d..3c78b7b173 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -264,7 +264,8 @@ PARAM_DEFINE_INT32(SENS_BOARD_ROT, 0); /** * PX4Flow board rotation * - * This parameter defines the rotation of the PX4FLOW board relative to the platform. + * This parameter defines the rotation of the PX4FLOW board relative to the platform. + * Zero rotation is defined as Y on flow board pointing towards front of vehicle * Possible values are: * 0 = No rotation * 1 = Yaw 45° @@ -274,24 +275,6 @@ PARAM_DEFINE_INT32(SENS_BOARD_ROT, 0); * 5 = Yaw 225° * 6 = Yaw 270° * 7 = Yaw 315° - * 8 = Roll 180° - * 9 = Roll 180°, Yaw 45° - * 10 = Roll 180°, Yaw 90° - * 11 = Roll 180°, Yaw 135° - * 12 = Pitch 180° - * 13 = Roll 180°, Yaw 225° - * 14 = Roll 180°, Yaw 270° - * 15 = Roll 180°, Yaw 315° - * 16 = Roll 90° - * 17 = Roll 90°, Yaw 45° - * 18 = Roll 90°, Yaw 90° - * 19 = Roll 90°, Yaw 135° - * 20 = Roll 270° - * 21 = Roll 270°, Yaw 45° - * 22 = Roll 270°, Yaw 90° - * 23 = Roll 270°, Yaw 135° - * 24 = Pitch 90° - * 25 = Pitch 270° * * @group Sensor Calibration */ From 3e06a65516697c6bdf840d8104328ad09c0ccfbd Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 24 Dec 2014 12:56:10 -0800 Subject: [PATCH 289/652] mpu6000: monitor some key registers for correct values this will catch both bad SPI bus comms and a sensor that has been reset causing incorrect configuration. --- src/drivers/mpu6000/mpu6000.cpp | 142 +++++++++++++++++++++++++++++--- 1 file changed, 132 insertions(+), 10 deletions(-) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index c9c27892f4..3d720cd94b 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -231,8 +231,11 @@ private: perf_counter_t _gyro_reads; perf_counter_t _sample_perf; perf_counter_t _bad_transfers; + perf_counter_t _bad_registers; perf_counter_t _good_transfers; + uint8_t _register_wait; + math::LowPassFilter2p _accel_filter_x; math::LowPassFilter2p _accel_filter_y; math::LowPassFilter2p _accel_filter_z; @@ -242,6 +245,14 @@ private: enum Rotation _rotation; + // this is used to support runtime checking of key + // configuration registers to detect SPI bus errors and sensor + // reset +#define MPU6000_NUM_CHECKED_REGISTERS 4 + static const uint8_t _checked_registers[MPU6000_NUM_CHECKED_REGISTERS]; + uint8_t _checked_values[MPU6000_NUM_CHECKED_REGISTERS]; + uint8_t _checked_next; + /** * Start automatic measurement. */ @@ -281,7 +292,7 @@ private: * @param The register to read. * @return The value that was read. */ - uint8_t read_reg(unsigned reg); + uint8_t read_reg(unsigned reg, uint32_t speed=MPU6000_LOW_BUS_SPEED); uint16_t read_reg16(unsigned reg); /** @@ -303,6 +314,14 @@ private: */ void modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits); + /** + * Write a register in the MPU6000, updating _checked_values + * + * @param reg The register to write. + * @param value The new value to write. + */ + void write_checked_reg(unsigned reg, uint8_t value); + /** * Set the MPU6000 measurement range. * @@ -347,11 +366,26 @@ private: */ void _set_sample_rate(uint16_t desired_sample_rate_hz); + /* + check that key registers still have the right value + */ + void check_registers(void); + /* do not allow to copy this class due to pointer data members */ MPU6000(const MPU6000&); MPU6000 operator=(const MPU6000&); }; +/* + list of registers that will be checked in check_registers(). Note + that MPUREG_PRODUCT_ID must be first in the list. + */ +const uint8_t MPU6000::_checked_registers[MPU6000_NUM_CHECKED_REGISTERS] = { MPUREG_PRODUCT_ID, + MPUREG_GYRO_CONFIG, + MPUREG_ACCEL_CONFIG, + MPUREG_USER_CTRL }; + + /** * Helper class implementing the gyro driver node. */ @@ -407,14 +441,17 @@ MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev _gyro_reads(perf_alloc(PC_COUNT, "mpu6000_gyro_read")), _sample_perf(perf_alloc(PC_ELAPSED, "mpu6000_read")), _bad_transfers(perf_alloc(PC_COUNT, "mpu6000_bad_transfers")), + _bad_registers(perf_alloc(PC_COUNT, "mpu6000_bad_registers")), _good_transfers(perf_alloc(PC_COUNT, "mpu6000_good_transfers")), + _register_wait(0), _accel_filter_x(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _accel_filter_y(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _accel_filter_z(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _gyro_filter_x(MPU6000_GYRO_DEFAULT_RATE, MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ), _gyro_filter_y(MPU6000_GYRO_DEFAULT_RATE, MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ), _gyro_filter_z(MPU6000_GYRO_DEFAULT_RATE, MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ), - _rotation(rotation) + _rotation(rotation), + _checked_next(0) { // disable debug() calls _debug_enabled = false; @@ -460,6 +497,7 @@ MPU6000::~MPU6000() perf_free(_accel_reads); perf_free(_gyro_reads); perf_free(_bad_transfers); + perf_free(_bad_registers); perf_free(_good_transfers); } @@ -590,7 +628,7 @@ void MPU6000::reset() up_udelay(1000); // Disable I2C bus (recommended on datasheet) - write_reg(MPUREG_USER_CTRL, BIT_I2C_IF_DIS); + write_checked_reg(MPUREG_USER_CTRL, BIT_I2C_IF_DIS); irqrestore(state); usleep(1000); @@ -605,7 +643,7 @@ void MPU6000::reset() _set_dlpf_filter(MPU6000_DEFAULT_ONCHIP_FILTER_FREQ); usleep(1000); // Gyro scale 2000 deg/s () - write_reg(MPUREG_GYRO_CONFIG, BITS_FS_2000DPS); + write_checked_reg(MPUREG_GYRO_CONFIG, BITS_FS_2000DPS); usleep(1000); // correct gyro scale factors @@ -624,7 +662,7 @@ void MPU6000::reset() case MPU6000_REV_C5: // Accel scale 8g (4096 LSB/g) // Rev C has different scaling than rev D - write_reg(MPUREG_ACCEL_CONFIG, 1 << 3); + write_checked_reg(MPUREG_ACCEL_CONFIG, 1 << 3); break; case MPU6000ES_REV_D6: @@ -639,7 +677,7 @@ void MPU6000::reset() // presumably won't have the accel scaling bug default: // Accel scale 8g (4096 LSB/g) - write_reg(MPUREG_ACCEL_CONFIG, 2 << 3); + write_checked_reg(MPUREG_ACCEL_CONFIG, 2 << 3); break; } @@ -684,6 +722,7 @@ MPU6000::probe() case MPU6000_REV_D9: case MPU6000_REV_D10: debug("ID 0x%02x", _product); + _checked_values[0] = _product; return OK; } @@ -734,7 +773,7 @@ MPU6000::_set_dlpf_filter(uint16_t frequency_hz) } else { filter = BITS_DLPF_CFG_2100HZ_NOLPF; } - write_reg(MPUREG_CONFIG, filter); + write_checked_reg(MPUREG_CONFIG, filter); } ssize_t @@ -1094,12 +1133,12 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg) } uint8_t -MPU6000::read_reg(unsigned reg) +MPU6000::read_reg(unsigned reg, uint32_t speed) { uint8_t cmd[2] = { (uint8_t)(reg | DIR_READ), 0}; // general register transfer at low clock speed - set_frequency(MPU6000_LOW_BUS_SPEED); + set_frequency(speed); transfer(cmd, cmd, sizeof(cmd)); @@ -1144,6 +1183,17 @@ MPU6000::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits) write_reg(reg, val); } +void +MPU6000::write_checked_reg(unsigned reg, uint8_t value) +{ + write_reg(reg, value); + for (uint8_t i=0; imeasure(); } +void +MPU6000::check_registers(void) +{ + /* + we read the register at full speed, even though it isn't + listed as a high speed register. The low speed requirement + for some registers seems to be a propgation delay + requirement for changing sensor configuration, which should + not apply to reading a single register. It is also a better + test of SPI bus health to read at the same speed as we read + the data registers. + */ + uint8_t v; + if ((v=read_reg(_checked_registers[_checked_next], MPU6000_HIGH_BUS_SPEED)) != + _checked_values[_checked_next]) { + /* + if we get the wrong value then we know the SPI bus + or sensor is very sick. We set _register_wait to 20 + and wait until we have seen 20 good values in a row + before we consider the sensor to be OK again. + */ + perf_count(_bad_registers); + + /* + try to fix the bad register value. We only try to + fix one per loop to prevent a bad sensor hogging the + bus. We skip zero as that is the PRODUCT_ID, which + is not writeable + */ + if (_checked_next != 0) { + write_reg(_checked_registers[_checked_next], _checked_values[_checked_next]); + } +#if 0 + if (_register_wait == 0) { + ::printf("MPU6000: %02x:%02x should be %02x\n", + (unsigned)_checked_registers[_checked_next], + (unsigned)v, + (unsigned)_checked_values[_checked_next]); + } +#endif + _register_wait = 20; + } else { + _checked_next = (_checked_next+1) % MPU6000_NUM_CHECKED_REGISTERS; + } +} + void MPU6000::measure() { @@ -1254,6 +1350,8 @@ MPU6000::measure() */ mpu_report.cmd = DIR_READ | MPUREG_INT_STATUS; + check_registers(); + // sensor transfer at high clock speed set_frequency(MPU6000_HIGH_BUS_SPEED); @@ -1292,6 +1390,14 @@ MPU6000::measure() } perf_count(_good_transfers); + + if (_register_wait != 0) { + // we are waiting for some good transfers before using + // the sensor again. We still increment + // _good_transfers, but don't return any data yet + _register_wait--; + return; + } /* @@ -1321,7 +1427,12 @@ MPU6000::measure() * Adjust and scale results to m/s^2. */ grb.timestamp = arb.timestamp = hrt_absolute_time(); - grb.error_count = arb.error_count = 0; // not reported + + // report the error count as the sum of the number of bad + // transfers and bad register reads. This allows the higher + // level code to decide if it should use this sensor based on + // whether it has had failures + grb.error_count = arb.error_count = perf_event_count(_bad_transfers) + perf_event_count(_bad_registers); /* * 1) Scale raw value to SI units using scaling from datasheet. @@ -1411,9 +1522,20 @@ MPU6000::print_info() perf_print_counter(_accel_reads); perf_print_counter(_gyro_reads); perf_print_counter(_bad_transfers); + perf_print_counter(_bad_registers); perf_print_counter(_good_transfers); _accel_reports->print_info("accel queue"); _gyro_reports->print_info("gyro queue"); + ::printf("checked_next: %u\n", _checked_next); + for (uint8_t i=0; i Date: Mon, 29 Dec 2014 11:03:08 +1100 Subject: [PATCH 290/652] lsm303d: replace old register checking with new check_registers() method this uses the same method as is now used in the MPU6000 to check that the sensor retains its correct values Conflicts: src/drivers/lsm303d/lsm303d.cpp --- src/drivers/lsm303d/lsm303d.cpp | 347 ++++++++++++-------------------- 1 file changed, 125 insertions(+), 222 deletions(-) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 01c89192a7..dbd5c1f4c3 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -77,10 +77,6 @@ #endif static const int ERROR = -1; -// enable this to debug the buggy lsm303d sensor in very early -// prototype pixhawk boards -#define CHECK_EXTREMES 0 - /* SPI protocol address bits */ #define DIR_READ (1<<7) #define DIR_WRITE (0<<7) @@ -241,16 +237,6 @@ public: */ void print_registers(); - /** - * toggle logging - */ - void toggle_logging(); - - /** - * check for extreme accel values - */ - void check_extremes(const accel_report *arb); - protected: virtual int probe(); @@ -292,30 +278,25 @@ private: perf_counter_t _accel_sample_perf; perf_counter_t _mag_sample_perf; - perf_counter_t _reg1_resets; - perf_counter_t _reg7_resets; - perf_counter_t _extreme_values; perf_counter_t _accel_reschedules; + perf_counter_t _bad_registers; + + uint8_t _register_wait; math::LowPassFilter2p _accel_filter_x; math::LowPassFilter2p _accel_filter_y; math::LowPassFilter2p _accel_filter_z; - // expceted values of reg1 and reg7 to catch in-flight - // brownouts of the sensor - uint8_t _reg1_expected; - uint8_t _reg7_expected; - - // accel logging - int _accel_log_fd; - bool _accel_logging_enabled; - uint64_t _last_extreme_us; - uint64_t _last_log_us; - uint64_t _last_log_sync_us; - uint64_t _last_log_reg_us; - uint64_t _last_log_alarm_us; enum Rotation _rotation; + // this is used to support runtime checking of key + // configuration registers to detect SPI bus errors and sensor + // reset +#define LSM303D_NUM_CHECKED_REGISTERS 6 + static const uint8_t _checked_registers[LSM303D_NUM_CHECKED_REGISTERS]; + uint8_t _checked_values[LSM303D_NUM_CHECKED_REGISTERS]; + uint8_t _checked_next; + /** * Start automatic measurement. */ @@ -356,6 +337,11 @@ private: */ static void mag_measure_trampoline(void *arg); + /** + * check key registers for correct values + */ + void check_registers(void); + /** * Fetch accel measurements from the sensor and update the report ring. */ @@ -407,6 +393,14 @@ private: */ void modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits); + /** + * Write a register in the LSM303D, updating _checked_values + * + * @param reg The register to write. + * @param value The new value to write. + */ + void write_checked_reg(unsigned reg, uint8_t value); + /** * Set the LSM303D accel measurement range. * @@ -468,6 +462,17 @@ private: LSM303D operator=(const LSM303D&); }; +/* + list of registers that will be checked in check_registers(). Note + that ADDR_WHO_AM_I must be first in the list. + */ +const uint8_t LSM303D::_checked_registers[LSM303D_NUM_CHECKED_REGISTERS] = { ADDR_WHO_AM_I, + ADDR_CTRL_REG1, + ADDR_CTRL_REG2, + ADDR_CTRL_REG5, + ADDR_CTRL_REG6, + ADDR_CTRL_REG7 }; + /** * Helper class implementing the mag driver node. */ @@ -528,23 +533,14 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device, enum Rotation rota _mag_read(0), _accel_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_accel_read")), _mag_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_mag_read")), - _reg1_resets(perf_alloc(PC_COUNT, "lsm303d_reg1_resets")), - _reg7_resets(perf_alloc(PC_COUNT, "lsm303d_reg7_resets")), - _extreme_values(perf_alloc(PC_COUNT, "lsm303d_extremes")), _accel_reschedules(perf_alloc(PC_COUNT, "lsm303d_accel_resched")), + _bad_registers(perf_alloc(PC_COUNT, "lsm303d_bad_registers")), + _register_wait(0), _accel_filter_x(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _accel_filter_y(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _accel_filter_z(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), - _reg1_expected(0), - _reg7_expected(0), - _accel_log_fd(-1), - _accel_logging_enabled(false), - _last_extreme_us(0), - _last_log_us(0), - _last_log_sync_us(0), - _last_log_reg_us(0), - _last_log_alarm_us(0), - _rotation(rotation) + _rotation(rotation), + _checked_next(0) { _device_id.devid_s.devtype = DRV_MAG_DEVTYPE_LSM303D; @@ -586,9 +582,7 @@ LSM303D::~LSM303D() /* delete the perf counter */ perf_free(_accel_sample_perf); perf_free(_mag_sample_perf); - perf_free(_reg1_resets); - perf_free(_reg7_resets); - perf_free(_extreme_values); + perf_free(_bad_registers); perf_free(_accel_reschedules); } @@ -703,13 +697,12 @@ LSM303D::reset() disable_i2c(); /* enable accel*/ - _reg1_expected = REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A | REG1_BDU_UPDATE | REG1_RATE_800HZ_A; - write_reg(ADDR_CTRL_REG1, _reg1_expected); + write_checked_reg(ADDR_CTRL_REG1, + REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A | REG1_BDU_UPDATE | REG1_RATE_800HZ_A); /* enable mag */ - _reg7_expected = REG7_CONT_MODE_M; - write_reg(ADDR_CTRL_REG7, _reg7_expected); - write_reg(ADDR_CTRL_REG5, REG5_RES_HIGH_M); + write_checked_reg(ADDR_CTRL_REG7, REG7_CONT_MODE_M); + write_checked_reg(ADDR_CTRL_REG5, REG5_RES_HIGH_M); write_reg(ADDR_CTRL_REG3, 0x04); // DRDY on ACCEL on INT1 write_reg(ADDR_CTRL_REG4, 0x04); // DRDY on MAG on INT2 @@ -739,128 +732,14 @@ LSM303D::probe() /* verify that the device is attached and functioning */ bool success = (read_reg(ADDR_WHO_AM_I) == WHO_I_AM); - if (success) + if (success) { + _checked_values[0] = WHO_I_AM; return OK; + } return -EIO; } -#define ACCEL_LOGFILE "/fs/microsd/lsm303d.log" - -/** - check for extreme accelerometer values and log to a file on the SD card - */ -void -LSM303D::check_extremes(const accel_report *arb) -{ - const float extreme_threshold = 30; - static bool boot_ok = false; - bool is_extreme = (fabsf(arb->x) > extreme_threshold && - fabsf(arb->y) > extreme_threshold && - fabsf(arb->z) > extreme_threshold); - if (is_extreme) { - perf_count(_extreme_values); - // force accel logging on if we see extreme values - _accel_logging_enabled = true; - } else { - boot_ok = true; - } - - if (! _accel_logging_enabled) { - // logging has been disabled by user, close - if (_accel_log_fd != -1) { - ::close(_accel_log_fd); - _accel_log_fd = -1; - } - return; - } - if (_accel_log_fd == -1) { - // keep last 10 logs - ::unlink(ACCEL_LOGFILE ".9"); - for (uint8_t i=8; i>0; i--) { - uint8_t len = strlen(ACCEL_LOGFILE)+3; - char log1[len], log2[len]; - snprintf(log1, sizeof(log1), "%s.%u", ACCEL_LOGFILE, (unsigned)i); - snprintf(log2, sizeof(log2), "%s.%u", ACCEL_LOGFILE, (unsigned)(i+1)); - ::rename(log1, log2); - } - ::rename(ACCEL_LOGFILE, ACCEL_LOGFILE ".1"); - - // open the new logfile - _accel_log_fd = ::open(ACCEL_LOGFILE, O_WRONLY|O_CREAT|O_TRUNC, 0666); - if (_accel_log_fd == -1) { - return; - } - } - - uint64_t now = hrt_absolute_time(); - // log accels at 1Hz - if (_last_log_us == 0 || - now - _last_log_us > 1000*1000) { - _last_log_us = now; - ::dprintf(_accel_log_fd, "ARB %llu %.3f %.3f %.3f %d %d %d boot_ok=%u\r\n", - (unsigned long long)arb->timestamp, - (double)arb->x, (double)arb->y, (double)arb->z, - (int)arb->x_raw, - (int)arb->y_raw, - (int)arb->z_raw, - (unsigned)boot_ok); - } - - const uint8_t reglist[] = { ADDR_WHO_AM_I, 0x02, 0x15, ADDR_STATUS_A, ADDR_STATUS_M, ADDR_CTRL_REG0, ADDR_CTRL_REG1, - ADDR_CTRL_REG2, ADDR_CTRL_REG3, ADDR_CTRL_REG4, ADDR_CTRL_REG5, ADDR_CTRL_REG6, - ADDR_CTRL_REG7, ADDR_OUT_TEMP_L, ADDR_OUT_TEMP_H, ADDR_INT_CTRL_M, ADDR_INT_SRC_M, - ADDR_REFERENCE_X, ADDR_REFERENCE_Y, ADDR_REFERENCE_Z, ADDR_OUT_X_L_A, ADDR_OUT_X_H_A, - ADDR_OUT_Y_L_A, ADDR_OUT_Y_H_A, ADDR_OUT_Z_L_A, ADDR_OUT_Z_H_A, ADDR_FIFO_CTRL, - ADDR_FIFO_SRC, ADDR_IG_CFG1, ADDR_IG_SRC1, ADDR_IG_THS1, ADDR_IG_DUR1, ADDR_IG_CFG2, - ADDR_IG_SRC2, ADDR_IG_THS2, ADDR_IG_DUR2, ADDR_CLICK_CFG, ADDR_CLICK_SRC, - ADDR_CLICK_THS, ADDR_TIME_LIMIT, ADDR_TIME_LATENCY, ADDR_TIME_WINDOW, - ADDR_ACT_THS, ADDR_ACT_DUR, - ADDR_OUT_X_L_M, ADDR_OUT_X_H_M, - ADDR_OUT_Y_L_M, ADDR_OUT_Y_H_M, ADDR_OUT_Z_L_M, ADDR_OUT_Z_H_M, 0x02, 0x15, ADDR_WHO_AM_I}; - uint8_t regval[sizeof(reglist)]; - for (uint8_t i=0; i 250*1000)) || - (now - _last_log_reg_us > 10*1000*1000)) { - _last_log_reg_us = now; - ::dprintf(_accel_log_fd, "XREG %llu", (unsigned long long)hrt_absolute_time()); - for (uint8_t i=0; i 10*1000*1000) { - _last_log_sync_us = now; - ::fsync(_accel_log_fd); - } - - // play alarm every 10s if we have had an extreme value - if (perf_event_count(_extreme_values) != 0 && - (now - _last_log_alarm_us > 10*1000*1000)) { - _last_log_alarm_us = now; - int tfd = ::open(TONEALARM_DEVICE_PATH, 0); - if (tfd != -1) { - uint8_t tone = 3; - if (!is_extreme) { - tone = 3; - } else if (boot_ok) { - tone = 4; - } else { - tone = 5; - } - ::ioctl(tfd, TONE_SET_ALARM, tone); - ::close(tfd); - } - } -} - ssize_t LSM303D::read(struct file *filp, char *buffer, size_t buflen) { @@ -879,9 +758,6 @@ LSM303D::read(struct file *filp, char *buffer, size_t buflen) */ while (count--) { if (_accel_reports->get(arb)) { -#if CHECK_EXTREMES - check_extremes(arb); -#endif ret += sizeof(*arb); arb++; } @@ -1262,6 +1138,17 @@ LSM303D::write_reg(unsigned reg, uint8_t value) transfer(cmd, nullptr, sizeof(cmd)); } +void +LSM303D::write_checked_reg(unsigned reg, uint8_t value) +{ + write_reg(reg, value); + for (uint8_t i=0; imag_measure(); } +void +LSM303D::check_registers(void) +{ + uint8_t v; + if ((v=read_reg(_checked_registers[_checked_next])) != _checked_values[_checked_next]) { + /* + if we get the wrong value then we know the SPI bus + or sensor is very sick. We set _register_wait to 20 + and wait until we have seen 20 good values in a row + before we consider the sensor to be OK again. + */ + perf_count(_bad_registers); + + /* + try to fix the bad register value. We only try to + fix one per loop to prevent a bad sensor hogging the + bus. We skip zero as that is the WHO_AM_I, which + is not writeable + */ + if (_checked_next != 0) { + write_reg(_checked_registers[_checked_next], _checked_values[_checked_next]); + } +#if 1 + if (_register_wait == 0) { + ::printf("LSM303D: %02x:%02x should be %02x\n", + (unsigned)_checked_registers[_checked_next], + (unsigned)v, + (unsigned)_checked_values[_checked_next]); + } +#endif + _register_wait = 20; + } + _checked_next = (_checked_next+1) % LSM303D_NUM_CHECKED_REGISTERS; +} + void LSM303D::measure() { @@ -1522,16 +1443,13 @@ LSM303D::measure() // read a value and then miss the next value. // Note that DRDY is not available when the lsm303d is // connected on the external bus +#ifdef GPIO_EXTI_ACCEL_DRDY if (_bus == PX4_SPI_BUS_SENSORS && stm32_gpioread(GPIO_EXTI_ACCEL_DRDY) == 0) { perf_count(_accel_reschedules); hrt_call_delay(&_accel_call, 100); return; } - if (read_reg(ADDR_CTRL_REG1) != _reg1_expected) { - perf_count(_reg1_resets); - reset(); - return; - } +#endif /* status register and data as read back from the device */ @@ -1550,6 +1468,16 @@ LSM303D::measure() /* start the performance counter */ perf_begin(_accel_sample_perf); + check_registers(); + + if (_register_wait != 0) { + // we are waiting for some good transfers before using + // the sensor again. + _register_wait--; + perf_end(_accel_sample_perf); + return; + } + /* fetch data from the sensor */ memset(&raw_accel_report, 0, sizeof(raw_accel_report)); raw_accel_report.cmd = ADDR_STATUS_A | DIR_READ | ADDR_INCREMENT; @@ -1572,7 +1500,12 @@ LSM303D::measure() accel_report.timestamp = hrt_absolute_time(); - accel_report.error_count = 0; // not reported + + // report the error count as the sum of the number of bad bad + // register reads. This allows the higher level code to decide + // if it should use this sensor based on whether it has had + // failures + accel_report.error_count = perf_event_count(_bad_registers); accel_report.x_raw = raw_accel_report.x; accel_report.y_raw = raw_accel_report.y; @@ -1611,12 +1544,6 @@ LSM303D::measure() void LSM303D::mag_measure() { - if (read_reg(ADDR_CTRL_REG7) != _reg7_expected) { - perf_count(_reg7_resets); - reset(); - return; - } - /* status register and data as read back from the device */ #pragma pack(push, 1) struct { @@ -1691,8 +1618,19 @@ LSM303D::print_info() printf("accel reads: %u\n", _accel_read); printf("mag reads: %u\n", _mag_read); perf_print_counter(_accel_sample_perf); + perf_print_counter(_bad_registers); _accel_reports->print_info("accel reports"); _mag_reports->print_info("mag reports"); + ::printf("checked_next: %u\n", _checked_next); + for (uint8_t i=0; itoggle_logging(); - - exit(0); -} - void usage() { - warnx("missing command: try 'start', 'info', 'test', 'reset', 'regdump', 'logging'"); + warnx("missing command: try 'start', 'info', 'test', 'reset', 'regdump'"); warnx("options:"); warnx(" -X (external bus)"); warnx(" -R rotation"); @@ -2126,11 +2035,5 @@ lsm303d_main(int argc, char *argv[]) if (!strcmp(verb, "regdump")) lsm303d::regdump(); - /* - * dump device registers - */ - if (!strcmp(verb, "logging")) - lsm303d::logging(); - - errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info', 'logging' or 'regdump'"); + errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info', or 'regdump'"); } From ae3a92569d3b10a5f8c125ff54910a686594ea68 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 29 Dec 2014 14:00:09 +1100 Subject: [PATCH 291/652] mpu6000: try resetting the mpu6000 up to 5 times this mirrors the ardupilot driver. We have seen situations where the mpu6000 on the Pixhawk comes up in SLEEP mode, despite a reset --- src/drivers/mpu6000/mpu6000.cpp | 58 +++++++++++++++++++++------------ 1 file changed, 37 insertions(+), 21 deletions(-) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 3d720cd94b..eb9666dc5a 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -233,6 +233,7 @@ private: perf_counter_t _bad_transfers; perf_counter_t _bad_registers; perf_counter_t _good_transfers; + perf_counter_t _reset_retries; uint8_t _register_wait; @@ -248,7 +249,7 @@ private: // this is used to support runtime checking of key // configuration registers to detect SPI bus errors and sensor // reset -#define MPU6000_NUM_CHECKED_REGISTERS 4 +#define MPU6000_NUM_CHECKED_REGISTERS 5 static const uint8_t _checked_registers[MPU6000_NUM_CHECKED_REGISTERS]; uint8_t _checked_values[MPU6000_NUM_CHECKED_REGISTERS]; uint8_t _checked_next; @@ -268,7 +269,7 @@ private: * * Resets the chip and measurements ranges, but not scale and offset. */ - void reset(); + int reset(); /** * Static trampoline from the hrt_call context; because we don't have a @@ -383,7 +384,8 @@ private: const uint8_t MPU6000::_checked_registers[MPU6000_NUM_CHECKED_REGISTERS] = { MPUREG_PRODUCT_ID, MPUREG_GYRO_CONFIG, MPUREG_ACCEL_CONFIG, - MPUREG_USER_CTRL }; + MPUREG_USER_CTRL, + MPUREG_PWR_MGMT_1}; /** @@ -443,6 +445,7 @@ MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev _bad_transfers(perf_alloc(PC_COUNT, "mpu6000_bad_transfers")), _bad_registers(perf_alloc(PC_COUNT, "mpu6000_bad_registers")), _good_transfers(perf_alloc(PC_COUNT, "mpu6000_good_transfers")), + _reset_retries(perf_alloc(PC_COUNT, "mpu6000_reset_retries")), _register_wait(0), _accel_filter_x(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _accel_filter_y(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), @@ -524,7 +527,8 @@ MPU6000::init() if (_gyro_reports == nullptr) goto out; - reset(); + if (reset() != OK) + goto out; /* Initialize offsets and scales */ _accel_scale.x_offset = 0; @@ -609,27 +613,39 @@ out: return ret; } -void MPU6000::reset() +int MPU6000::reset() { // if the mpu6000 is initialised after the l3gd20 and lsm303d // then if we don't do an irqsave/irqrestore here the mpu6000 // frequenctly comes up in a bad state where all transfers // come as zero - irqstate_t state; - state = irqsave(); + uint8_t tries = 5; + while (--tries != 0) { + irqstate_t state; + state = irqsave(); - write_reg(MPUREG_PWR_MGMT_1, BIT_H_RESET); - up_udelay(10000); + write_reg(MPUREG_PWR_MGMT_1, BIT_H_RESET); + up_udelay(10000); - // Wake up device and select GyroZ clock. Note that the - // MPU6000 starts up in sleep mode, and it can take some time - // for it to come out of sleep - write_reg(MPUREG_PWR_MGMT_1, MPU_CLK_SEL_PLLGYROZ); - up_udelay(1000); + // Wake up device and select GyroZ clock. Note that the + // MPU6000 starts up in sleep mode, and it can take some time + // for it to come out of sleep + write_checked_reg(MPUREG_PWR_MGMT_1, MPU_CLK_SEL_PLLGYROZ); + up_udelay(1000); + + // Disable I2C bus (recommended on datasheet) + write_checked_reg(MPUREG_USER_CTRL, BIT_I2C_IF_DIS); + irqrestore(state); - // Disable I2C bus (recommended on datasheet) - write_checked_reg(MPUREG_USER_CTRL, BIT_I2C_IF_DIS); - irqrestore(state); + if (read_reg(MPUREG_PWR_MGMT_1) == MPU_CLK_SEL_PLLGYROZ) { + break; + } + perf_count(_reset_retries); + usleep(2000); + } + if (read_reg(MPUREG_PWR_MGMT_1) != MPU_CLK_SEL_PLLGYROZ) { + return -EIO; + } usleep(1000); @@ -698,6 +714,7 @@ void MPU6000::reset() // write_reg(MPUREG_PWR_MGMT_1,MPU_CLK_SEL_PLLGYROZ); usleep(1000); + return OK; } int @@ -913,8 +930,7 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) switch (cmd) { case SENSORIOCRESET: - reset(); - return OK; + return reset(); case SENSORIOCSPOLLRATE: { switch (arg) { @@ -1306,9 +1322,8 @@ MPU6000::check_registers(void) } #endif _register_wait = 20; - } else { - _checked_next = (_checked_next+1) % MPU6000_NUM_CHECKED_REGISTERS; } + _checked_next = (_checked_next+1) % MPU6000_NUM_CHECKED_REGISTERS; } void @@ -1524,6 +1539,7 @@ MPU6000::print_info() perf_print_counter(_bad_transfers); perf_print_counter(_bad_registers); perf_print_counter(_good_transfers); + perf_print_counter(_reset_retries); _accel_reports->print_info("accel queue"); _gyro_reports->print_info("gyro queue"); ::printf("checked_next: %u\n", _checked_next); From ca47952281cfe66732b08d3878eb6c8b1613abeb Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 29 Dec 2014 14:18:10 +1100 Subject: [PATCH 292/652] l3gd20: added register checking this checks at runtime that key registers have correct values --- src/drivers/l3gd20/l3gd20.cpp | 146 ++++++++++++++++++++++++++++------ 1 file changed, 122 insertions(+), 24 deletions(-) diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 82fa5cc6e7..de9d9140f0 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -225,6 +225,9 @@ private: perf_counter_t _sample_perf; perf_counter_t _reschedules; perf_counter_t _errors; + perf_counter_t _bad_registers; + + uint8_t _register_wait; math::LowPassFilter2p _gyro_filter_x; math::LowPassFilter2p _gyro_filter_y; @@ -235,6 +238,14 @@ private: enum Rotation _rotation; + // this is used to support runtime checking of key + // configuration registers to detect SPI bus errors and sensor + // reset +#define L3GD20_NUM_CHECKED_REGISTERS 7 + static const uint8_t _checked_registers[L3GD20_NUM_CHECKED_REGISTERS]; + uint8_t _checked_values[L3GD20_NUM_CHECKED_REGISTERS]; + uint8_t _checked_next; + /** * Start automatic measurement. */ @@ -266,6 +277,11 @@ private: */ static void measure_trampoline(void *arg); + /** + * check key registers for correct values + */ + void check_registers(void); + /** * Fetch measurements from the sensor and update the report ring. */ @@ -298,6 +314,14 @@ private: */ void modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits); + /** + * Write a register in the L3GD20, updating _checked_values + * + * @param reg The register to write. + * @param value The new value to write. + */ + void write_checked_reg(unsigned reg, uint8_t value); + /** * Set the L3GD20 measurement range. * @@ -338,6 +362,18 @@ private: L3GD20 operator=(const L3GD20&); }; +/* + list of registers that will be checked in check_registers(). Note + that ADDR_WHO_AM_I must be first in the list. + */ +const uint8_t L3GD20::_checked_registers[L3GD20_NUM_CHECKED_REGISTERS] = { ADDR_WHO_AM_I, + ADDR_CTRL_REG1, + ADDR_CTRL_REG2, + ADDR_CTRL_REG3, + ADDR_CTRL_REG4, + ADDR_CTRL_REG5, + ADDR_FIFO_CTRL_REG }; + L3GD20::L3GD20(int bus, const char* path, spi_dev_e device, enum Rotation rotation) : SPI("L3GD20", path, bus, device, SPIDEV_MODE3, 11*1000*1000 /* will be rounded to 10.4 MHz, within margins for L3GD20 */), _call{}, @@ -355,11 +391,14 @@ L3GD20::L3GD20(int bus, const char* path, spi_dev_e device, enum Rotation rotati _sample_perf(perf_alloc(PC_ELAPSED, "l3gd20_read")), _reschedules(perf_alloc(PC_COUNT, "l3gd20_reschedules")), _errors(perf_alloc(PC_COUNT, "l3gd20_errors")), + _bad_registers(perf_alloc(PC_COUNT, "l3gd20_bad_registers")), + _register_wait(0), _gyro_filter_x(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ), _gyro_filter_y(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ), _gyro_filter_z(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ), _is_l3g4200d(false), - _rotation(rotation) + _rotation(rotation), + _checked_next(0) { // enable debug() calls _debug_enabled = true; @@ -389,6 +428,7 @@ L3GD20::~L3GD20() perf_free(_sample_perf); perf_free(_reschedules); perf_free(_errors); + perf_free(_bad_registers); } int @@ -448,29 +488,27 @@ L3GD20::probe() (void)read_reg(ADDR_WHO_AM_I); bool success = false; + uint8_t v = 0; - /* verify that the device is attached and functioning, accept L3GD20 and L3GD20H */ - if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM) { - + /* verify that the device is attached and functioning, accept + * L3GD20, L3GD20H and L3G4200D */ + if ((v=read_reg(ADDR_WHO_AM_I)) == WHO_I_AM) { _orientation = SENSOR_BOARD_ROTATION_DEFAULT; success = true; - } - - - if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM_H) { + } else if ((v=read_reg(ADDR_WHO_AM_I)) == WHO_I_AM_H) { _orientation = SENSOR_BOARD_ROTATION_180_DEG; success = true; - } - - /* Detect the L3G4200D used on AeroCore */ - if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM_L3G4200D) { + } else if ((v=read_reg(ADDR_WHO_AM_I)) == WHO_I_AM_L3G4200D) { + /* Detect the L3G4200D used on AeroCore */ _is_l3g4200d = true; _orientation = SENSOR_BOARD_ROTATION_DEFAULT; success = true; } - if (success) + if (success) { + _checked_values[0] = v; return OK; + } return -EIO; } @@ -672,6 +710,18 @@ L3GD20::write_reg(unsigned reg, uint8_t value) transfer(cmd, nullptr, sizeof(cmd)); } +void +L3GD20::write_checked_reg(unsigned reg, uint8_t value) +{ + write_reg(reg, value); + for (uint8_t i=0; iprint_info("report queue"); + ::printf("checked_next: %u\n", _checked_next); + for (uint8_t i=0; i Date: Mon, 29 Dec 2014 16:15:41 +1100 Subject: [PATCH 293/652] mpu6000: added factory self-test function this follows the factory calibration self-test method in the datasheet to see if the sensor still has the same calibration it had in the factory --- src/drivers/mpu6000/mpu6000.cpp | 226 +++++++++++++++++++++++++++++--- 1 file changed, 208 insertions(+), 18 deletions(-) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index eb9666dc5a..2559c73e05 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -113,6 +113,10 @@ #define MPUREG_FIFO_COUNTL 0x73 #define MPUREG_FIFO_R_W 0x74 #define MPUREG_PRODUCT_ID 0x0C +#define MPUREG_TRIM1 0x0D +#define MPUREG_TRIM2 0x0E +#define MPUREG_TRIM3 0x0F +#define MPUREG_TRIM4 0x10 // Configuration bits MPU 3000 and MPU 6000 (not revised)? #define BIT_SLEEP 0x40 @@ -121,6 +125,9 @@ #define MPU_CLK_SEL_PLLGYROX 0x01 #define MPU_CLK_SEL_PLLGYROZ 0x03 #define MPU_EXT_SYNC_GYROX 0x02 +#define BITS_GYRO_ST_X 0x80 +#define BITS_GYRO_ST_Y 0x40 +#define BITS_GYRO_ST_Z 0x20 #define BITS_FS_250DPS 0x00 #define BITS_FS_500DPS 0x08 #define BITS_FS_1000DPS 0x10 @@ -196,6 +203,13 @@ public: void print_registers(); + /** + * Test behaviour against factory offsets + * + * @return 0 on success, 1 on failure + */ + int factory_self_test(); + protected: virtual int probe(); @@ -254,6 +268,10 @@ private: uint8_t _checked_values[MPU6000_NUM_CHECKED_REGISTERS]; uint8_t _checked_next; + // use this to avoid processing measurements when in factory + // self test + volatile bool _in_factory_test; + /** * Start automatic measurement. */ @@ -375,6 +393,24 @@ private: /* do not allow to copy this class due to pointer data members */ MPU6000(const MPU6000&); MPU6000 operator=(const MPU6000&); + +#pragma pack(push, 1) + /** + * Report conversation within the MPU6000, including command byte and + * interrupt status. + */ + struct MPUReport { + uint8_t cmd; + uint8_t status; + uint8_t accel_x[2]; + uint8_t accel_y[2]; + uint8_t accel_z[2]; + uint8_t temp[2]; + uint8_t gyro_x[2]; + uint8_t gyro_y[2]; + uint8_t gyro_z[2]; + }; +#pragma pack(pop) }; /* @@ -454,7 +490,8 @@ MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev _gyro_filter_y(MPU6000_GYRO_DEFAULT_RATE, MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ), _gyro_filter_z(MPU6000_GYRO_DEFAULT_RATE, MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ), _rotation(rotation), - _checked_next(0) + _checked_next(0), + _in_factory_test(false) { // disable debug() calls _debug_enabled = false; @@ -889,6 +926,152 @@ MPU6000::gyro_self_test() return 0; } + +/* + perform a self-test comparison to factory trim values. This takes + about 200ms and will return OK if the current values are within 14% + of the expected values + */ +int +MPU6000::factory_self_test() +{ + _in_factory_test = true; + uint8_t saved_gyro_config = read_reg(MPUREG_GYRO_CONFIG); + uint8_t saved_accel_config = read_reg(MPUREG_ACCEL_CONFIG); + const uint16_t repeats = 100; + int ret = OK; + + // gyro self test has to be done at 250DPS + write_reg(MPUREG_GYRO_CONFIG, BITS_FS_250DPS); + + struct MPUReport mpu_report; + float accel_baseline[3]; + float gyro_baseline[3]; + float accel[3]; + float gyro[3]; + float accel_ftrim[3]; + float gyro_ftrim[3]; + + // get baseline values without self-test enabled + set_frequency(MPU6000_HIGH_BUS_SPEED); + + memset(accel_baseline, 0, sizeof(accel_baseline)); + memset(gyro_baseline, 0, sizeof(gyro_baseline)); + memset(accel, 0, sizeof(accel)); + memset(gyro, 0, sizeof(gyro)); + + for (uint8_t i=0; i>3)&0x1C) | ((trims[3]>>4)&0x03); + atrim[1] = ((trims[1]>>3)&0x1C) | ((trims[3]>>2)&0x03); + atrim[2] = ((trims[2]>>3)&0x1C) | ((trims[3]>>0)&0x03); + gtrim[0] = trims[0] & 0x1F; + gtrim[1] = trims[1] & 0x1F; + gtrim[2] = trims[2] & 0x1F; + + // convert factory trims to right units + for (uint8_t i=0; i<3; i++) { + accel_ftrim[i] = 4096 * 0.34f * powf(0.92f/0.34f, (atrim[i]-1)/30.0f); + gyro_ftrim[i] = 25 * 131.0f * powf(1.046f, gtrim[i]-1); + } + // Y gyro trim is negative + gyro_ftrim[1] *= -1; + + for (uint8_t i=0; i<3; i++) { + float diff = accel[i]-accel_baseline[i]; + float err = 100*(diff - accel_ftrim[i]) / accel_ftrim[i]; + ::printf("ACCEL[%u] baseline=%d accel=%d diff=%d ftrim=%d err=%d\n", + (unsigned)i, + (int)(1000*accel_baseline[i]), + (int)(1000*accel[i]), + (int)(1000*diff), + (int)(1000*accel_ftrim[i]), + (int)err); + if (fabsf(err) > 14) { + ::printf("FAIL\n"); + ret = -EIO; + } + } + for (uint8_t i=0; i<3; i++) { + float diff = gyro[i]-gyro_baseline[i]; + float err = 100*(diff - gyro_ftrim[i]) / gyro_ftrim[i]; + ::printf("GYRO[%u] baseline=%d gyro=%d diff=%d ftrim=%d err=%d\n", + (unsigned)i, + (int)(1000*gyro_baseline[i]), + (int)(1000*gyro[i]), + (int)(1000*(gyro[i]-gyro_baseline[i])), + (int)(1000*gyro_ftrim[i]), + (int)err); + if (fabsf(err) > 14) { + ::printf("FAIL\n"); + ret = -EIO; + } + } + + write_reg(MPUREG_GYRO_CONFIG, saved_gyro_config); + write_reg(MPUREG_ACCEL_CONFIG, saved_accel_config); + + _in_factory_test = false; + + return ret; +} + ssize_t MPU6000::gyro_read(struct file *filp, char *buffer, size_t buflen) { @@ -1329,24 +1512,12 @@ MPU6000::check_registers(void) void MPU6000::measure() { -#pragma pack(push, 1) - /** - * Report conversation within the MPU6000, including command byte and - * interrupt status. - */ - struct MPUReport { - uint8_t cmd; - uint8_t status; - uint8_t accel_x[2]; - uint8_t accel_y[2]; - uint8_t accel_z[2]; - uint8_t temp[2]; - uint8_t gyro_x[2]; - uint8_t gyro_y[2]; - uint8_t gyro_z[2]; - } mpu_report; -#pragma pack(pop) + if (_in_factory_test) { + // don't publish any data while in factory test mode + return; + } + struct MPUReport mpu_report; struct Report { int16_t accel_x; int16_t accel_y; @@ -1635,6 +1806,7 @@ void test(bool); void reset(bool); void info(bool); void regdump(bool); +void factorytest(bool); void usage(); /** @@ -1826,6 +1998,21 @@ regdump(bool external_bus) exit(0); } +/** + * Dump the register information + */ +void +factorytest(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"); + + (*g_dev_ptr)->factory_self_test(); + + exit(0); +} + void usage() { @@ -1892,5 +2079,8 @@ mpu6000_main(int argc, char *argv[]) if (!strcmp(verb, "regdump")) mpu6000::regdump(external_bus); + if (!strcmp(verb, "factorytest")) + mpu6000::factorytest(external_bus); + errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info' or 'regdump'"); } From b1574666084b64b43b241a56ac01596035ebccdf Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 29 Dec 2014 20:44:05 +1100 Subject: [PATCH 294/652] mpu6000: monitor some more registers --- src/drivers/mpu6000/mpu6000.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 2559c73e05..91eb40b962 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -263,7 +263,7 @@ private: // this is used to support runtime checking of key // configuration registers to detect SPI bus errors and sensor // reset -#define MPU6000_NUM_CHECKED_REGISTERS 5 +#define MPU6000_NUM_CHECKED_REGISTERS 9 static const uint8_t _checked_registers[MPU6000_NUM_CHECKED_REGISTERS]; uint8_t _checked_values[MPU6000_NUM_CHECKED_REGISTERS]; uint8_t _checked_next; @@ -418,10 +418,14 @@ private: that MPUREG_PRODUCT_ID must be first in the list. */ const uint8_t MPU6000::_checked_registers[MPU6000_NUM_CHECKED_REGISTERS] = { MPUREG_PRODUCT_ID, + MPUREG_PWR_MGMT_1, + MPUREG_CONFIG, MPUREG_GYRO_CONFIG, MPUREG_ACCEL_CONFIG, MPUREG_USER_CTRL, - MPUREG_PWR_MGMT_1}; + MPUREG_INT_ENABLE, + MPUREG_INT_PIN_CFG, + MPUREG_SMPLRT_DIV }; /** From 333039d3db7b1f3b4bfb06c27874bf3372b7d750 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 29 Dec 2014 20:44:46 +1100 Subject: [PATCH 295/652] mpu6000: added "mpu6000 testerror" command used to generate a error case for reset testing --- src/drivers/mpu6000/mpu6000.cpp | 47 +++++++++++++++++++++++++++++++-- 1 file changed, 45 insertions(+), 2 deletions(-) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 91eb40b962..8dfac9859c 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -210,6 +210,9 @@ public: */ int factory_self_test(); + // deliberately cause a sensor error + void test_error(); + protected: virtual int probe(); @@ -1072,10 +1075,31 @@ MPU6000::factory_self_test() write_reg(MPUREG_ACCEL_CONFIG, saved_accel_config); _in_factory_test = false; + if (ret == OK) { + ::printf("PASSED\n"); + } return ret; } + +/* + deliberately trigger an error in the sensor to trigger recovery + */ +void +MPU6000::test_error() +{ + _in_factory_test = true; + // deliberately trigger an error. This was noticed during + // development as a handy way to test the reset logic + uint8_t data[16]; + memset(data, 0, sizeof(data)); + transfer(data, data, sizeof(data)); + ::printf("error triggered\n"); + print_registers(); + _in_factory_test = false; +} + ssize_t MPU6000::gyro_read(struct file *filp, char *buffer, size_t buflen) { @@ -1810,6 +1834,7 @@ void test(bool); void reset(bool); void info(bool); void regdump(bool); +void testerror(bool); void factorytest(bool); void usage(); @@ -2002,6 +2027,21 @@ regdump(bool external_bus) exit(0); } +/** + * deliberately produce an error to test recovery + */ +void +testerror(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"); + + (*g_dev_ptr)->test_error(); + + exit(0); +} + /** * Dump the register information */ @@ -2020,7 +2060,7 @@ factorytest(bool external_bus) void usage() { - warnx("missing command: try 'start', 'info', 'test', 'reset', 'regdump'"); + warnx("missing command: try 'start', 'info', 'test', 'reset', 'regdump', 'factorytest', 'testerror'"); warnx("options:"); warnx(" -X (external bus)"); warnx(" -R rotation"); @@ -2086,5 +2126,8 @@ mpu6000_main(int argc, char *argv[]) if (!strcmp(verb, "factorytest")) mpu6000::factorytest(external_bus); - errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info' or 'regdump'"); + if (!strcmp(verb, "testerror")) + mpu6000::testerror(external_bus); + + errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info', 'regdump', 'factorytest' or 'testerror'"); } From e537de20e3f06525ce9900109a0574142d3e6833 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 29 Dec 2014 20:45:31 +1100 Subject: [PATCH 296/652] mpu6000: wait for 10ms after a full reset this prevents the mpu6000 getting in a really weird state! --- src/drivers/mpu6000/mpu6000.cpp | 31 ++++++++++++++++++++++++------- 1 file changed, 24 insertions(+), 7 deletions(-) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 8dfac9859c..558006d0df 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -253,6 +253,7 @@ private: perf_counter_t _reset_retries; uint8_t _register_wait; + uint64_t _reset_wait; math::LowPassFilter2p _accel_filter_x; math::LowPassFilter2p _accel_filter_y; @@ -490,6 +491,7 @@ MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev _good_transfers(perf_alloc(PC_COUNT, "mpu6000_good_transfers")), _reset_retries(perf_alloc(PC_COUNT, "mpu6000_reset_retries")), _register_wait(0), + _reset_wait(0), _accel_filter_x(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _accel_filter_y(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _accel_filter_z(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), @@ -749,9 +751,9 @@ int MPU6000::reset() usleep(1000); // INT CFG => Interrupt on Data Ready - write_reg(MPUREG_INT_ENABLE, BIT_RAW_RDY_EN); // INT: Raw data ready + write_checked_reg(MPUREG_INT_ENABLE, BIT_RAW_RDY_EN); // INT: Raw data ready usleep(1000); - write_reg(MPUREG_INT_PIN_CFG, BIT_INT_ANYRD_2CLEAR); // INT: Clear on any read + write_checked_reg(MPUREG_INT_PIN_CFG, BIT_INT_ANYRD_2CLEAR); // INT: Clear on any read usleep(1000); // Oscillator set @@ -800,7 +802,7 @@ MPU6000::_set_sample_rate(uint16_t desired_sample_rate_hz) uint8_t div = 1000 / desired_sample_rate_hz; if(div>200) div=200; if(div<1) div=1; - write_reg(MPUREG_SMPLRT_DIV, div-1); + write_checked_reg(MPUREG_SMPLRT_DIV, div-1); _sample_rate = 1000 / div; } @@ -937,7 +939,7 @@ MPU6000::gyro_self_test() /* perform a self-test comparison to factory trim values. This takes about 200ms and will return OK if the current values are within 14% - of the expected values + of the expected values (as per datasheet) */ int MPU6000::factory_self_test() @@ -1518,10 +1520,20 @@ MPU6000::check_registers(void) /* try to fix the bad register value. We only try to fix one per loop to prevent a bad sensor hogging the - bus. We skip zero as that is the PRODUCT_ID, which - is not writeable + bus. */ - if (_checked_next != 0) { + if (_checked_next == 0) { + // if the product_id is wrong then reset the + // sensor completely + write_reg(MPUREG_PWR_MGMT_1, BIT_H_RESET); + // after doing a reset we need to wait a long + // time before we do any other register writes + // or we will end up with the mpu6000 in a + // bizarre state where it has all correct + // register values but large offsets on the + // accel axes + _reset_wait = hrt_absolute_time() + 10000; + } else { write_reg(_checked_registers[_checked_next], _checked_values[_checked_next]); } #if 0 @@ -1545,6 +1557,11 @@ MPU6000::measure() return; } + if (hrt_absolute_time() < _reset_wait) { + // we're waiting for a reset to complete + return; + } + struct MPUReport mpu_report; struct Report { int16_t accel_x; From 4a81384b2d001e5a777a05b6f4c995ea35da2451 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 30 Dec 2014 08:41:00 +1100 Subject: [PATCH 297/652] mpu6000: make register fixup much closer to a reset() this may help automatic reset on the faulty boards --- src/drivers/mpu6000/mpu6000.cpp | 42 +++++++++++++++++++++++++++++---- 1 file changed, 37 insertions(+), 5 deletions(-) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 558006d0df..e7e88bb82b 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -254,6 +254,7 @@ private: uint8_t _register_wait; uint64_t _reset_wait; + uint64_t _printf_wait; math::LowPassFilter2p _accel_filter_x; math::LowPassFilter2p _accel_filter_y; @@ -423,13 +424,14 @@ private: */ const uint8_t MPU6000::_checked_registers[MPU6000_NUM_CHECKED_REGISTERS] = { MPUREG_PRODUCT_ID, MPUREG_PWR_MGMT_1, - MPUREG_CONFIG, + MPUREG_USER_CTRL, + MPUREG_SMPLRT_DIV, + MPUREG_CONFIG, MPUREG_GYRO_CONFIG, MPUREG_ACCEL_CONFIG, - MPUREG_USER_CTRL, MPUREG_INT_ENABLE, - MPUREG_INT_PIN_CFG, - MPUREG_SMPLRT_DIV }; + MPUREG_INT_PIN_CFG }; + /** @@ -492,6 +494,7 @@ MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev _reset_retries(perf_alloc(PC_COUNT, "mpu6000_reset_retries")), _register_wait(0), _reset_wait(0), + _printf_wait(0), _accel_filter_x(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _accel_filter_y(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _accel_filter_z(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), @@ -1522,7 +1525,7 @@ MPU6000::check_registers(void) fix one per loop to prevent a bad sensor hogging the bus. */ - if (_checked_next == 0) { + if (_register_wait == 0 || _checked_next == 0) { // if the product_id is wrong then reset the // sensor completely write_reg(MPUREG_PWR_MGMT_1, BIT_H_RESET); @@ -1533,8 +1536,37 @@ MPU6000::check_registers(void) // register values but large offsets on the // accel axes _reset_wait = hrt_absolute_time() + 10000; + _checked_next = 0; } else { write_reg(_checked_registers[_checked_next], _checked_values[_checked_next]); + _reset_wait = hrt_absolute_time() + 1000; + if (_checked_next == 1 && hrt_absolute_time() > _printf_wait) { + /* + rather bizarrely this printf() seems + to be critical for successfully + resetting the sensor. If you take + this printf out then all the + registers do get successfully reset, + but the sensor ends up with a large + fixed offset on the + accelerometers. Try a up_udelay() + of various sizes instead of a + printf() doesn't work. That makes no + sense at all, and investigating why + this is would be worthwhile. + + The rate limit on the printf to 5Hz + prevents this from causing enough + delays in interrupt context to cause + the vehicle to not be able to fly + correctly. + */ + _printf_wait = hrt_absolute_time() + 200*1000UL; + ::printf("Setting %u %02x to %02x\n", + (unsigned)_checked_next, + (unsigned)_checked_registers[_checked_next], + (unsigned)_checked_values[_checked_next]); + } } #if 0 if (_register_wait == 0) { From b455d647b264ebe3390bbc1a3077de3117c1a1dd Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 30 Dec 2014 13:14:29 +1100 Subject: [PATCH 298/652] l3gd20: added "l3gd20 regdump" command --- src/drivers/l3gd20/l3gd20.cpp | 43 +++++++++++++++++++++++++++++++++-- 1 file changed, 41 insertions(+), 2 deletions(-) diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index de9d9140f0..63974a7df0 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -200,6 +200,9 @@ public: */ void print_info(); + // print register dump + void print_registers(); + protected: virtual int probe(); @@ -1074,6 +1077,20 @@ L3GD20::print_info() } } +void +L3GD20::print_registers() +{ + printf("L3GD20 registers\n"); + for (uint8_t reg=0; reg<=0x40; reg++) { + uint8_t v = read_reg(reg); + printf("%02x:%02x ",(unsigned)reg, (unsigned)v); + if ((reg+1) % 16 == 0) { + printf("\n"); + } + } + printf("\n"); +} + int L3GD20::self_test() { @@ -1109,6 +1126,7 @@ void start(bool external_bus, enum Rotation rotation); void test(); void reset(); void info(); +void regdump(); /** * Start the driver. @@ -1247,10 +1265,25 @@ info() exit(0); } +/** + * Dump the register information + */ +void +regdump(void) +{ + if (g_dev == nullptr) + errx(1, "driver not running"); + + printf("regdump @ %p\n", g_dev); + g_dev->print_registers(); + + exit(0); +} + void usage() { - warnx("missing command: try 'start', 'info', 'test', 'reset'"); + warnx("missing command: try 'start', 'info', 'test', 'reset' or 'regdump'"); warnx("options:"); warnx(" -X (external bus)"); warnx(" -R rotation"); @@ -1307,5 +1340,11 @@ l3gd20_main(int argc, char *argv[]) if (!strcmp(verb, "info")) l3gd20::info(); - errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'"); + /* + * Print register information. + */ + if (!strcmp(verb, "regdump")) + l3gd20::regdump(); + + errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info' or 'regdump'"); } From a8cea3a4da89457b3a8fc0fbb424a6af055453f0 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 30 Dec 2014 16:07:47 +1100 Subject: [PATCH 299/652] l3gd20: use the I2C disable bit on l3gd20H this seems to prevent a mpu6000 reset from causing an issue on the l3gd20H --- src/drivers/l3gd20/l3gd20.cpp | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 63974a7df0..3a0c05c9e2 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -142,6 +142,7 @@ static const int ERROR = -1; #define ADDR_INT1_TSH_ZH 0x36 #define ADDR_INT1_TSH_ZL 0x37 #define ADDR_INT1_DURATION 0x38 +#define ADDR_LOW_ODR 0x39 /* Internal configuration values */ @@ -244,7 +245,7 @@ private: // this is used to support runtime checking of key // configuration registers to detect SPI bus errors and sensor // reset -#define L3GD20_NUM_CHECKED_REGISTERS 7 +#define L3GD20_NUM_CHECKED_REGISTERS 8 static const uint8_t _checked_registers[L3GD20_NUM_CHECKED_REGISTERS]; uint8_t _checked_values[L3GD20_NUM_CHECKED_REGISTERS]; uint8_t _checked_next; @@ -375,7 +376,8 @@ const uint8_t L3GD20::_checked_registers[L3GD20_NUM_CHECKED_REGISTERS] = { ADDR_ ADDR_CTRL_REG3, ADDR_CTRL_REG4, ADDR_CTRL_REG5, - ADDR_FIFO_CTRL_REG }; + ADDR_FIFO_CTRL_REG, + ADDR_LOW_ODR }; L3GD20::L3GD20(int bus, const char* path, spi_dev_e device, enum Rotation rotation) : SPI("L3GD20", path, bus, device, SPIDEV_MODE3, 11*1000*1000 /* will be rounded to 10.4 MHz, within margins for L3GD20 */), @@ -844,6 +846,11 @@ L3GD20::disable_i2c(void) uint8_t a = read_reg(0x05); write_reg(0x05, (0x20 | a)); if (read_reg(0x05) == (a | 0x20)) { + // this sets the I2C_DIS bit on the + // L3GD20H. The l3gd20 datasheet doesn't + // mention this register, but it does seem to + // accept it. + write_checked_reg(ADDR_LOW_ODR, 0x08); return; } } From e0396ffab7df6fdc89b95db2d964dd4e6e911d18 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 31 Dec 2014 08:31:07 +0100 Subject: [PATCH 300/652] mavlink tests module: -Wno-attributes -Wno-packed This was introduced in ca97d0156c07ad6cc09e4623140a7f47214946f5 / pull 1542 for the mavlink module --- src/modules/mavlink/mavlink_tests/module.mk | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_tests/module.mk b/src/modules/mavlink/mavlink_tests/module.mk index 1cc28cce15..b46d2bd355 100644 --- a/src/modules/mavlink/mavlink_tests/module.mk +++ b/src/modules/mavlink/mavlink_tests/module.mk @@ -47,4 +47,6 @@ MODULE_STACKSIZE = 5000 MAXOPTIMIZATION = -Os -EXTRACXXFLAGS = -Weffc++ -DMAVLINK_FTP_UNIT_TEST +EXTRACXXFLAGS = -Weffc++ -DMAVLINK_FTP_UNIT_TEST -Wno-attributes -Wno-packed + +EXTRACFLAGS = -Wno-packed From 6bc67396767adb50773620ab3faf10b50c655844 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 4 Dec 2014 07:41:59 +1100 Subject: [PATCH 301/652] makefiles: removed stray spaces --- makefiles/toolchain_gnu-arm-eabi.mk | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/makefiles/toolchain_gnu-arm-eabi.mk b/makefiles/toolchain_gnu-arm-eabi.mk index 7119c723b0..60db0e8a4d 100644 --- a/makefiles/toolchain_gnu-arm-eabi.mk +++ b/makefiles/toolchain_gnu-arm-eabi.mk @@ -109,10 +109,10 @@ ARCHOPTIMIZATION = $(MAXOPTIMIZATION) \ -fno-strict-aliasing \ -fno-strength-reduce \ -fomit-frame-pointer \ - -funsafe-math-optimizations \ - -fno-builtin-printf \ - -ffunction-sections \ - -fdata-sections + -funsafe-math-optimizations \ + -fno-builtin-printf \ + -ffunction-sections \ + -fdata-sections # enable precise stack overflow tracking # note - requires corresponding support in NuttX From c9e795156f2dd60431d10d253e29a8802c7ece22 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 4 Dec 2014 07:42:30 +1100 Subject: [PATCH 302/652] makefiles: make it easier to use ccache for build on windows --- makefiles/toolchain_gnu-arm-eabi.mk | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/makefiles/toolchain_gnu-arm-eabi.mk b/makefiles/toolchain_gnu-arm-eabi.mk index 60db0e8a4d..d7e48f03be 100644 --- a/makefiles/toolchain_gnu-arm-eabi.mk +++ b/makefiles/toolchain_gnu-arm-eabi.mk @@ -228,7 +228,7 @@ DEP_INCLUDES = $(subst .o,.d,$(OBJS)) define COMPILE @$(ECHO) "CC: $1" @$(MKDIR) -p $(dir $2) - $(Q) $(CC) -MD -c $(CFLAGS) $(abspath $1) -o $2 + $(Q) $(CCACHE) $(CC) -MD -c $(CFLAGS) $(abspath $1) -o $2 endef # Compile C++ source $1 to $2 @@ -237,7 +237,7 @@ endef define COMPILEXX @$(ECHO) "CXX: $1" @$(MKDIR) -p $(dir $2) - $(Q) $(CXX) -MD -c $(CXXFLAGS) $(abspath $1) -o $2 + $(Q) $(CCACHE) $(CXX) -MD -c $(CXXFLAGS) $(abspath $1) -o $2 endef # Assemble $1 into $2 From 72eafad5104ca0919f822fe44391c69f1ca80e8c Mon Sep 17 00:00:00 2001 From: tumbili Date: Wed, 31 Dec 2014 16:25:15 +0100 Subject: [PATCH 303/652] introduced vtol_fw_permanent stabilization: allows vtol to be attitude-stabilized in manual mode --- src/modules/commander/commander.cpp | 6 +++--- src/modules/uORB/topics/vehicle_status.h | 2 ++ src/modules/uORB/topics/vtol_vehicle_status.h | 1 + .../vtol_att_control/vtol_att_control_main.cpp | 12 ++++++++++++ .../vtol_att_control/vtol_att_control_params.c | 12 ++++++++++++ 5 files changed, 30 insertions(+), 3 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index dc0594bf2d..086f291f68 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1267,7 +1267,7 @@ int commander_thread_main(int argc, char *argv[]) if (updated) { /* vtol status changed */ orb_copy(ORB_ID(vtol_vehicle_status), vtol_vehicle_status_sub, &vtol_status); - + status.vtol_fw_permanent_stab = vtol_status.fw_permanent_stab; /* 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; @@ -2246,8 +2246,8 @@ set_control_mode() case NAVIGATION_STATE_MANUAL: control_mode.flag_control_manual_enabled = true; control_mode.flag_control_auto_enabled = false; - control_mode.flag_control_rates_enabled = status.is_rotary_wing; - control_mode.flag_control_attitude_enabled = status.is_rotary_wing; + control_mode.flag_control_rates_enabled = (status.is_rotary_wing || status.vtol_fw_permanent_stab); + control_mode.flag_control_attitude_enabled = (status.is_rotary_wing || status.vtol_fw_permanent_stab); control_mode.flag_control_altitude_enabled = false; control_mode.flag_control_climb_rate_enabled = false; control_mode.flag_control_position_enabled = false; diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index b4d5c7b884..b56e81e04e 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -189,6 +189,8 @@ struct vehicle_status_s { this is only true while flying as a multicopter */ bool is_vtol; /**< True if the system is VTOL capable */ + bool vtol_fw_permanent_stab; /**< True if vtol should stabilize attitude for fw in manual mode */ + bool condition_battery_voltage_valid; bool condition_system_in_air_restore; /**< true if we can restore in mid air */ bool condition_system_sensors_initialized; diff --git a/src/modules/uORB/topics/vtol_vehicle_status.h b/src/modules/uORB/topics/vtol_vehicle_status.h index 24ecca9faa..7b4e22bc88 100644 --- a/src/modules/uORB/topics/vtol_vehicle_status.h +++ b/src/modules/uORB/topics/vtol_vehicle_status.h @@ -54,6 +54,7 @@ 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 */ + bool fw_permanent_stab; /**< In fw mode stabilize attitude even if in manual mode*/ }; /** 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 9a80562f3e..10a3950acb 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -134,6 +134,7 @@ private: struct { param_t idle_pwm_mc; //pwm value for idle in mc mode param_t vtol_motor_count; + param_t vtol_fw_permanent_stab; // in fw mode stabilize attitude also in manual mode 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 @@ -142,6 +143,7 @@ private: struct { param_t idle_pwm_mc; param_t vtol_motor_count; + param_t vtol_fw_permanent_stab; param_t mc_airspeed_min; param_t mc_airspeed_trim; param_t mc_airspeed_max; @@ -234,9 +236,11 @@ VtolAttitudeControl::VtolAttitudeControl() : _params.idle_pwm_mc = PWM_LOWEST_MIN; _params.vtol_motor_count = 0; + _params.vtol_fw_permanent_stab = 0; _params_handles.idle_pwm_mc = param_find("VT_IDLE_PWM_MC"); _params_handles.vtol_motor_count = param_find("VT_MOT_COUNT"); + _params_handles.vtol_fw_permanent_stab = param_find("VT_FW_PERM_STAB"); _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"); @@ -411,6 +415,9 @@ VtolAttitudeControl::parameters_update() /* vtol motor count */ param_get(_params_handles.vtol_motor_count, &_params.vtol_motor_count); + /* vtol fw permanent stabilization */ + param_get(_params_handles.vtol_fw_permanent_stab, &_params.vtol_fw_permanent_stab); + /* vtol mc mode min airspeed */ param_get(_params_handles.mc_airspeed_min, &v); _params.mc_airspeed_min = v; @@ -597,6 +604,9 @@ void VtolAttitudeControl::task_main() parameters_update(); // initialize parameter cache + /* update vtol vehicle status*/ + _vtol_vehicle_status.fw_permanent_stab = _params.vtol_fw_permanent_stab == 1 ? true : false; + // make sure we start with idle in mc mode set_idle_mc(); flag_idle_mc = true; @@ -647,6 +657,8 @@ void VtolAttitudeControl::task_main() parameters_update(); } + _vtol_vehicle_status.fw_permanent_stab = _params.vtol_fw_permanent_stab == 1 ? true : false; + 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. 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 e21bccb0c7..0ab4463171 100644 --- a/src/modules/vtol_att_control/vtol_att_control_params.c +++ b/src/modules/vtol_att_control/vtol_att_control_params.c @@ -86,3 +86,15 @@ PARAM_DEFINE_FLOAT(VT_MC_ARSPD_MAX,30.0f); */ PARAM_DEFINE_FLOAT(VT_MC_ARSPD_TRIM,10.0f); +/** + * Permanent stabilization in fw mode + * + * If set to one this parameter will cause permanent attitude stabilization in fw mode. + * This parameter has been introduced for pure convenience sake. + * + * @min 0.0 + * @max 1.0 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_INT32(VT_FW_PERM_STAB,0); + From 353ea9beacc5f670bb82a6dc234fcaf7c4c0f183 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 1 Jan 2015 08:42:45 +1100 Subject: [PATCH 304/652] l3gd20: added testerror command useful for testing error handling --- src/drivers/l3gd20/l3gd20.cpp | 35 +++++++++++++++++++++++++++++++++-- 1 file changed, 33 insertions(+), 2 deletions(-) diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 3a0c05c9e2..e5ace48c94 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -204,6 +204,9 @@ public: // print register dump void print_registers(); + // trigger an error + void test_error(); + protected: virtual int probe(); @@ -1098,6 +1101,13 @@ L3GD20::print_registers() printf("\n"); } +void +L3GD20::test_error() +{ + // trigger a deliberate error + write_reg(ADDR_CTRL_REG3, 0); +} + int L3GD20::self_test() { @@ -1287,10 +1297,25 @@ regdump(void) exit(0); } +/** + * trigger an error + */ +void +test_error(void) +{ + if (g_dev == nullptr) + errx(1, "driver not running"); + + printf("regdump @ %p\n", g_dev); + g_dev->test_error(); + + exit(0); +} + void usage() { - warnx("missing command: try 'start', 'info', 'test', 'reset' or 'regdump'"); + warnx("missing command: try 'start', 'info', 'test', 'reset', 'testerror' or 'regdump'"); warnx("options:"); warnx(" -X (external bus)"); warnx(" -R rotation"); @@ -1353,5 +1378,11 @@ l3gd20_main(int argc, char *argv[]) if (!strcmp(verb, "regdump")) l3gd20::regdump(); - errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info' or 'regdump'"); + /* + * trigger an error + */ + if (!strcmp(verb, "testerror")) + l3gd20::test_error(); + + errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info', 'testerror' or 'regdump'"); } From 8db206615183a4bd8064734c78554fffab5c1c6d Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 1 Jan 2015 08:43:13 +1100 Subject: [PATCH 305/652] l3gd20: removed printf in interrupt context this is not safe --- src/drivers/l3gd20/l3gd20.cpp | 8 -------- 1 file changed, 8 deletions(-) diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index e5ace48c94..71a54c0f93 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -924,14 +924,6 @@ L3GD20::check_registers(void) if (_checked_next != 0) { write_reg(_checked_registers[_checked_next], _checked_values[_checked_next]); } -#if 1 - if (_register_wait == 0) { - ::printf("L3GD20: %02x:%02x should be %02x\n", - (unsigned)_checked_registers[_checked_next], - (unsigned)v, - (unsigned)_checked_values[_checked_next]); - } -#endif _register_wait = 20; } _checked_next = (_checked_next+1) % L3GD20_NUM_CHECKED_REGISTERS; From 1bb3974c8429b2e680d258d37ad42790b608564b Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 1 Jan 2015 08:43:36 +1100 Subject: [PATCH 306/652] l3gd20: check DRDY after check_registers() is called this allows us to recover from an error that disables data ready --- src/drivers/l3gd20/l3gd20.cpp | 23 ++++++++++++----------- 1 file changed, 12 insertions(+), 11 deletions(-) diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 71a54c0f93..5c92350170 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -932,17 +932,6 @@ L3GD20::check_registers(void) void L3GD20::measure() { -#if L3GD20_USE_DRDY - // if the gyro doesn't have any data ready then re-schedule - // for 100 microseconds later. This ensures we don't double - // read a value and then miss the next value - if (_bus == PX4_SPI_BUS_SENSORS && stm32_gpioread(GPIO_EXTI_GYRO_DRDY) == 0) { - perf_count(_reschedules); - hrt_call_delay(&_call, 100); - return; - } -#endif - /* status register and data as read back from the device */ #pragma pack(push, 1) struct { @@ -962,6 +951,18 @@ L3GD20::measure() check_registers(); +#if L3GD20_USE_DRDY + // if the gyro doesn't have any data ready then re-schedule + // for 100 microseconds later. This ensures we don't double + // read a value and then miss the next value + if (_bus == PX4_SPI_BUS_SENSORS && stm32_gpioread(GPIO_EXTI_GYRO_DRDY) == 0) { + perf_count(_reschedules); + hrt_call_delay(&_call, 100); + perf_end(_sample_perf); + return; + } +#endif + /* fetch data from the sensor */ memset(&raw_report, 0, sizeof(raw_report)); raw_report.cmd = ADDR_OUT_TEMP | DIR_READ | ADDR_INCREMENT; From 5c1da70d0c738c0614ea1652a510f126c31369d8 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 1 Jan 2015 08:44:12 +1100 Subject: [PATCH 307/652] l3gd20: fixed reporting of error count --- src/drivers/l3gd20/l3gd20.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 5c92350170..4e2f198b8a 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -993,7 +993,7 @@ L3GD20::measure() * 74 from all measurements centers them around zero. */ report.timestamp = hrt_absolute_time(); - report.error_count = 0; // not recorded + report.error_count = perf_event_count(_bad_registers); switch (_orientation) { From 2ac3a6fcf4ab051b53f635d696a45edac438228e Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 1 Jan 2015 08:44:50 +1100 Subject: [PATCH 308/652] l3m303d: added testerror command useful for testing error handling --- src/drivers/lsm303d/lsm303d.cpp | 36 +++++++++++++++++++++++++++++++-- 1 file changed, 34 insertions(+), 2 deletions(-) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index dbd5c1f4c3..a062b28286 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -237,6 +237,11 @@ public: */ void print_registers(); + /** + * deliberately trigger an error + */ + void test_error(); + protected: virtual int probe(); @@ -1690,6 +1695,13 @@ LSM303D::print_registers() } } +void +LSM303D::test_error() +{ + // trigger an error + write_reg(ADDR_CTRL_REG3, 0); +} + LSM303D_mag::LSM303D_mag(LSM303D *parent) : CDev("LSM303D_mag", LSM303D_DEVICE_PATH_MAG), _parent(parent), @@ -1969,10 +1981,24 @@ regdump() exit(0); } +/** + * trigger an error + */ +void +test_error() +{ + if (g_dev == nullptr) + errx(1, "driver not running\n"); + + g_dev->test_error(); + + exit(0); +} + void usage() { - warnx("missing command: try 'start', 'info', 'test', 'reset', 'regdump'"); + warnx("missing command: try 'start', 'info', 'test', 'reset', 'testerror' or 'regdump'"); warnx("options:"); warnx(" -X (external bus)"); warnx(" -R rotation"); @@ -2035,5 +2061,11 @@ lsm303d_main(int argc, char *argv[]) if (!strcmp(verb, "regdump")) lsm303d::regdump(); - errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info', or 'regdump'"); + /* + * trigger an error + */ + if (!strcmp(verb, "testerror")) + lsm303d::test_error(); + + errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info', 'testerror' or 'regdump'"); } From e4318345f3a105ac769f93f09f6e11cfb7aa0072 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 1 Jan 2015 08:45:25 +1100 Subject: [PATCH 309/652] lsm303d: added two more checked registers these are key for DRDY behaviour --- src/drivers/lsm303d/lsm303d.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index a062b28286..ace2289200 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -297,7 +297,7 @@ private: // this is used to support runtime checking of key // configuration registers to detect SPI bus errors and sensor // reset -#define LSM303D_NUM_CHECKED_REGISTERS 6 +#define LSM303D_NUM_CHECKED_REGISTERS 8 static const uint8_t _checked_registers[LSM303D_NUM_CHECKED_REGISTERS]; uint8_t _checked_values[LSM303D_NUM_CHECKED_REGISTERS]; uint8_t _checked_next; @@ -474,6 +474,8 @@ private: const uint8_t LSM303D::_checked_registers[LSM303D_NUM_CHECKED_REGISTERS] = { ADDR_WHO_AM_I, ADDR_CTRL_REG1, ADDR_CTRL_REG2, + ADDR_CTRL_REG3, + ADDR_CTRL_REG4, ADDR_CTRL_REG5, ADDR_CTRL_REG6, ADDR_CTRL_REG7 }; @@ -708,8 +710,8 @@ LSM303D::reset() /* enable mag */ write_checked_reg(ADDR_CTRL_REG7, REG7_CONT_MODE_M); write_checked_reg(ADDR_CTRL_REG5, REG5_RES_HIGH_M); - write_reg(ADDR_CTRL_REG3, 0x04); // DRDY on ACCEL on INT1 - write_reg(ADDR_CTRL_REG4, 0x04); // DRDY on MAG on INT2 + write_checked_reg(ADDR_CTRL_REG3, 0x04); // DRDY on ACCEL on INT1 + write_checked_reg(ADDR_CTRL_REG4, 0x04); // DRDY on MAG on INT2 accel_set_range(LSM303D_ACCEL_DEFAULT_RANGE_G); accel_set_samplerate(LSM303D_ACCEL_DEFAULT_RATE); From 1de7fab974b21afdc43caaeb6c8ecdf2ac88cc10 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 1 Jan 2015 08:45:51 +1100 Subject: [PATCH 310/652] lsm303d: removed unsafe printf in interrupt context --- src/drivers/lsm303d/lsm303d.cpp | 8 -------- 1 file changed, 8 deletions(-) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index ace2289200..58dcdf410a 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -1429,14 +1429,6 @@ LSM303D::check_registers(void) if (_checked_next != 0) { write_reg(_checked_registers[_checked_next], _checked_values[_checked_next]); } -#if 1 - if (_register_wait == 0) { - ::printf("LSM303D: %02x:%02x should be %02x\n", - (unsigned)_checked_registers[_checked_next], - (unsigned)v, - (unsigned)_checked_values[_checked_next]); - } -#endif _register_wait = 20; } _checked_next = (_checked_next+1) % LSM303D_NUM_CHECKED_REGISTERS; From 083d9e3e698e794640ac216959ba34157609e6e1 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 1 Jan 2015 08:46:13 +1100 Subject: [PATCH 311/652] lsm303d: check DRDY after check_registers() this allows recovery from a state where DRDY is not set --- src/drivers/lsm303d/lsm303d.cpp | 27 ++++++++++++++------------- 1 file changed, 14 insertions(+), 13 deletions(-) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 58dcdf410a..3e5ff1f7ce 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -1437,19 +1437,6 @@ LSM303D::check_registers(void) void LSM303D::measure() { - // if the accel doesn't have any data ready then re-schedule - // for 100 microseconds later. This ensures we don't double - // read a value and then miss the next value. - // Note that DRDY is not available when the lsm303d is - // connected on the external bus -#ifdef GPIO_EXTI_ACCEL_DRDY - if (_bus == PX4_SPI_BUS_SENSORS && stm32_gpioread(GPIO_EXTI_ACCEL_DRDY) == 0) { - perf_count(_accel_reschedules); - hrt_call_delay(&_accel_call, 100); - return; - } -#endif - /* status register and data as read back from the device */ #pragma pack(push, 1) @@ -1469,6 +1456,20 @@ LSM303D::measure() check_registers(); + // if the accel doesn't have any data ready then re-schedule + // for 100 microseconds later. This ensures we don't double + // read a value and then miss the next value. + // Note that DRDY is not available when the lsm303d is + // connected on the external bus +#ifdef GPIO_EXTI_ACCEL_DRDY + if (_bus == PX4_SPI_BUS_SENSORS && stm32_gpioread(GPIO_EXTI_ACCEL_DRDY) == 0) { + perf_count(_accel_reschedules); + hrt_call_delay(&_accel_call, 100); + perf_end(_accel_sample_perf); + return; + } +#endif + if (_register_wait != 0) { // we are waiting for some good transfers before using // the sensor again. From b5f6fedfa81297587c185079b803c85069704c9a Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 1 Jan 2015 08:46:40 +1100 Subject: [PATCH 312/652] lsm303d: show all perf counters in "info" --- src/drivers/lsm303d/lsm303d.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 3e5ff1f7ce..4f4e5f39c4 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -1618,7 +1618,9 @@ LSM303D::print_info() printf("accel reads: %u\n", _accel_read); printf("mag reads: %u\n", _mag_read); perf_print_counter(_accel_sample_perf); + perf_print_counter(_mag_sample_perf); perf_print_counter(_bad_registers); + perf_print_counter(_accel_reschedules); _accel_reports->print_info("accel reports"); _mag_reports->print_info("mag reports"); ::printf("checked_next: %u\n", _checked_next); From 82b9e08e4c2053df2e01a5c6b0576ca73d9a5c0d Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 1 Jan 2015 08:47:07 +1100 Subject: [PATCH 313/652] mpu6000: removed unsafe printf in interrupt context instead delay 3ms between register writes. This seems to give a quite high probability of correctly resetting the sensor, and does still reliably detect the sensor going bad, which is the most important thing in this code --- src/drivers/mpu6000/mpu6000.cpp | 42 ++++----------------------------- 1 file changed, 4 insertions(+), 38 deletions(-) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index e7e88bb82b..6cac28a7db 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -254,7 +254,6 @@ private: uint8_t _register_wait; uint64_t _reset_wait; - uint64_t _printf_wait; math::LowPassFilter2p _accel_filter_x; math::LowPassFilter2p _accel_filter_y; @@ -494,7 +493,6 @@ MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev _reset_retries(perf_alloc(PC_COUNT, "mpu6000_reset_retries")), _register_wait(0), _reset_wait(0), - _printf_wait(0), _accel_filter_x(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _accel_filter_y(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _accel_filter_z(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), @@ -1539,43 +1537,11 @@ MPU6000::check_registers(void) _checked_next = 0; } else { write_reg(_checked_registers[_checked_next], _checked_values[_checked_next]); - _reset_wait = hrt_absolute_time() + 1000; - if (_checked_next == 1 && hrt_absolute_time() > _printf_wait) { - /* - rather bizarrely this printf() seems - to be critical for successfully - resetting the sensor. If you take - this printf out then all the - registers do get successfully reset, - but the sensor ends up with a large - fixed offset on the - accelerometers. Try a up_udelay() - of various sizes instead of a - printf() doesn't work. That makes no - sense at all, and investigating why - this is would be worthwhile. - - The rate limit on the printf to 5Hz - prevents this from causing enough - delays in interrupt context to cause - the vehicle to not be able to fly - correctly. - */ - _printf_wait = hrt_absolute_time() + 200*1000UL; - ::printf("Setting %u %02x to %02x\n", - (unsigned)_checked_next, - (unsigned)_checked_registers[_checked_next], - (unsigned)_checked_values[_checked_next]); - } + // waiting 3ms between register writes seems + // to raise the chance of the sensor + // recovering considerably + _reset_wait = hrt_absolute_time() + 3000; } -#if 0 - if (_register_wait == 0) { - ::printf("MPU6000: %02x:%02x should be %02x\n", - (unsigned)_checked_registers[_checked_next], - (unsigned)v, - (unsigned)_checked_values[_checked_next]); - } -#endif _register_wait = 20; } _checked_next = (_checked_next+1) % MPU6000_NUM_CHECKED_REGISTERS; From 7864958f1c2da5b9a64b92082ee287883479768e Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 1 Jan 2015 10:03:34 +1100 Subject: [PATCH 314/652] l3gd20: fixed build warning --- src/drivers/l3gd20/l3gd20.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 4e2f198b8a..08bc1fead8 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -1137,6 +1137,7 @@ void test(); void reset(); void info(); void regdump(); +void test_error(); /** * Start the driver. From 8470105f9efe57f6abb9f1caa20deb4355d5a7f9 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 1 Jan 2015 10:03:42 +1100 Subject: [PATCH 315/652] lsm303d: fixed build warning --- src/drivers/lsm303d/lsm303d.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 4f4e5f39c4..00484db797 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -1773,6 +1773,7 @@ void reset(); void info(); void regdump(); void usage(); +void test_error(); /** * Start the driver. From 7b6620b319e41abc546b9d6ab4f522a106686b28 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 1 Jan 2015 10:59:42 +1100 Subject: [PATCH 316/652] ll40ls: fixed exit code on external sensor startup failure --- src/drivers/ll40ls/ll40ls.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/drivers/ll40ls/ll40ls.cpp b/src/drivers/ll40ls/ll40ls.cpp index 4676c6ad76..e68f241521 100644 --- a/src/drivers/ll40ls/ll40ls.cpp +++ b/src/drivers/ll40ls/ll40ls.cpp @@ -746,6 +746,9 @@ start(int bus) if (g_dev_ext != nullptr && OK != g_dev_ext->init()) { delete g_dev_ext; g_dev_ext = nullptr; + if (bus == PX4_I2C_BUS_EXPANSION) { + goto fail; + } } } From a7bc38869e2121b7f63e63e0fae4bfe4117a3315 Mon Sep 17 00:00:00 2001 From: tumbili Date: Thu, 1 Jan 2015 14:51:18 +0100 Subject: [PATCH 317/652] added pitch trim for vtol in fixed wing mode --- .../vtol_att_control/vtol_att_control_main.cpp | 9 ++++++++- .../vtol_att_control/vtol_att_control_params.c | 11 +++++++++++ 2 files changed, 19 insertions(+), 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 10a3950acb..c72a85ecd4 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -138,6 +138,7 @@ private: 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 + float fw_pitch_trim; // trim for neutral elevon position in fw mode } _params; struct { @@ -147,6 +148,7 @@ private: param_t mc_airspeed_min; param_t mc_airspeed_trim; param_t mc_airspeed_max; + param_t fw_pitch_trim; } _params_handles; perf_counter_t _loop_perf; /**< loop performance counter */ @@ -244,6 +246,7 @@ VtolAttitudeControl::VtolAttitudeControl() : _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"); + _params_handles.fw_pitch_trim = param_find("VT_FW_PITCH_TRIM"); /* fetch initial parameter values */ parameters_update(); @@ -430,6 +433,10 @@ VtolAttitudeControl::parameters_update() param_get(_params_handles.mc_airspeed_trim, &v); _params.mc_airspeed_trim = v; + /* vtol pitch trim for fw mode */ + param_get(_params_handles.fw_pitch_trim, &v); + _params.fw_pitch_trim = v; + return OK; } @@ -459,7 +466,7 @@ void VtolAttitudeControl::fill_fw_att_control_output() _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[1] = _actuators_fw_in.control[1] + _params.fw_pitch_trim; // 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 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 0ab4463171..d1d4697f3f 100644 --- a/src/modules/vtol_att_control/vtol_att_control_params.c +++ b/src/modules/vtol_att_control/vtol_att_control_params.c @@ -98,3 +98,14 @@ PARAM_DEFINE_FLOAT(VT_MC_ARSPD_TRIM,10.0f); */ PARAM_DEFINE_INT32(VT_FW_PERM_STAB,0); +/** + * Fixed wing pitch trim + * + * This parameter allows to adjust the neutral elevon position in fixed wing mode. + * + * @min -1 + * @max 1 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VT_FW_PITCH_TRIM,0.0f); + From dd8dfc971cb459dbdc849a0f320431516625bfee Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 1 Jan 2015 17:59:31 +0100 Subject: [PATCH 318/652] update mavlink c library --- mavlink/include/mavlink/v1.0 | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mavlink/include/mavlink/v1.0 b/mavlink/include/mavlink/v1.0 index b5732d07ec..42f1a37397 160000 --- a/mavlink/include/mavlink/v1.0 +++ b/mavlink/include/mavlink/v1.0 @@ -1 +1 @@ -Subproject commit b5732d07ec89025a9d321a923fe027d7df7b30a6 +Subproject commit 42f1a37397335b3c4c96b0a41c73d85e80d54a60 From 500ac1443bcfc9082a76b0a2a0a72b0a3d539b9b Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 2 Jan 2015 16:45:08 +1100 Subject: [PATCH 319/652] lsm303d: detect large fixed values and report an error we have logs where the lsm303d gets large fixed values for long periods. This will detect that error case and raise error_count so the higher level sensor integration code can choose another sensor --- src/drivers/lsm303d/lsm303d.cpp | 49 +++++++++++++++++++++++++++++---- 1 file changed, 44 insertions(+), 5 deletions(-) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 00484db797..57754e4c0d 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -285,6 +285,7 @@ private: perf_counter_t _mag_sample_perf; perf_counter_t _accel_reschedules; perf_counter_t _bad_registers; + perf_counter_t _bad_values; uint8_t _register_wait; @@ -294,6 +295,10 @@ private: enum Rotation _rotation; + // values used to + float _last_accel[3]; + uint8_t _constant_accel_count; + // this is used to support runtime checking of key // configuration registers to detect SPI bus errors and sensor // reset @@ -542,11 +547,13 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device, enum Rotation rota _mag_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_mag_read")), _accel_reschedules(perf_alloc(PC_COUNT, "lsm303d_accel_resched")), _bad_registers(perf_alloc(PC_COUNT, "lsm303d_bad_registers")), + _bad_values(perf_alloc(PC_COUNT, "lsm303d_bad_values")), _register_wait(0), _accel_filter_x(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _accel_filter_y(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _accel_filter_z(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _rotation(rotation), + _constant_accel_count(0), _checked_next(0) { _device_id.devid_s.devtype = DRV_MAG_DEVTYPE_LSM303D; @@ -590,6 +597,7 @@ LSM303D::~LSM303D() perf_free(_accel_sample_perf); perf_free(_mag_sample_perf); perf_free(_bad_registers); + perf_free(_bad_values); perf_free(_accel_reschedules); } @@ -1501,11 +1509,11 @@ LSM303D::measure() accel_report.timestamp = hrt_absolute_time(); - // report the error count as the sum of the number of bad bad - // register reads. This allows the higher level code to decide - // if it should use this sensor based on whether it has had - // failures - accel_report.error_count = perf_event_count(_bad_registers); + // report the error count as the sum of the number of bad + // register reads and bad values. This allows the higher level + // code to decide if it should use this sensor based on + // whether it has had failures + accel_report.error_count = perf_event_count(_bad_registers) + perf_event_count(_bad_values); accel_report.x_raw = raw_accel_report.x; accel_report.y_raw = raw_accel_report.y; @@ -1515,6 +1523,36 @@ LSM303D::measure() float y_in_new = ((accel_report.y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; float z_in_new = ((accel_report.z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; + /* + we have logs where the accelerometers get stuck at a fixed + large value. We want to detect this and mark the sensor as + being faulty + */ + if (fabsf(_last_accel[0] - x_in_new) < 0.001f && + fabsf(_last_accel[1] - y_in_new) < 0.001f && + fabsf(_last_accel[2] - z_in_new) < 0.001f && + fabsf(x_in_new) > 20 && + fabsf(y_in_new) > 20 && + fabsf(z_in_new) > 20) { + _constant_accel_count += 1; + } else { + _constant_accel_count = 0; + } + if (_constant_accel_count > 100) { + // we've had 100 constant accel readings with large + // values. The sensor is almost certainly dead. We + // will raise the error_count so that the top level + // flight code will know to avoid this sensor, but + // we'll still give the data so that it can be logged + // and viewed + perf_count(_bad_values); + _constant_accel_count = 0; + } + + _last_accel[0] = x_in_new; + _last_accel[1] = y_in_new; + _last_accel[2] = z_in_new; + accel_report.x = _accel_filter_x.apply(x_in_new); accel_report.y = _accel_filter_y.apply(y_in_new); accel_report.z = _accel_filter_z.apply(z_in_new); @@ -1620,6 +1658,7 @@ LSM303D::print_info() perf_print_counter(_accel_sample_perf); perf_print_counter(_mag_sample_perf); perf_print_counter(_bad_registers); + perf_print_counter(_bad_values); perf_print_counter(_accel_reschedules); _accel_reports->print_info("accel reports"); _mag_reports->print_info("mag reports"); From 2effc9a23d8df3922f9f7cec9b0af9b15b2e2367 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 10 Oct 2014 11:57:46 +0200 Subject: [PATCH 320/652] MS5611: Allow two instances --- src/drivers/ms5611/ms5611.cpp | 60 ++++++++++++++++++++++++++--------- 1 file changed, 45 insertions(+), 15 deletions(-) diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp index 889643d0df..340a2e660d 100644 --- a/src/drivers/ms5611/ms5611.cpp +++ b/src/drivers/ms5611/ms5611.cpp @@ -787,7 +787,9 @@ MS5611::print_info() namespace ms5611 { -MS5611 *g_dev; +/* initialize explicitely for clarity */ +MS5611 *g_dev_ext = nullptr; +MS5611 *g_dev_int = nullptr; void start(bool external_bus); void test(); @@ -852,9 +854,13 @@ start(bool external_bus) int fd; prom_u prom_buf; - if (g_dev != nullptr) + if (external_bus && (g_dev_ext != nullptr)) /* if already started, the still command succeeded */ - errx(0, "already started"); + errx(0, "ext already started"); + + if (g_dev_int != nullptr) + /* if already started, the still command succeeded */ + errx(0, "int already started"); device::Device *interface = nullptr; @@ -872,13 +878,25 @@ start(bool external_bus) errx(1, "interface init failed"); } - g_dev = new MS5611(interface, prom_buf); - if (g_dev == nullptr) { - delete interface; - errx(1, "failed to allocate driver"); + if (external_bus) { + g_dev_ext = new MS5611(interface, prom_buf); + if (g_dev_ext == nullptr) { + delete interface; + errx(1, "failed to allocate driver"); + } + if (g_dev_ext->init() != OK) + goto fail; + } else { + + g_dev_int = new MS5611(interface, prom_buf); + if (g_dev_int == nullptr) { + delete interface; + errx(1, "failed to allocate driver"); + } + if (g_dev_int->init() != OK) + goto fail; + } - if (g_dev->init() != OK) - goto fail; /* set the poll rate to default, starts automatic data collection */ fd = open(MS5611_BARO_DEVICE_PATH, O_RDONLY); @@ -895,9 +913,14 @@ start(bool external_bus) fail: - if (g_dev != nullptr) { - delete g_dev; - g_dev = nullptr; + if (g_dev_int != nullptr) { + delete g_dev_int; + g_dev_int = nullptr; + } + + if (g_dev_ext != nullptr) { + delete g_dev_ext; + g_dev_ext = nullptr; } errx(1, "driver start failed"); @@ -994,11 +1017,18 @@ reset() void info() { - if (g_dev == nullptr) + if (g_dev_ext == nullptr && g_dev_int == nullptr) errx(1, "driver not running"); - printf("state @ %p\n", g_dev); - g_dev->print_info(); + if (g_dev_ext) { + warnx("ext:"); + g_dev_ext->print_info(); + } + + if (g_dev_int) { + warnx("int:"); + g_dev_int->print_info(); + } exit(0); } From bf134e697971194923fa6834c9919550d418665c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 10 Oct 2014 12:11:45 +0200 Subject: [PATCH 321/652] Allow to get access to dev name --- src/drivers/device/device.h | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/src/drivers/device/device.h b/src/drivers/device/device.h index 67aaa0affc..4d4bed8358 100644 --- a/src/drivers/device/device.h +++ b/src/drivers/device/device.h @@ -445,6 +445,13 @@ protected: */ virtual int unregister_class_devname(const char *class_devname, unsigned class_instance); + /** + * Get the device name. + * + * @return the file system string of the device handle + */ + const char* get_devname() { return _devname; } + bool _pub_blocked; /**< true if publishing should be blocked */ private: From a77c9225df3aa494d5db64824926174c95381b97 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 10 Oct 2014 12:12:17 +0200 Subject: [PATCH 322/652] Allow access to both device handles, make external use a special handle --- src/drivers/ms5611/ms5611.cpp | 70 ++++++++++++++++++++++++----------- 1 file changed, 48 insertions(+), 22 deletions(-) diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp index 340a2e660d..beca29d6dc 100644 --- a/src/drivers/ms5611/ms5611.cpp +++ b/src/drivers/ms5611/ms5611.cpp @@ -91,12 +91,13 @@ static const int ERROR = -1; /* internal conversion time: 9.17 ms, so should not be read at rates higher than 100 Hz */ #define MS5611_CONVERSION_INTERVAL 10000 /* microseconds */ #define MS5611_MEASUREMENT_RATIO 3 /* pressure measurements per temperature measurement */ -#define MS5611_BARO_DEVICE_PATH "/dev/ms5611" +#define MS5611_BARO_DEVICE_PATH_EXT "/dev/ms5611_ext" +#define MS5611_BARO_DEVICE_PATH_INT "/dev/ms5611_int" class MS5611 : public device::CDev { public: - MS5611(device::Device *interface, ms5611::prom_u &prom_buf); + MS5611(device::Device *interface, ms5611::prom_u &prom_buf, const char* path); ~MS5611(); virtual int init(); @@ -195,8 +196,8 @@ protected: */ extern "C" __EXPORT int ms5611_main(int argc, char *argv[]); -MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf) : - CDev("MS5611", MS5611_BARO_DEVICE_PATH), +MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf, const char* path) : + CDev("MS5611", path), _interface(interface), _prom(prom_buf.s), _measure_ticks(0), @@ -224,7 +225,7 @@ MS5611::~MS5611() stop_cycle(); if (_class_instance != -1) - unregister_class_devname(MS5611_BARO_DEVICE_PATH, _class_instance); + unregister_class_devname(get_devname(), _class_instance); /* free any existing reports */ if (_reports != nullptr) @@ -792,10 +793,10 @@ MS5611 *g_dev_ext = nullptr; MS5611 *g_dev_int = nullptr; void start(bool external_bus); -void test(); -void reset(); +void test(bool external_bus); +void reset(bool external_bus); void info(); -void calibrate(unsigned altitude); +void calibrate(unsigned altitude, bool external_bus); void usage(); /** @@ -879,27 +880,34 @@ start(bool external_bus) } if (external_bus) { - g_dev_ext = new MS5611(interface, prom_buf); + g_dev_ext = new MS5611(interface, prom_buf, MS5611_BARO_DEVICE_PATH_EXT); if (g_dev_ext == nullptr) { delete interface; errx(1, "failed to allocate driver"); } + if (g_dev_ext->init() != OK) goto fail; + + fd = open(MS5611_BARO_DEVICE_PATH_EXT, O_RDONLY); + } else { - g_dev_int = new MS5611(interface, prom_buf); + g_dev_int = new MS5611(interface, prom_buf, MS5611_BARO_DEVICE_PATH_INT); if (g_dev_int == nullptr) { delete interface; errx(1, "failed to allocate driver"); } + if (g_dev_int->init() != OK) goto fail; + fd = open(MS5611_BARO_DEVICE_PATH_INT, O_RDONLY); + } /* set the poll rate to default, starts automatic data collection */ - fd = open(MS5611_BARO_DEVICE_PATH, O_RDONLY); + if (fd < 0) { warnx("can't open baro device"); goto fail; @@ -932,16 +940,22 @@ fail: * and automatic modes. */ void -test() +test(bool external_bus) { struct baro_report report; ssize_t sz; int ret; - int fd = open(MS5611_BARO_DEVICE_PATH, O_RDONLY); + int fd; + + if (external_bus) { + fd = open(MS5611_BARO_DEVICE_PATH_EXT, O_RDONLY); + } else { + fd = open(MS5611_BARO_DEVICE_PATH_INT, O_RDONLY); + } if (fd < 0) - err(1, "%s open failed (try 'ms5611 start' if the driver is not running)", MS5611_BARO_DEVICE_PATH); + err(1, "open failed (try 'ms5611 start' if the driver is not running)"); /* do a simple demand read */ sz = read(fd, &report, sizeof(report)); @@ -995,9 +1009,15 @@ test() * Reset the driver. */ void -reset() +reset(bool external_bus) { - int fd = open(MS5611_BARO_DEVICE_PATH, O_RDONLY); + int fd; + + if (external_bus) { + fd = open(MS5611_BARO_DEVICE_PATH_EXT, O_RDONLY); + } else { + fd = open(MS5611_BARO_DEVICE_PATH_INT, O_RDONLY); + } if (fd < 0) err(1, "failed "); @@ -1037,16 +1057,22 @@ info() * Calculate actual MSL pressure given current altitude */ void -calibrate(unsigned altitude) +calibrate(unsigned altitude, bool external_bus) { struct baro_report report; float pressure; float p1; - int fd = open(MS5611_BARO_DEVICE_PATH, O_RDONLY); + int fd; + + if (external_bus) { + fd = open(MS5611_BARO_DEVICE_PATH_EXT, O_RDONLY); + } else { + fd = open(MS5611_BARO_DEVICE_PATH_INT, O_RDONLY); + } if (fd < 0) - err(1, "%s open failed (try 'ms5611 start' if the driver is not running)", MS5611_BARO_DEVICE_PATH); + err(1, "open failed (try 'ms5611 start' if the driver is not running)"); /* start the sensor polling at max */ if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX)) @@ -1141,13 +1167,13 @@ ms5611_main(int argc, char *argv[]) * Test the driver/device. */ if (!strcmp(verb, "test")) - ms5611::test(); + ms5611::test(external_bus); /* * Reset the driver. */ if (!strcmp(verb, "reset")) - ms5611::reset(); + ms5611::reset(external_bus); /* * Print driver information. @@ -1164,7 +1190,7 @@ ms5611_main(int argc, char *argv[]) long altitude = strtol(argv[optind+1], nullptr, 10); - ms5611::calibrate(altitude); + ms5611::calibrate(altitude, external_bus); } errx(1, "unrecognised command, try 'start', 'test', 'reset' or 'info'"); From 4c3ebee15b2d3626b328d313b1356634e70ab6d8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 10 Oct 2014 13:29:25 +0200 Subject: [PATCH 323/652] Hackery on option parsing to make MS5611 comply --- src/drivers/ms5611/ms5611.cpp | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp index beca29d6dc..aed72de01f 100644 --- a/src/drivers/ms5611/ms5611.cpp +++ b/src/drivers/ms5611/ms5611.cpp @@ -855,13 +855,13 @@ start(bool external_bus) int fd; prom_u prom_buf; - if (external_bus && (g_dev_ext != nullptr)) + if (external_bus && (g_dev_ext != nullptr)) { /* if already started, the still command succeeded */ errx(0, "ext already started"); - - if (g_dev_int != nullptr) + } else if (!external_bus && (g_dev_int != nullptr)) { /* if already started, the still command succeeded */ errx(0, "int already started"); + } device::Device *interface = nullptr; @@ -1157,6 +1157,12 @@ ms5611_main(int argc, char *argv[]) const char *verb = argv[optind]; + if (argc > optind+1) { + if (!strcmp(argv[optind+1], "-X")) { + external_bus = true; + } + } + /* * Start/load the driver. */ From d638cbda83ccf6d94c8ecf62141642c0a19d3167 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 15 Oct 2014 16:33:46 +0200 Subject: [PATCH 324/652] start MS5611 in parallel --- 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 973db9017d..ad84f44f59 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -52,6 +52,10 @@ then if lsm303d start then fi + + if ms5611 -X start + then + fi fi if meas_airspeed start From d20ebd739045c3fb1fb215c549707618f0b2554b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 15 Oct 2014 21:49:52 +0200 Subject: [PATCH 325/652] Add logging for 2nd baro Conflicts: src/modules/sdlog2/sdlog2_messages.h --- src/modules/sdlog2/sdlog2.c | 18 ++++++++++++++++++ src/modules/sdlog2/sdlog2_messages.h | 6 +++++- 2 files changed, 23 insertions(+), 1 deletion(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 0a8564da6a..2daf73bf9e 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1112,6 +1112,7 @@ int sdlog2_thread_main(int argc, char *argv[]) hrt_abstime magnetometer_timestamp = 0; hrt_abstime barometer_timestamp = 0; hrt_abstime differential_pressure_timestamp = 0; + hrt_abstime barometer1_timestamp = 0; hrt_abstime gyro1_timestamp = 0; hrt_abstime accelerometer1_timestamp = 0; hrt_abstime magnetometer1_timestamp = 0; @@ -1257,6 +1258,7 @@ int sdlog2_thread_main(int argc, char *argv[]) bool write_IMU1 = false; bool write_IMU2 = false; bool write_SENS = false; + bool write_SENS1 = false; if (buf.sensor.timestamp != gyro_timestamp) { gyro_timestamp = buf.sensor.timestamp; @@ -1307,6 +1309,22 @@ int sdlog2_thread_main(int argc, char *argv[]) LOGBUFFER_WRITE_AND_COUNT(SENS); } + if (buf.sensor.baro1_timestamp != barometer1_timestamp) { + barometer1_timestamp = buf.sensor.baro1_timestamp; + write_SENS1 = true; + } + + if (write_SENS1) { + log_msg.msg_type = LOG_AIR1_MSG; + log_msg.body.log_SENS.baro_pres = buf.sensor.baro1_pres_mbar; + log_msg.body.log_SENS.baro_alt = buf.sensor.baro1_alt_meter; + log_msg.body.log_SENS.baro_temp = buf.sensor.baro1_temp_celcius; + log_msg.body.log_SENS.diff_pres = buf.sensor.differential_pressure1_pa; + log_msg.body.log_SENS.diff_pres_filtered = buf.sensor.differential_pressure1_filtered_pa; + // XXX moving to AIR0-AIR2 instead of SENS + LOGBUFFER_WRITE_AND_COUNT(SENS); + } + if (buf.sensor.accelerometer1_timestamp != accelerometer1_timestamp) { accelerometer1_timestamp = buf.sensor.accelerometer1_timestamp; write_IMU1 = true; diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 5b047f5387..5941bfac0e 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -424,6 +424,9 @@ struct log_ENCD_s { float vel1; }; +/* --- AIR SPEED SENSORS - DIFF. PRESSURE --- */ +#define LOG_AIR1_MSG 40 + /********** SYSTEM MESSAGES, ID > 0x80 **********/ /* --- TIME - TIME STAMP --- */ @@ -457,7 +460,8 @@ static const struct log_format_s log_formats[] = { 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"), LOG_FORMAT_S(IMU2, IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"), - LOG_FORMAT(SENS, "fffff", "BaroPres,BaroAlt,BaroTemp,DiffPres,DiffPresFilt"), + LOG_FORMAT_S(SENS, SENS, "fffff", "BaroPres,BaroAlt,BaroTemp,DiffPres,DiffPresFilt"), + LOG_FORMAT_S(AIR1, SENS, "fffff", "BaroPa,BaroAlt,BaroTmp,DiffPres,DiffPresF"), 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"), From 08297db39a909df9b024867eeb23720532f5625f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 15 Oct 2014 21:50:12 +0200 Subject: [PATCH 326/652] Support for 2nd baro --- src/drivers/drv_baro.h | 1 + src/drivers/ms5611/ms5611.cpp | 21 ++++----------------- 2 files changed, 5 insertions(+), 17 deletions(-) diff --git a/src/drivers/drv_baro.h b/src/drivers/drv_baro.h index 248b9a73dc..3e28d3d3d9 100644 --- a/src/drivers/drv_baro.h +++ b/src/drivers/drv_baro.h @@ -65,6 +65,7 @@ struct baro_report { */ ORB_DECLARE(sensor_baro0); ORB_DECLARE(sensor_baro1); +ORB_DECLARE(sensor_baro2); /* * ioctl() definitions diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp index aed72de01f..0a793cbc97 100644 --- a/src/drivers/ms5611/ms5611.cpp +++ b/src/drivers/ms5611/ms5611.cpp @@ -134,6 +134,7 @@ protected: unsigned _msl_pressure; /* in Pa */ orb_advert_t _baro_topic; + orb_id_t _orb_id; int _class_instance; @@ -262,6 +263,7 @@ MS5611::init() /* register alternate interfaces if we have to */ _class_instance = register_class_devname(BARO_DEVICE_PATH); + _orb_id = ORB_ID_TRIPLE(sensor_baro, _class_instance); struct baro_report brp; /* do a first measurement cycle to populate reports with valid data */ @@ -301,14 +303,7 @@ MS5611::init() ret = OK; - switch (_class_instance) { - case CLASS_DEVICE_PRIMARY: - _baro_topic = orb_advertise(ORB_ID(sensor_baro0), &brp); - break; - case CLASS_DEVICE_SECONDARY: - _baro_topic = orb_advertise(ORB_ID(sensor_baro1), &brp); - break; - } + _baro_topic = orb_advertise(_orb_id, &brp); if (_baro_topic < 0) { warnx("failed to create sensor_baro publication"); @@ -730,15 +725,7 @@ MS5611::collect() /* publish it */ if (!(_pub_blocked)) { /* publish it */ - switch (_class_instance) { - case CLASS_DEVICE_PRIMARY: - orb_publish(ORB_ID(sensor_baro0), _baro_topic, &report); - break; - - case CLASS_DEVICE_SECONDARY: - orb_publish(ORB_ID(sensor_baro1), _baro_topic, &report); - break; - } + orb_publish(_orb_id, _baro_topic, &report); } if (_reports->force(&report)) { From f5b6b831617632dfb237f8331e29242774b7c0c1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 15 Oct 2014 21:50:52 +0200 Subject: [PATCH 327/652] Added 2nd baro to sensor combined topic --- src/modules/uORB/topics/sensor_combined.h | 20 +++++++++++++++----- 1 file changed, 15 insertions(+), 5 deletions(-) diff --git a/src/modules/uORB/topics/sensor_combined.h b/src/modules/uORB/topics/sensor_combined.h index 06dfcdab30..583a39ded4 100644 --- a/src/modules/uORB/topics/sensor_combined.h +++ b/src/modules/uORB/topics/sensor_combined.h @@ -122,15 +122,25 @@ struct sensor_combined_s { float baro_pres_mbar; /**< Barometric pressure, already temp. comp. */ float baro_alt_meter; /**< Altitude, already temp. comp. */ float baro_temp_celcius; /**< Temperature in degrees celsius */ - float adc_voltage_v[10]; /**< ADC voltages of ADC Chan 10/11/12/13 or -1 */ - unsigned adc_mapping[10]; /**< Channel indices of each of these values */ - float mcu_temp_celcius; /**< Internal temperature measurement of MCU */ uint64_t baro_timestamp; /**< Barometer timestamp */ - float differential_pressure_pa; /**< Airspeed sensor differential pressure */ - uint64_t differential_pressure_timestamp; /**< Last measurement timestamp */ + float baro1_pres_mbar; /**< Barometric pressure, already temp. comp. */ + float baro1_alt_meter; /**< Altitude, already temp. comp. */ + float baro1_temp_celcius; /**< Temperature in degrees celsius */ + uint64_t baro1_timestamp; /**< Barometer timestamp */ + + float adc_voltage_v[10]; /**< ADC voltages of ADC Chan 10/11/12/13 or -1 */ + unsigned adc_mapping[10]; /**< Channel indices of each of these values */ + float mcu_temp_celcius; /**< Internal temperature measurement of MCU */ + + float differential_pressure_pa; /**< Airspeed sensor differential pressure */ + uint64_t differential_pressure_timestamp; /**< Last measurement timestamp */ float differential_pressure_filtered_pa; /**< Low pass filtered airspeed sensor differential pressure reading */ + float differential_pressure1_pa; /**< Airspeed sensor differential pressure */ + uint64_t differential_pressure1_timestamp; /**< Last measurement timestamp */ + float differential_pressure1_filtered_pa; /**< Low pass filtered airspeed sensor differential pressure reading */ + }; /** From a12ac452a1f8309429d50e1c2477fd15e48a04e6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 15 Oct 2014 21:51:05 +0200 Subject: [PATCH 328/652] Objects common --- src/modules/uORB/objects_common.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index d834c35747..293412455e 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -63,6 +63,7 @@ ORB_DEFINE(sensor_gyro2, struct gyro_report); #include ORB_DEFINE(sensor_baro0, struct baro_report); ORB_DEFINE(sensor_baro1, struct baro_report); +ORB_DEFINE(sensor_baro2, struct baro_report); #include ORB_DEFINE(sensor_range_finder, struct range_finder_report); From 1f81a8bd611e295161b68e188041ab79b4d017db Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 15 Oct 2014 21:52:59 +0200 Subject: [PATCH 329/652] 2nd baro support for common topics --- src/modules/sensors/sensors.cpp | 46 +++++++++++++++++++++++++++++++++ 1 file changed, 46 insertions(+) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 952b0447dc..018eadaff3 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -226,6 +226,7 @@ private: int _mag2_sub; /**< raw mag2 data subscription */ int _rc_sub; /**< raw rc channels data subscription */ int _baro_sub; /**< raw baro data subscription */ + int _baro1_sub; /**< raw baro data subscription */ int _airspeed_sub; /**< airspeed subscription */ int _diff_pres_sub; /**< raw differential pressure subscription */ int _vcontrol_mode_sub; /**< vehicle control mode subscription */ @@ -523,6 +524,7 @@ Sensors::Sensors() : _mag2_sub(-1), _rc_sub(-1), _baro_sub(-1), + _baro1_sub(-1), _vcontrol_mode_sub(-1), _params_sub(-1), _rc_parameter_map_sub(-1), @@ -1258,6 +1260,34 @@ Sensors::mag_poll(struct sensor_combined_s &raw) raw.magnetometer_timestamp = mag_report.timestamp; } + + orb_check(_mag1_sub, &mag_updated); + + if (mag_updated) { + struct mag_report mag_report; + + orb_copy(ORB_ID(sensor_mag1), _mag1_sub, &mag_report); + + raw.magnetometer1_raw[0] = mag_report.x_raw; + raw.magnetometer1_raw[1] = mag_report.y_raw; + raw.magnetometer1_raw[2] = mag_report.z_raw; + + raw.magnetometer1_timestamp = mag_report.timestamp; + } + + orb_check(_mag2_sub, &mag_updated); + + if (mag_updated) { + struct mag_report mag_report; + + orb_copy(ORB_ID(sensor_mag2), _mag2_sub, &mag_report); + + raw.magnetometer2_raw[0] = mag_report.x_raw; + raw.magnetometer2_raw[1] = mag_report.y_raw; + raw.magnetometer2_raw[2] = mag_report.z_raw; + + raw.magnetometer2_timestamp = mag_report.timestamp; + } } void @@ -1276,6 +1306,21 @@ Sensors::baro_poll(struct sensor_combined_s &raw) raw.baro_timestamp = _barometer.timestamp; } + + orb_check(_baro1_sub, &baro_updated); + + if (baro_updated) { + + struct baro_report baro_report; + + orb_copy(ORB_ID(sensor_baro1), _baro1_sub, &baro_report); + + raw.baro1_pres_mbar = baro_report.pressure; // Pressure in mbar + raw.baro1_alt_meter = baro_report.altitude; // Altitude in meters + raw.baro1_temp_celcius = baro_report.temperature; // Temperature in degrees celcius + + raw.baro1_timestamp = baro_report.timestamp; + } } void @@ -1881,6 +1926,7 @@ Sensors::task_main() _mag2_sub = orb_subscribe(ORB_ID(sensor_mag2)); _rc_sub = orb_subscribe(ORB_ID(input_rc)); _baro_sub = orb_subscribe(ORB_ID(sensor_baro0)); + _baro1_sub = orb_subscribe(ORB_ID(sensor_baro1)); _diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure)); _vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); From f28e8d6731014afa13bef368d8a58fe7f3579249 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 31 Dec 2014 02:14:35 +0100 Subject: [PATCH 330/652] sdlog2: Adjust frame size --- src/modules/sdlog2/module.mk | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/sdlog2/module.mk b/src/modules/sdlog2/module.mk index f1118000e8..4fd6fd1c28 100644 --- a/src/modules/sdlog2/module.mk +++ b/src/modules/sdlog2/module.mk @@ -42,7 +42,7 @@ MODULE_PRIORITY = "SCHED_PRIORITY_MAX-30" SRCS = sdlog2.c \ logbuffer.c -MODULE_STACKSIZE = 1200 +MODULE_STACKSIZE = 1300 MAXOPTIMIZATION = -Os From ebc4cc3a7dfc6a705a4585c75e92969a6a0e798f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 31 Dec 2014 02:24:21 +0100 Subject: [PATCH 331/652] sdlog2: Adjust frame size warning limit, cross-checked stack size of app --- src/modules/sdlog2/module.mk | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/sdlog2/module.mk b/src/modules/sdlog2/module.mk index 4fd6fd1c28..91a9611d45 100644 --- a/src/modules/sdlog2/module.mk +++ b/src/modules/sdlog2/module.mk @@ -42,9 +42,9 @@ MODULE_PRIORITY = "SCHED_PRIORITY_MAX-30" SRCS = sdlog2.c \ logbuffer.c -MODULE_STACKSIZE = 1300 +MODULE_STACKSIZE = 1200 MAXOPTIMIZATION = -Os -EXTRACFLAGS = -Wframe-larger-than=1200 +EXTRACFLAGS = -Wframe-larger-than=1300 From f7cfcc9e2e4961f2ba9ff2323858ef793c67f8e0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 2 Jan 2015 19:01:47 +0100 Subject: [PATCH 332/652] README cleanup --- README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index 5e4bc478d6..56bb4ae2ad 100644 --- a/README.md +++ b/README.md @@ -11,7 +11,7 @@ * [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) + * [Downloads](http://px4.io/downloads) * Mailing list: [Google Groups](http://groups.google.com/group/px4users) ### Developers ### @@ -25,7 +25,7 @@ http://px4.io/dev/ This repository contains code supporting these boards: * PX4FMUv1.x * PX4FMUv2.x - * AeroCore + * AeroCore (v1 and v2) ## NuttShell (NSH) ## From 61d57539fb534a7f99ed8416a4102fb5fc0138d2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 2 Jan 2015 19:15:56 +0100 Subject: [PATCH 333/652] Readme: more links --- README.md | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/README.md b/README.md index 56bb4ae2ad..1a559d3c3c 100644 --- a/README.md +++ b/README.md @@ -14,6 +14,10 @@ * [Downloads](http://px4.io/downloads) * Mailing list: [Google Groups](http://groups.google.com/group/px4users) +### Users ### + +Please refer to the [user documentation](https://pixhawk.org/users/start) for flying drones with PX4. + ### Developers ### Contributing guide: From 859185ac72877a305b3e08d98a6ace8ae8196c92 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Fri, 2 Jan 2015 17:44:28 -0500 Subject: [PATCH 334/652] move autodeclination unittest to gtest and delete the sample --- unittests/CMakeLists.txt | 26 +++++ unittests/Makefile | 28 +----- unittests/autodeclination_test.cpp | 53 ++-------- unittests/sample.cc | 68 ------------- unittests/sample.h | 43 -------- unittests/sample_unittest.cc | 153 ----------------------------- 6 files changed, 38 insertions(+), 333 deletions(-) create mode 100644 unittests/CMakeLists.txt delete mode 100644 unittests/sample.cc delete mode 100644 unittests/sample.h delete mode 100644 unittests/sample_unittest.cc diff --git a/unittests/CMakeLists.txt b/unittests/CMakeLists.txt new file mode 100644 index 0000000000..d264ae8cdc --- /dev/null +++ b/unittests/CMakeLists.txt @@ -0,0 +1,26 @@ +cmake_minimum_required(VERSION 2.8) +project(unittests) +enable_testing() + +set(GTEST_DIR gtest) +add_subdirectory(${GTEST_DIR}) +include_directories(${GTEST_DIR}/include) +include_directories(${CMAKE_SOURCE_DIR}) +include_directories(${CMAKE_SOURCE_DIR}/../src) +include_directories(${CMAKE_SOURCE_DIR}/../src/modules) +include_directories(${CMAKE_SOURCE_DIR}/../src/lib) + +add_definitions(-D__EXPORT=) + +function(add_gtest) + foreach(test_name ${ARGN}) + target_link_libraries(${test_name} gtest_main) + add_test(${test_name}Test ${test_name}) + endforeach() +endfunction() + + +# add each test +# todo: add mixer_test sbus2_test st24_test sf0x_test +add_executable(autodeclination_test autodeclination_test.cpp ${CMAKE_SOURCE_DIR}/../src/lib/geo_lookup/geo_mag_declination.c) +add_gtest(autodeclination_test) diff --git a/unittests/Makefile b/unittests/Makefile index e55411a753..e685708684 100644 --- a/unittests/Makefile +++ b/unittests/Makefile @@ -40,7 +40,7 @@ gtest_main.a: gtest-all.o gtest_main.o $(AR) $(ARFLAGS) $@ $^ -all: sample_unittest mixer_test sbus2_test autodeclination_test st24_test sf0x_test +all: mixer_test sbus2_test st24_test sf0x_test MIXER_FILES=../src/systemcmds/tests/test_mixer.cpp \ ../src/systemcmds/tests/test_conv.cpp \ @@ -66,24 +66,6 @@ SF0X_FILES= \ sf0x_test.cpp \ ../src/drivers/sf0x/sf0x_parser.cpp -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) @@ -93,19 +75,15 @@ sbus2_test: $(SBUS2_FILES) sf0x_test: $(SF0X_FILES) $(CC) -o sf0x_test $(SF0X_FILES) $(CFLAGS) -autodeclination_test: $(SBUS2_FILES) - $(CC) -o autodeclination_test $(AUTODECLINATION_FILES) $(CFLAGS) - st24_test: $(ST24_FILES) $(CC) -o st24_test $(ST24_FILES) $(CFLAGS) -unittests: clean mixer_test sbus2_test sf0x_test autodeclination_test st24_test +unittests: clean mixer_test sbus2_test sf0x_test st24_test ./mixer_test ./sbus2_test ./sf0x_test - ./autodeclination_test ./st24_test .PHONY: clean clean: - rm -f gtest.a gtest_main.a *.o $(ODIR)/*.o *~ core $(INCDIR)/*~ sample_unittest 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 st24_test sf0x_test diff --git a/unittests/autodeclination_test.cpp b/unittests/autodeclination_test.cpp index 1bda6eb795..a27d5f1008 100644 --- a/unittests/autodeclination_test.cpp +++ b/unittests/autodeclination_test.cpp @@ -1,52 +1,17 @@ - +#include #include #include -#include #include -#include -#include -#include +#include + #include -#include -// #include "../../src/systemcmds/tests/tests.h" #include +#include +#include +#include -int main(int argc, char *argv[]) { - warnx("autodeclination test started"); +#include "gtest/gtest.h" - 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(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, 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; +TEST(AutoDeclinationTest, AutoDeclination) { + ASSERT_NEAR(get_mag_declination(47.0, 8.0), 0.6, 0.5) << "declination differs more than 1 degree"; } diff --git a/unittests/sample.cc b/unittests/sample.cc deleted file mode 100644 index 3f0c838f29..0000000000 --- a/unittests/sample.cc +++ /dev/null @@ -1,68 +0,0 @@ -// 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 deleted file mode 100644 index 3dfeb98c45..0000000000 --- a/unittests/sample.h +++ /dev/null @@ -1,43 +0,0 @@ -// 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 deleted file mode 100644 index 8dc3f5c38f..0000000000 --- a/unittests/sample_unittest.cc +++ /dev/null @@ -1,153 +0,0 @@ -// 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 c520119129c8815c3d79299917154bcec7a58707 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Fri, 2 Jan 2015 22:40:30 -0500 Subject: [PATCH 335/652] unittests Makefile call new cmake based unittests --- .gitignore | 1 + unittests/Makefile | 8 ++++++-- 2 files changed, 7 insertions(+), 2 deletions(-) diff --git a/.gitignore b/.gitignore index 5328de0098..9959229f5f 100644 --- a/.gitignore +++ b/.gitignore @@ -39,3 +39,4 @@ tags .ropeproject *.orig Firmware.zip +unittests/build diff --git a/unittests/Makefile b/unittests/Makefile index e685708684..e9453ceddc 100644 --- a/unittests/Makefile +++ b/unittests/Makefile @@ -78,7 +78,11 @@ sf0x_test: $(SF0X_FILES) st24_test: $(ST24_FILES) $(CC) -o st24_test $(ST24_FILES) $(CFLAGS) -unittests: clean mixer_test sbus2_test sf0x_test st24_test +cmake_gtests: + mkdir -p build + cd build && cmake .. && $(MAKE) && $(MAKE) test + +unittests: clean mixer_test sbus2_test sf0x_test st24_test cmake_gtests ./mixer_test ./sbus2_test ./sf0x_test @@ -86,4 +90,4 @@ unittests: clean mixer_test sbus2_test sf0x_test st24_test .PHONY: clean clean: - rm -f gtest.a gtest_main.a *.o $(ODIR)/*.o *~ core $(INCDIR)/*~ sample_unittest mixer_test sbus2_test st24_test sf0x_test + rm -rf gtest.a gtest_main.a *.o $(ODIR)/*.o *~ core $(INCDIR)/*~ sample_unittest mixer_test sbus2_test st24_test sf0x_test build From cd381161164eb4f3bd30764f1b62a1e39b8db39f Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Fri, 2 Jan 2015 22:47:10 -0500 Subject: [PATCH 336/652] add cmake to travis --- .travis.yml | 2 +- unittests/Makefile | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/.travis.yml b/.travis.yml index e7dc6029cc..02882ac96e 100644 --- a/.travis.yml +++ b/.travis.yml @@ -10,7 +10,7 @@ before_script: # 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 - - sudo apt-get install flex bison libncurses5-dev autoconf texinfo build-essential libtool zlib1g-dev genromfs git wget + - sudo apt-get install flex bison libncurses5-dev autoconf texinfo build-essential libtool zlib1g-dev genromfs git wget cmake - pushd . - cd ~ - wget https://launchpadlibrarian.net/174121628/gcc-arm-none-eabi-4_7-2014q2-20140408-linux.tar.bz2 diff --git a/unittests/Makefile b/unittests/Makefile index e9453ceddc..5faa50bb5f 100644 --- a/unittests/Makefile +++ b/unittests/Makefile @@ -80,7 +80,7 @@ st24_test: $(ST24_FILES) cmake_gtests: mkdir -p build - cd build && cmake .. && $(MAKE) && $(MAKE) test + cd build && CC=gcc cmake .. && $(MAKE) && $(MAKE) test unittests: clean mixer_test sbus2_test sf0x_test st24_test cmake_gtests ./mixer_test From dd165100fb8c0943c7518b40ae67c36b408baa0d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 1 Jan 2015 15:47:07 +0100 Subject: [PATCH 337/652] Support for HMC5983, which can also be attached via SPI --- src/drivers/hmc5883/hmc5883.cpp | 237 ++++++++++++++-------------- src/drivers/hmc5883/hmc5883.h | 52 ++++++ src/drivers/hmc5883/hmc5883_i2c.cpp | 175 ++++++++++++++++++++ src/drivers/hmc5883/hmc5883_spi.cpp | 189 ++++++++++++++++++++++ src/drivers/hmc5883/module.mk | 4 +- 5 files changed, 539 insertions(+), 118 deletions(-) create mode 100644 src/drivers/hmc5883/hmc5883.h create mode 100644 src/drivers/hmc5883/hmc5883_i2c.cpp create mode 100644 src/drivers/hmc5883/hmc5883_spi.cpp diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index d4dbf37782..5989703dd2 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -34,7 +34,7 @@ /** * @file hmc5883.cpp * - * Driver for the HMC5883 magnetometer connected via I2C. + * Driver for the HMC5883 / HMC5983 magnetometer connected via I2C or SPI. */ #include @@ -74,11 +74,12 @@ #include #include +#include "hmc5883.h" + /* * HMC5883 internal constants and data structures. */ -#define HMC5883L_ADDRESS PX4_I2C_OBDEV_HMC5883 #define HMC5883L_DEVICE_PATH_INT "/dev/hmc5883_int" #define HMC5883L_DEVICE_PATH_EXT "/dev/hmc5883_ext" @@ -95,9 +96,6 @@ #define ADDR_DATA_OUT_Y_MSB 0x07 #define ADDR_DATA_OUT_Y_LSB 0x08 #define ADDR_STATUS 0x09 -#define ADDR_ID_A 0x0a -#define ADDR_ID_B 0x0b -#define ADDR_ID_C 0x0c /* modes not changeable outside of driver */ #define HMC5883L_MODE_NORMAL (0 << 0) /* default */ @@ -115,10 +113,11 @@ #define STATUS_REG_DATA_OUT_LOCK (1 << 1) /* page 16: set if data is only partially read, read device to reset */ #define STATUS_REG_DATA_READY (1 << 0) /* page 16: set if all axes have valid measurements */ -#define ID_A_WHO_AM_I 'H' -#define ID_B_WHO_AM_I '4' -#define ID_C_WHO_AM_I '3' - +enum HMC5883_BUS { + HMC5883_BUS_ALL, + HMC5883_BUS_INTERNAL, + HMC5883_BUS_EXTERNAL +}; /* oddly, ERROR is not defined for c++ */ #ifdef ERROR @@ -130,10 +129,10 @@ static const int ERROR = -1; # error This requires CONFIG_SCHED_WORKQUEUE. #endif -class HMC5883 : public device::I2C +class HMC5883 : public device::CDev { public: - HMC5883(int bus, const char *path, enum Rotation rotation); + HMC5883(device::Device *interface, const char *path, enum Rotation rotation); virtual ~HMC5883(); virtual int init(); @@ -147,7 +146,7 @@ public: void print_info(); protected: - virtual int probe(); + Device *_interface; private: work_s _work; @@ -174,7 +173,6 @@ private: bool _sensor_ok; /**< sensor was found and reports ok */ bool _calibrated; /**< the calibration is valid */ - int _bus; /**< the bus the device is connected to */ enum Rotation _rotation; struct mag_report _last_report; /**< used for info() */ @@ -182,15 +180,6 @@ private: uint8_t _range_bits; uint8_t _conf_reg; - /** - * 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. * @@ -349,8 +338,9 @@ private: extern "C" __EXPORT int hmc5883_main(int argc, char *argv[]); -HMC5883::HMC5883(int bus, const char *path, enum Rotation rotation) : - I2C("HMC5883", path, bus, HMC5883L_ADDRESS, 400000), +HMC5883::HMC5883(device::Device *interface, const char *path, enum Rotation rotation) : + CDev("HMC5883", path), + _interface(interface), _work{}, _measure_ticks(0), _reports(nullptr), @@ -369,7 +359,6 @@ HMC5883::HMC5883(int bus, const char *path, enum Rotation rotation) : _conf_errors(perf_alloc(PC_COUNT, "hmc5883_conf_errors")), _sensor_ok(false), _calibrated(false), - _bus(bus), _rotation(rotation), _last_report{0}, _range_bits(0), @@ -416,9 +405,11 @@ HMC5883::init() { int ret = ERROR; - /* do I2C init (and probe) first */ - if (I2C::init() != OK) + ret = CDev::init(); + if (ret != OK) { + debug("CDev init failed"); goto out; + } /* allocate basic report buffers */ _reports = new RingBuffer(2, sizeof(mag_report)); @@ -429,20 +420,7 @@ HMC5883::init() reset(); _class_instance = register_class_devname(MAG_DEVICE_PATH); - - switch (_class_instance) { - case CLASS_DEVICE_PRIMARY: - _mag_orb_id = ORB_ID(sensor_mag0); - break; - - case CLASS_DEVICE_SECONDARY: - _mag_orb_id = ORB_ID(sensor_mag1); - break; - - case CLASS_DEVICE_TERTIARY: - _mag_orb_id = ORB_ID(sensor_mag2); - break; - } + _mag_orb_id = ORB_ID_TRIPLE(sensor_mag, _class_instance); ret = OK; /* sensor is ok, but not calibrated */ @@ -559,30 +537,6 @@ void HMC5883::check_conf(void) } } -int -HMC5883::probe() -{ - uint8_t data[3] = {0, 0, 0}; - - _retries = 10; - - if (read_reg(ADDR_ID_A, data[0]) || - read_reg(ADDR_ID_B, data[1]) || - read_reg(ADDR_ID_C, data[2])) - debug("read_reg fail"); - - _retries = 2; - - if ((data[0] != ID_A_WHO_AM_I) || - (data[1] != ID_B_WHO_AM_I) || - (data[2] != ID_C_WHO_AM_I)) { - debug("ID byte mismatch (%02x,%02x,%02x)", data[0], data[1], data[2]); - return -EIO; - } - - return OK; -} - ssize_t HMC5883::read(struct file *filp, char *buffer, size_t buflen) { @@ -643,6 +597,8 @@ HMC5883::read(struct file *filp, char *buffer, size_t buflen) int HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg) { + unsigned dummy = arg; + switch (cmd) { case SENSORIOCSPOLLRATE: { switch (arg) { @@ -768,14 +724,12 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg) return check_calibration(); case MAGIOCGEXTERNAL: - if (_bus == PX4_I2C_BUS_EXPANSION) - return 1; - else - return 0; + debug("MAGIOCGEXTERNAL in main driver"); + return _interface->ioctl(cmd, dummy); default: /* give it to the superclass */ - return I2C::ioctl(filp, cmd, arg); + return CDev::ioctl(filp, cmd, arg); } } @@ -890,7 +844,6 @@ HMC5883::collect() } report; int ret; - uint8_t cmd; uint8_t check_counter; perf_begin(_sample_perf); @@ -908,8 +861,7 @@ HMC5883::collect() */ /* get measurements from the device */ - cmd = ADDR_DATA_OUT_X_MSB; - ret = transfer(&cmd, 1, (uint8_t *)&hmc_report, sizeof(hmc_report)); + ret = _interface->read(ADDR_DATA_OUT_X_MSB, (uint8_t *)&hmc_report, sizeof(hmc_report)); if (ret != OK) { perf_count(_comms_errors); @@ -946,14 +898,14 @@ HMC5883::collect() /* scale values for output */ -#ifdef PX4_I2C_BUS_ONBOARD - if (_bus == PX4_I2C_BUS_ONBOARD) { + // XXX revisit for SPI part, might require a bus type IOCTL + unsigned dummy; + if (!_interface->ioctl(MAGIOCGEXTERNAL, dummy)) { // convert onboard so it matches offboard for the // scaling below report.y = -report.y; report.x = -report.x; } -#endif /* the standard external mag by 3DR has x pointing to the * right, y pointing backwards, and z down, therefore switch x @@ -1291,15 +1243,17 @@ int HMC5883::set_excitement(unsigned enable) int HMC5883::write_reg(uint8_t reg, uint8_t val) { - uint8_t cmd[] = { reg, val }; - - return transfer(&cmd[0], 2, nullptr, 0); + uint8_t buf = val; + return _interface->write(reg, &buf, 1); } int HMC5883::read_reg(uint8_t reg, uint8_t &val) { - return transfer(®, 1, &val, 1); + uint8_t buf = val; + int ret = _interface->read(reg, &buf, 1); + val = buf; + return ret; } float @@ -1351,6 +1305,7 @@ void test(int bus); void reset(int bus); int info(int bus); int calibrate(int bus); +const char* get_path(int bus); void usage(); /** @@ -1360,43 +1315,87 @@ void usage(); * is either successfully up and running or failed to start. */ void -start(int bus, enum Rotation rotation) +start(int external_bus, enum Rotation rotation) { int fd; /* create the driver, attempt expansion bus first */ - if (bus == -1 || bus == PX4_I2C_BUS_EXPANSION) { - if (g_dev_ext != nullptr) + if (external_bus == HMC5883_BUS_ALL || external_bus == HMC5883_BUS_EXTERNAL) { + if (g_dev_ext != nullptr) { errx(0, "already started external"); - g_dev_ext = new HMC5883(PX4_I2C_BUS_EXPANSION, HMC5883L_DEVICE_PATH_EXT, rotation); - if (g_dev_ext != nullptr && OK != g_dev_ext->init()) { - delete g_dev_ext; - g_dev_ext = nullptr; + } + + device::Device *interface = nullptr; + + /* create the driver, only attempt I2C for the external bus */ + if (interface == nullptr && (HMC5883_I2C_interface != nullptr)) { + interface = HMC5883_I2C_interface(PX4_I2C_BUS_EXPANSION); + } + + if (interface == nullptr) { + warnx("failed to allocate an interface"); + } + + if (interface->init() != OK) { + delete interface; + warnx("interface init failed"); + } else { + + g_dev_ext = new HMC5883(interface, HMC5883L_DEVICE_PATH_EXT, rotation); + if (g_dev_ext != nullptr && OK != g_dev_ext->init()) { + delete g_dev_ext; + g_dev_ext = nullptr; + } + } } -#ifdef PX4_I2C_BUS_ONBOARD /* if this failed, attempt onboard sensor */ - if (bus == -1 || bus == PX4_I2C_BUS_ONBOARD) { - if (g_dev_int != nullptr) + if (external_bus == HMC5883_BUS_ALL || external_bus == HMC5883_BUS_INTERNAL) { + if (g_dev_int != nullptr) { errx(0, "already started internal"); - g_dev_int = new HMC5883(PX4_I2C_BUS_ONBOARD, HMC5883L_DEVICE_PATH_INT, rotation); - if (g_dev_int != nullptr && OK != g_dev_int->init()) { + } - /* tear down the failing onboard instance */ - delete g_dev_int; - g_dev_int = nullptr; + device::Device *interface = nullptr; - if (bus == PX4_I2C_BUS_ONBOARD) { + /* create the driver, try SPI first, fall back to I2C if unsuccessful */ +#ifdef PX4_SPIDEV_HMC + if (HMC5883_SPI_interface != nullptr) { + interface = HMC5883_SPI_interface(PX4_SPI_BUS_SENSORS); + } +#endif + +#ifdef PX4_I2C_BUS_ONBOARD + if (interface == nullptr && (HMC5883_I2C_interface != nullptr)) { + interface = HMC5883_I2C_interface(PX4_I2C_BUS_ONBOARD); + } +#endif + if (interface == nullptr) { + warnx("failed to allocate an interface"); + } + + if (interface->init() != OK) { + delete interface; + warnx("interface init failed"); + } else { + + g_dev_int = new HMC5883(interface, HMC5883L_DEVICE_PATH_INT, rotation); + if (g_dev_int != nullptr && OK != g_dev_int->init()) { + + /* tear down the failing onboard instance */ + delete g_dev_int; + g_dev_int = nullptr; + + if (external_bus == HMC5883_BUS_INTERNAL) { + goto fail; + } + } + if (g_dev_int == nullptr && external_bus == HMC5883_BUS_INTERNAL) { goto fail; } } - if (g_dev_int == nullptr && bus == PX4_I2C_BUS_ONBOARD) { - goto fail; - } } -#endif if (g_dev_int == nullptr && g_dev_ext == nullptr) goto fail; @@ -1425,11 +1424,11 @@ start(int bus, enum Rotation rotation) exit(0); fail: - if (g_dev_int != nullptr && (bus == -1 || bus == PX4_I2C_BUS_ONBOARD)) { + if (g_dev_int != nullptr && (external_bus == HMC5883_BUS_ALL || external_bus == HMC5883_BUS_INTERNAL)) { delete g_dev_int; g_dev_int = nullptr; } - if (g_dev_ext != nullptr && (bus == -1 || bus == PX4_I2C_BUS_EXPANSION)) { + if (g_dev_ext != nullptr && (external_bus == HMC5883_BUS_ALL || external_bus == HMC5883_BUS_EXTERNAL)) { delete g_dev_ext; g_dev_ext = nullptr; } @@ -1448,7 +1447,7 @@ test(int bus) struct mag_report report; ssize_t sz; int ret; - const char *path = (bus==PX4_I2C_BUS_ONBOARD?HMC5883L_DEVICE_PATH_INT:HMC5883L_DEVICE_PATH_EXT); + const char *path = get_path(bus); int fd = open(path, O_RDONLY); @@ -1549,7 +1548,7 @@ test(int bus) int calibrate(int bus) { int ret; - const char *path = (bus==PX4_I2C_BUS_ONBOARD?HMC5883L_DEVICE_PATH_INT:HMC5883L_DEVICE_PATH_EXT); + const char *path = get_path(bus); int fd = open(path, O_RDONLY); @@ -1576,7 +1575,7 @@ int calibrate(int bus) void reset(int bus) { - const char *path = (bus==PX4_I2C_BUS_ONBOARD?HMC5883L_DEVICE_PATH_INT:HMC5883L_DEVICE_PATH_EXT); + const char *path = get_path(bus); int fd = open(path, O_RDONLY); @@ -1600,12 +1599,12 @@ info(int bus) { int ret = 1; - HMC5883 *g_dev = (bus == PX4_I2C_BUS_ONBOARD ? g_dev_int : g_dev_ext); + HMC5883 *g_dev = (bus == HMC5883_BUS_INTERNAL ? g_dev_int : g_dev_ext); if (g_dev == nullptr) { warnx("not running on bus %d", bus); } else { - warnx("running on bus: %d (%s)\n", bus, ((PX4_I2C_BUS_ONBOARD) ? "onboard" : "offboard")); + warnx("running on bus: %d (%s)\n", bus, ((HMC5883_BUS_INTERNAL) ? "onboard" : "offboard")); g_dev->print_info(); ret = 0; @@ -1614,6 +1613,12 @@ info(int bus) return ret; } +const char* +get_path(int bus) +{ + return ((bus == HMC5883_BUS_INTERNAL) ? HMC5883L_DEVICE_PATH_INT : HMC5883L_DEVICE_PATH_EXT); +} + void usage() { @@ -1622,7 +1627,7 @@ usage() warnx(" -R rotation"); warnx(" -C calibrate on start"); warnx(" -X only external bus"); -#ifdef PX4_I2C_BUS_ONBOARD +#if (PX4_I2C_BUS_ONBOARD || PX4_SPIDEV_HMC) warnx(" -I only internal bus"); #endif } @@ -1633,7 +1638,7 @@ int hmc5883_main(int argc, char *argv[]) { int ch; - int bus = -1; + int bus = HMC5883_BUS_ALL; enum Rotation rotation = ROTATION_NONE; bool calibrate = false; @@ -1642,13 +1647,13 @@ hmc5883_main(int argc, char *argv[]) case 'R': rotation = (enum Rotation)atoi(optarg); break; -#ifdef PX4_I2C_BUS_ONBOARD +#if (PX4_I2C_BUS_ONBOARD || PX4_SPIDEV_HMC) case 'I': - bus = PX4_I2C_BUS_ONBOARD; + bus = HMC5883_BUS_INTERNAL; break; #endif case 'X': - bus = PX4_I2C_BUS_EXPANSION; + bus = HMC5883_BUS_EXTERNAL; break; case 'C': calibrate = true; @@ -1692,13 +1697,13 @@ hmc5883_main(int argc, char *argv[]) * Print driver information. */ if (!strcmp(verb, "info") || !strcmp(verb, "status")) { - if (bus == -1) { + if (bus == HMC5883_BUS_ALL) { int ret = 0; - if (hmc5883::info(PX4_I2C_BUS_ONBOARD)) { + if (hmc5883::info(HMC5883_BUS_INTERNAL)) { ret = 1; } - if (hmc5883::info(PX4_I2C_BUS_EXPANSION)) { + if (hmc5883::info(HMC5883_BUS_EXTERNAL)) { ret = 1; } exit(ret); diff --git a/src/drivers/hmc5883/hmc5883.h b/src/drivers/hmc5883/hmc5883.h new file mode 100644 index 0000000000..0eb7736295 --- /dev/null +++ b/src/drivers/hmc5883/hmc5883.h @@ -0,0 +1,52 @@ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file hmc5883.h + * + * Shared defines for the hmc5883 driver. + */ + +#pragma once + +#define ADDR_ID_A 0x0a +#define ADDR_ID_B 0x0b +#define ADDR_ID_C 0x0c + +#define ID_A_WHO_AM_I 'H' +#define ID_B_WHO_AM_I '4' +#define ID_C_WHO_AM_I '3' + +/* interface factories */ +extern device::Device *HMC5883_SPI_interface(int bus) weak_function; +extern device::Device *HMC5883_I2C_interface(int bus) weak_function; diff --git a/src/drivers/hmc5883/hmc5883_i2c.cpp b/src/drivers/hmc5883/hmc5883_i2c.cpp new file mode 100644 index 0000000000..2d3e8fa08a --- /dev/null +++ b/src/drivers/hmc5883/hmc5883_i2c.cpp @@ -0,0 +1,175 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + + /** + * @file HMC5883_I2C.cpp + * + * I2C interface for HMC5883 / HMC 5983 + */ + +/* XXX trim includes */ +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include + +#include "hmc5883.h" + +#include "board_config.h" + +#ifdef PX4_I2C_OBDEV_HMC5883 + +#define HMC5883L_ADDRESS PX4_I2C_OBDEV_HMC5883 + +device::Device *HMC5883_I2C_interface(int bus); + +class HMC5883_I2C : public device::I2C +{ +public: + HMC5883_I2C(int bus); + virtual ~HMC5883_I2C(); + + virtual int init(); + virtual int read(unsigned address, void *data, unsigned count); + virtual int write(unsigned address, void *data, unsigned count); + + virtual int ioctl(unsigned operation, unsigned &arg); + +protected: + virtual int probe(); + +}; + +device::Device * +HMC5883_I2C_interface(int bus) +{ + return new HMC5883_I2C(bus); +} + +HMC5883_I2C::HMC5883_I2C(int bus) : + I2C("HMC5883_I2C", nullptr, bus, 0, 400000) +{ +} + +HMC5883_I2C::~HMC5883_I2C() +{ +} + +int +HMC5883_I2C::init() +{ + /* this will call probe() */ + return I2C::init(); +} + +int +HMC5883_I2C::ioctl(unsigned operation, unsigned &arg) +{ + int ret; + + switch (operation) { + + case MAGIOCGEXTERNAL: + if (_bus == PX4_I2C_BUS_EXPANSION) { + return 1; + } else { + return 0; + } + + default: + ret = -EINVAL; + } + + return ret; +} + +int +HMC5883_I2C::probe() +{ + uint8_t data[3] = {0, 0, 0}; + + _retries = 10; + + if (read(ADDR_ID_A, &data[0], 1) || + read(ADDR_ID_B, &data[1], 1) || + read(ADDR_ID_C, &data[2], 1)) { + debug("read_reg fail"); + return -EIO; + } + + _retries = 2; + + if ((data[0] != ID_A_WHO_AM_I) || + (data[1] != ID_B_WHO_AM_I) || + (data[2] != ID_C_WHO_AM_I)) { + debug("ID byte mismatch (%02x,%02x,%02x)", data[0], data[1], data[2]); + return -EIO; + } + + return OK; +} + +int +HMC5883_I2C::write(unsigned address, void *data, unsigned count) +{ + uint8_t buf[32]; + + if (sizeof(buf) < (count + 1)) { + return -EIO; + } + + buf[0] = address; + memcpy(&buf[1], data, count); + + return transfer(&buf[0], count + 1, nullptr, 0); +} + +int +HMC5883_I2C::read(unsigned address, void *data, unsigned count) +{ + uint8_t cmd = address; + return transfer(&cmd, 1, (uint8_t *)data, count); +} + +#endif /* PX4_I2C_OBDEV_HMC5883 */ diff --git a/src/drivers/hmc5883/hmc5883_spi.cpp b/src/drivers/hmc5883/hmc5883_spi.cpp new file mode 100644 index 0000000000..25a2f2b405 --- /dev/null +++ b/src/drivers/hmc5883/hmc5883_spi.cpp @@ -0,0 +1,189 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + + /** + * @file HMC5883_SPI.cpp + * + * SPI interface for HMC5983 + */ + +/* XXX trim includes */ +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include + +#include "hmc5883.h" +#include + +#ifdef PX4_SPIDEV_HMC + +/* SPI protocol address bits */ +#define DIR_READ (1<<7) +#define DIR_WRITE (0<<7) +#define ADDR_INCREMENT (1<<6) + +#define HMC_MAX_SEND_LEN 4 +#define HMC_MAX_RCV_LEN 8 + +device::Device *HMC5883_SPI_interface(int bus); + +class HMC5883_SPI : public device::SPI +{ +public: + HMC5883_SPI(int bus, spi_dev_e device); + virtual ~HMC5883_SPI(); + + virtual int init(); + virtual int read(unsigned address, void *data, unsigned count); + virtual int write(unsigned address, void *data, unsigned count); + + virtual int ioctl(unsigned operation, unsigned &arg); + +}; + +device::Device * +HMC5883_SPI_interface(int bus) +{ + return new HMC5883_SPI(bus, (spi_dev_e)PX4_SPIDEV_HMC); +} + +HMC5883_SPI::HMC5883_SPI(int bus, spi_dev_e device) : + SPI("HMC5883_SPI", nullptr, bus, device, SPIDEV_MODE3, 11*1000*1000 /* will be rounded to 10.4 MHz */) +{ +} + +HMC5883_SPI::~HMC5883_SPI() +{ +} + +int +HMC5883_SPI::init() +{ + int ret; + + ret = SPI::init(); + if (ret != OK) { + debug("SPI init failed"); + return -EIO; + } + + // read WHO_AM_I value + uint8_t data[3] = {0, 0, 0}; + + if (read(ADDR_ID_A, &data[0], 1) || + read(ADDR_ID_B, &data[1], 1) || + read(ADDR_ID_C, &data[2], 1)) { + debug("read_reg fail"); + } + + if ((data[0] != ID_A_WHO_AM_I) || + (data[1] != ID_B_WHO_AM_I) || + (data[2] != ID_C_WHO_AM_I)) { + debug("ID byte mismatch (%02x,%02x,%02x)", data[0], data[1], data[2]); + return -EIO; + } + + return OK; +} + +int +HMC5883_SPI::ioctl(unsigned operation, unsigned &arg) +{ + int ret; + + switch (operation) { + + case MAGIOCGEXTERNAL: + +#ifdef PX4_SPI_BUS_EXT + if (_bus == PX4_SPI_BUS_EXT) { + return 1; + } else +#endif + { + return 0; + } + + default: + { + ret = -EINVAL; + } + } + + return ret; +} + +int +HMC5883_SPI::write(unsigned address, void *data, unsigned count) +{ + uint8_t buf[32]; + + if (sizeof(buf) < (count + 1)) { + return -EIO; + } + + buf[0] = address | DIR_WRITE; + memcpy(&buf[1], data, count); + + return transfer(&buf[0], &buf[0], count + 1); +} + +int +HMC5883_SPI::read(unsigned address, void *data, unsigned count) +{ + uint8_t buf[32]; + + if (sizeof(buf) < (count + 1)) { + return -EIO; + } + + buf[0] = address | DIR_READ | ADDR_INCREMENT; + + int ret = transfer(&buf[0], &buf[0], count + 1); + memcpy(data, &buf[1], count); + return ret; +} + +#endif /* PX4_SPIDEV_HMC */ diff --git a/src/drivers/hmc5883/module.mk b/src/drivers/hmc5883/module.mk index be2ee72766..d4028b511d 100644 --- a/src/drivers/hmc5883/module.mk +++ b/src/drivers/hmc5883/module.mk @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. +# Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions @@ -37,7 +37,7 @@ MODULE_COMMAND = hmc5883 -SRCS = hmc5883.cpp +SRCS = hmc5883_i2c.cpp hmc5883_spi.cpp hmc5883.cpp MODULE_STACKSIZE = 1200 From f3e3565d1ba827f20c8cc724a56c8e429b0cc7cc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 2 Jan 2015 15:23:40 +0100 Subject: [PATCH 338/652] sensors app: Decent error handling / reporting --- src/modules/sensors/sensors.cpp | 101 +++++++++++++++++++++----------- 1 file changed, 68 insertions(+), 33 deletions(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 018eadaff3..2c798af3bd 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -401,27 +401,27 @@ private: /** * Do accel-related initialisation. */ - void accel_init(); + int accel_init(); /** * Do gyro-related initialisation. */ - void gyro_init(); + int gyro_init(); /** * Do mag-related initialisation. */ - void mag_init(); + int mag_init(); /** * Do baro-related initialisation. */ - void baro_init(); + int baro_init(); /** * Do adc-related initialisation. */ - void adc_init(); + int adc_init(); /** * Poll the accelerometer for updated data. @@ -507,7 +507,7 @@ Sensors::Sensors() : _fd_adc(-1), _last_adc(0), - _task_should_exit(false), + _task_should_exit(true), _sensors_task(-1), _hil_enabled(false), _publishing(true), @@ -913,8 +913,8 @@ Sensors::parameters_update() int barofd; barofd = open(BARO_DEVICE_PATH, 0); if (barofd < 0) { - warn("%s", BARO_DEVICE_PATH); - errx(1, "FATAL: no barometer found"); + warnx("ERROR: no barometer foundon %s", BARO_DEVICE_PATH); + return ERROR; } else { int baroret = ioctl(barofd, BAROIOCSMSLPRESSURE, (unsigned long)(_parameters.baro_qnh * 100)); @@ -929,7 +929,7 @@ Sensors::parameters_update() return OK; } -void +int Sensors::accel_init() { int fd; @@ -937,8 +937,8 @@ Sensors::accel_init() fd = open(ACCEL_DEVICE_PATH, 0); if (fd < 0) { - warn("%s", ACCEL_DEVICE_PATH); - errx(1, "FATAL: no accelerometer found"); + warnx("FATAL: no accelerometer found: %s", ACCEL_DEVICE_PATH); + return ERROR; } else { @@ -967,9 +967,11 @@ Sensors::accel_init() close(fd); } + + return OK; } -void +int Sensors::gyro_init() { int fd; @@ -977,8 +979,8 @@ Sensors::gyro_init() fd = open(GYRO_DEVICE_PATH, 0); if (fd < 0) { - warn("%s", GYRO_DEVICE_PATH); - errx(1, "FATAL: no gyro found"); + warnx("FATAL: no gyro found: %s", GYRO_DEVICE_PATH); + return ERROR; } else { @@ -1008,9 +1010,11 @@ Sensors::gyro_init() close(fd); } + + return OK; } -void +int Sensors::mag_init() { int fd; @@ -1019,8 +1023,8 @@ Sensors::mag_init() fd = open(MAG_DEVICE_PATH, 0); if (fd < 0) { - warn("%s", MAG_DEVICE_PATH); - errx(1, "FATAL: no magnetometer found"); + warnx("FATAL: no magnetometer found: %s", MAG_DEVICE_PATH); + return ERROR; } /* try different mag sampling rates */ @@ -1041,7 +1045,8 @@ Sensors::mag_init() ioctl(fd, SENSORIOCSPOLLRATE, 100); } else { - errx(1, "FATAL: mag sampling rate could not be set"); + warnx("FATAL: mag sampling rate could not be set"); + return ERROR; } } @@ -1050,7 +1055,8 @@ Sensors::mag_init() ret = ioctl(fd, MAGIOCGEXTERNAL, 0); if (ret < 0) { - errx(1, "FATAL: unknown if magnetometer is external or onboard"); + warnx("FATAL: unknown if magnetometer is external or onboard"); + return ERROR; } else if (ret == 1) { _mag_is_external = true; @@ -1060,9 +1066,11 @@ Sensors::mag_init() } close(fd); + + return OK; } -void +int Sensors::baro_init() { int fd; @@ -1070,26 +1078,30 @@ Sensors::baro_init() fd = open(BARO_DEVICE_PATH, 0); if (fd < 0) { - warn("%s", BARO_DEVICE_PATH); - errx(1, "FATAL: No barometer found"); + warnx("FATAL: No barometer found: %s", BARO_DEVICE_PATH); + return ERROR; } /* set the driver to poll at 150Hz */ ioctl(fd, SENSORIOCSPOLLRATE, 150); close(fd); + + return OK; } -void +int Sensors::adc_init() { _fd_adc = open(ADC_DEVICE_PATH, O_RDONLY | O_NONBLOCK); if (_fd_adc < 0) { - warn(ADC_DEVICE_PATH); - warnx("FATAL: no ADC found"); + warnx("FATAL: no ADC found: %s", ADC_DEVICE_PATH); + return ERROR; } + + return OK; } void @@ -1906,11 +1918,27 @@ Sensors::task_main() { /* start individual sensors */ - accel_init(); - gyro_init(); - mag_init(); - baro_init(); - adc_init(); + int ret; + ret = accel_init(); + if (ret) { + goto exit_immediate; + } + ret = gyro_init(); + if (ret) { + goto exit_immediate; + } + ret = mag_init(); + if (ret) { + goto exit_immediate; + } + ret = baro_init(); + if (ret) { + goto exit_immediate; + } + ret = adc_init(); + if (ret) { + goto exit_immediate; + } /* * do subscriptions @@ -1976,6 +2004,8 @@ Sensors::task_main() fds[0].fd = _gyro_sub; fds[0].events = POLLIN; + _task_should_exit = false; + while (!_task_should_exit) { /* wait for up to 50ms for data */ @@ -2026,8 +2056,9 @@ Sensors::task_main() warnx("[sensors] exiting."); +exit_immediate: _sensors_task = -1; - _exit(0); + _exit(ret); } int @@ -2043,9 +2074,13 @@ Sensors::start() (main_t)&Sensors::task_main_trampoline, nullptr); + /* wait until the task is up and running or has failed */ + while(_sensors_task > 0 && _task_should_exit) { + usleep(100); + } + if (_sensors_task < 0) { - warn("task start failed"); - return -errno; + return -ERROR; } return OK; From 268cf7efc59057119d261838157e17d6c2d318f6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 3 Jan 2015 17:45:25 +0100 Subject: [PATCH 339/652] HMC5883: Fix I2C operation --- src/drivers/hmc5883/hmc5883.cpp | 32 +++++++++++++++++++++-------- src/drivers/hmc5883/hmc5883_i2c.cpp | 2 +- 2 files changed, 24 insertions(+), 10 deletions(-) diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 5989703dd2..80b2c5f13b 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -1330,16 +1330,28 @@ start(int external_bus, enum Rotation rotation) /* create the driver, only attempt I2C for the external bus */ if (interface == nullptr && (HMC5883_I2C_interface != nullptr)) { interface = HMC5883_I2C_interface(PX4_I2C_BUS_EXPANSION); + + if (interface->init() != OK) { + delete interface; + interface = nullptr; + warnx("no device on I2C bus #%u", PX4_I2C_BUS_EXPANSION); + } } - if (interface == nullptr) { - warnx("failed to allocate an interface"); - } +#ifdef PX4_I2C_BUS_ONBOARD + if (interface == nullptr && (HMC5883_I2C_interface != nullptr)) { + interface = HMC5883_I2C_interface(PX4_I2C_BUS_ONBOARD); - if (interface->init() != OK) { - delete interface; - warnx("interface init failed"); - } else { + if (interface->init() != OK) { + delete interface; + interface = nullptr; + warnx("no device on I2C bus #%u", PX4_I2C_BUS_ONBOARD); + } + } +#endif + + /* interface will be null if init failed */ + if (interface != nullptr) { g_dev_ext = new HMC5883(interface, HMC5883L_DEVICE_PATH_EXT, rotation); if (g_dev_ext != nullptr && OK != g_dev_ext->init()) { @@ -1367,17 +1379,19 @@ start(int external_bus, enum Rotation rotation) #endif #ifdef PX4_I2C_BUS_ONBOARD + /* this device is already connected as external if present above */ if (interface == nullptr && (HMC5883_I2C_interface != nullptr)) { interface = HMC5883_I2C_interface(PX4_I2C_BUS_ONBOARD); } #endif if (interface == nullptr) { - warnx("failed to allocate an interface"); + warnx("no internal bus scanned"); + goto fail; } if (interface->init() != OK) { delete interface; - warnx("interface init failed"); + warnx("no device on internal bus"); } else { g_dev_int = new HMC5883(interface, HMC5883L_DEVICE_PATH_INT, rotation); diff --git a/src/drivers/hmc5883/hmc5883_i2c.cpp b/src/drivers/hmc5883/hmc5883_i2c.cpp index 2d3e8fa08a..782ea62fe7 100644 --- a/src/drivers/hmc5883/hmc5883_i2c.cpp +++ b/src/drivers/hmc5883/hmc5883_i2c.cpp @@ -88,7 +88,7 @@ HMC5883_I2C_interface(int bus) } HMC5883_I2C::HMC5883_I2C(int bus) : - I2C("HMC5883_I2C", nullptr, bus, 0, 400000) + I2C("HMC5883_I2C", nullptr, bus, HMC5883L_ADDRESS, 400000) { } From f99c29c50cbf4ed73b8fb05624726598b8ffec5b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 3 Jan 2015 17:47:41 +0100 Subject: [PATCH 340/652] Dev info: Reflect address changes --- src/drivers/device/i2c.h | 1 + 1 file changed, 1 insertion(+) diff --git a/src/drivers/device/i2c.h b/src/drivers/device/i2c.h index 206e71ddcc..8518596ea6 100644 --- a/src/drivers/device/i2c.h +++ b/src/drivers/device/i2c.h @@ -132,6 +132,7 @@ protected: */ void set_address(uint16_t address) { _address = address; + _device_id.devid_s.address = _address; } private: From 7d712a4cd1823fed21d6cdf39b60341382fd121c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 3 Jan 2015 17:47:58 +0100 Subject: [PATCH 341/652] Test image ROMFS: Start all sensors --- ROMFS/px4fmu_test/init.d/rcS | 20 ++++++++++++++++++++ 1 file changed, 20 insertions(+) diff --git a/ROMFS/px4fmu_test/init.d/rcS b/ROMFS/px4fmu_test/init.d/rcS index ef032de5cb..40b3500e0e 100644 --- a/ROMFS/px4fmu_test/init.d/rcS +++ b/ROMFS/px4fmu_test/init.d/rcS @@ -97,6 +97,26 @@ else set unit_test_failure_list "${unit_test_failure_list} commander_tests" fi +if hmc5883 -I start +then + # This is an FMUv3 + echo "FMUv3" + ms5611 start + mpu6000 -X start + mpu6000 start + lsm303d -X start + l3gd20 -X start + echo "EVALUATION ONLY SENSORS (not used in production)" + ms5611 -X start +else + # This is an FMUv1 or FMUv2 + echo "FMUv2 (or FMUv3 where 'hmc5883 -I start' failed)" + ms5611 start + mpu6000 start + lsm303d start + l3gd20 start +fi + if [ $unit_test_failure == 0 ] then echo From dc7ee4247f3cfee6647876ba1873e0508e29ae00 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 3 Jan 2015 17:54:22 +0100 Subject: [PATCH 342/652] HMC5883 startup: Ensure sensor configuration is always performed --- src/drivers/hmc5883/hmc5883.cpp | 14 ++++++-------- 1 file changed, 6 insertions(+), 8 deletions(-) diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 80b2c5f13b..fe70bd37fe 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -1320,10 +1320,9 @@ start(int external_bus, enum Rotation rotation) int fd; /* create the driver, attempt expansion bus first */ - if (external_bus == HMC5883_BUS_ALL || external_bus == HMC5883_BUS_EXTERNAL) { - if (g_dev_ext != nullptr) { - errx(0, "already started external"); - } + if (g_dev_ext != nullptr) { + warnx("already started external"); + } else if (external_bus == HMC5883_BUS_ALL || external_bus == HMC5883_BUS_EXTERNAL) { device::Device *interface = nullptr; @@ -1364,10 +1363,9 @@ start(int external_bus, enum Rotation rotation) /* if this failed, attempt onboard sensor */ - if (external_bus == HMC5883_BUS_ALL || external_bus == HMC5883_BUS_INTERNAL) { - if (g_dev_int != nullptr) { - errx(0, "already started internal"); - } + if (g_dev_int != nullptr) { + warnx("already started internal"); + } else if (external_bus == HMC5883_BUS_ALL || external_bus == HMC5883_BUS_INTERNAL) { device::Device *interface = nullptr; From b37b181818edf2fbb66b978c1cdd90cb8588e322 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 2 Jan 2015 11:59:18 +0100 Subject: [PATCH 343/652] NuttX: Enable RTC, require NuttX version with RTC support --- NuttX | 2 +- nuttx-configs/px4fmu-v2/nsh/defconfig | 4 +++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/NuttX b/NuttX index 3c36467c0d..255dc96065 160000 --- a/NuttX +++ b/NuttX @@ -1 +1 @@ -Subproject commit 3c36467c0d5572431a09ae50013328a4693ee070 +Subproject commit 255dc96065ce1fdd7f0f56ca5afb6b6112abfc70 diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index 50d5b0296e..af6634f563 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -477,7 +477,9 @@ CONFIG_SPI=y # CONFIG_SPI_OWNBUS is not set CONFIG_SPI_EXCHANGE=y # CONFIG_SPI_CMDDATA is not set -# CONFIG_RTC is not set +CONFIG_RTC=y +CONFIG_RTC_DATETIME=y +CONFIG_RTC_LSICLOCK=y CONFIG_WATCHDOG=y # CONFIG_ANALOG is not set # CONFIG_AUDIO_DEVICES is not set From 2a95b4a9b89324354967c7a45974a534142efec0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 3 Jan 2015 19:05:42 +0100 Subject: [PATCH 344/652] GPS: Update the RTC even when RTC is enabled --- src/drivers/gps/ubx.cpp | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index b0eb4ab662..91ecdb4741 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -808,7 +808,7 @@ UBX::payload_rx_done(void) UBX_TRACE_RXMSG("Rx NAV-TIMEUTC\n"); { - /* convert to unix timestamp */ + // convert to unix timestamp struct tm timeinfo; timeinfo.tm_year = _buf.payload_rx_nav_timeutc.year - 1900; timeinfo.tm_mon = _buf.payload_rx_nav_timeutc.month - 1; @@ -818,14 +818,15 @@ UBX::payload_rx_done(void) timeinfo.tm_sec = _buf.payload_rx_nav_timeutc.sec; time_t epoch = mktime(&timeinfo); -#ifndef CONFIG_RTC - //Since we lack a hardware RTC, set the system time clock based on GPS UTC - //TODO generalize this by moving into gps.cpp? + // FMUv2+ have a hardware RTC, but GPS helps us to configure it + // and control its drift. Since we rely on the HRT for our monotonic + // clock, updating it from time to time is safe. + + // TODO generalize this by moving into gps.cpp? timespec ts; ts.tv_sec = epoch; ts.tv_nsec = _buf.payload_rx_nav_timeutc.nano; clock_settime(CLOCK_REALTIME, &ts); -#endif _gps_position->time_gps_usec = (uint64_t)epoch * 1000000; //TODO: test this _gps_position->time_gps_usec += (uint64_t)(_buf.payload_rx_nav_timeutc.nano * 1e-3f); From 470d35aca18104b54e26c37b97c86788e91cee67 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 3 Jan 2015 19:19:02 +0100 Subject: [PATCH 345/652] Fix typo in RTC config --- nuttx-configs/px4fmu-v2/nsh/defconfig | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index af6634f563..f9ab949e17 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -479,7 +479,7 @@ CONFIG_SPI_EXCHANGE=y # CONFIG_SPI_CMDDATA is not set CONFIG_RTC=y CONFIG_RTC_DATETIME=y -CONFIG_RTC_LSICLOCK=y +CONFIG_RTC_HSECLOCK=y CONFIG_WATCHDOG=y # CONFIG_ANALOG is not set # CONFIG_AUDIO_DEVICES is not set From 7866904093209219b37e2cbc93371326ac8a2bf7 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Sat, 3 Jan 2015 22:47:00 +0100 Subject: [PATCH 346/652] Reference wiki and remove duplicated information --- ROMFS/px4fmu_common/mixers/README | 54 +++---------------------------- 1 file changed, 5 insertions(+), 49 deletions(-) diff --git a/ROMFS/px4fmu_common/mixers/README b/ROMFS/px4fmu_common/mixers/README index 60311232e8..f671a177c3 100644 --- a/ROMFS/px4fmu_common/mixers/README +++ b/ROMFS/px4fmu_common/mixers/README @@ -4,55 +4,8 @@ PX4 mixer definitions Files in this directory implement example mixers that can be used as a basis for customisation, or for general testing purposes. -Mixer basics ------------- - -Mixers combine control values from various sources (control tasks, user inputs, -etc.) and produce output values suitable for controlling actuators; servos, -motors, switches and so on. - -An actuator derives its value from the combination of one or more control -values. Each of the control values is scaled according to the actuator's -configuration and then combined to produce the actuator value, which may then be -further scaled to suit the specific output type. - -Internally, all scaling is performed using floating point values. Inputs and -outputs are clamped to the range -1.0 to 1.0. - -control control control - | | | - v v v - scale scale scale - | | | - | v | - +-------> mix <------+ - | - scale - | - v - out - -Scaling -------- - -Basic scalers provide linear scaling of the input to the output. - -Each scaler allows the input value to be scaled independently for inputs -greater/less than zero. An offset can be applied to the output, and lower and -upper boundary constraints can be applied. Negative scaling factors cause the -output to be inverted (negative input produces positive output). - -Scaler pseudocode: - -if (input < 0) - output = (input * NEGATIVE_SCALE) + OFFSET -else - output = (input * POSITIVE_SCALE) + OFFSET - -if (output < LOWER_LIMIT) - output = LOWER_LIMIT -if (output > UPPER_LIMIT) - output = UPPER_LIMIT +For a detailed description of the mixing architecture and examples see: +https://pixhawk.org/dev/mixing Syntax ------ @@ -65,6 +18,9 @@ Each file may define more than one mixer; the allocation of mixers to actuators is specific to the device reading the mixer definition, and the number of actuator outputs generated by a mixer is specific to the mixer. +For example: each simple or null mixer is assigned to outputs 1 to x +in the order they appear in the mixer file. + A mixer begins with a line of the form : From 48a8ea7f199d44f02621767cdde1f50ae228b31e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 4 Jan 2015 00:07:42 +0100 Subject: [PATCH 347/652] GPS driver: Fixed implicit cast causing time logic errors. Found by @Zefz --- src/drivers/gps/ubx.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index 91ecdb4741..5db3ced7cd 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -828,8 +828,8 @@ UBX::payload_rx_done(void) ts.tv_nsec = _buf.payload_rx_nav_timeutc.nano; clock_settime(CLOCK_REALTIME, &ts); - _gps_position->time_gps_usec = (uint64_t)epoch * 1000000; //TODO: test this - _gps_position->time_gps_usec += (uint64_t)(_buf.payload_rx_nav_timeutc.nano * 1e-3f); + _gps_position->time_gps_usec = ((uint64_t)epoch) * 1000000ULL; + _gps_position->time_gps_usec += _buf.payload_rx_nav_timeutc.nano / 1000; } _gps_position->timestamp_time = hrt_absolute_time(); From c1f89dbd5c9de5f1bbb1bc0f858911a9f06d6f9d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 4 Jan 2015 00:08:04 +0100 Subject: [PATCH 348/652] GPS driver: Check return value of settime and notify shell if call fails --- src/drivers/gps/ubx.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index 5db3ced7cd..4563fb65cf 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -826,7 +826,9 @@ UBX::payload_rx_done(void) timespec ts; ts.tv_sec = epoch; ts.tv_nsec = _buf.payload_rx_nav_timeutc.nano; - clock_settime(CLOCK_REALTIME, &ts); + if (clock_settime(CLOCK_REALTIME, &ts)) { + warn("failed setting clock"); + } _gps_position->time_gps_usec = ((uint64_t)epoch) * 1000000ULL; _gps_position->time_gps_usec += _buf.payload_rx_nav_timeutc.nano / 1000; From 5f147f3a6f9c823d1835646739ecb49a5a64e34b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 4 Jan 2015 00:14:09 +0100 Subject: [PATCH 349/652] Mixer: Rename to .md and change syntax to markup --- .../px4fmu_common/mixers/{README => README.md} | 17 ++++++----------- 1 file changed, 6 insertions(+), 11 deletions(-) rename ROMFS/px4fmu_common/mixers/{README => README.md} (95%) diff --git a/ROMFS/px4fmu_common/mixers/README b/ROMFS/px4fmu_common/mixers/README.md similarity index 95% rename from ROMFS/px4fmu_common/mixers/README rename to ROMFS/px4fmu_common/mixers/README.md index f671a177c3..b766d05aac 100644 --- a/ROMFS/px4fmu_common/mixers/README +++ b/ROMFS/px4fmu_common/mixers/README.md @@ -1,14 +1,12 @@ -PX4 mixer definitions -===================== +## PX4 mixer definitions ## Files in this directory implement example mixers that can be used as a basis for customisation, or for general testing purposes. For a detailed description of the mixing architecture and examples see: -https://pixhawk.org/dev/mixing +http://px4.io/dev/mixing -Syntax ------- +### Syntax ### Mixer definitions are text files; lines beginning with a single capital letter followed by a colon are significant. All other lines are ignored, meaning that @@ -28,8 +26,7 @@ A mixer begins with a line of the form The tag selects the mixer type; 'M' for a simple summing mixer, 'R' for a multirotor mixer, etc. -Null Mixer -.......... +#### Null Mixer #### A null mixer consumes no controls and generates a single actuator output whose value is always zero. Typically a null mixer is used as a placeholder in a @@ -39,8 +36,7 @@ The null mixer definition has the form: Z: -Simple Mixer -............ +#### Simple Mixer #### A simple mixer combines zero or more control inputs into a single actuator output. Inputs are scaled, and the mixing function sums the result before @@ -78,8 +74,7 @@ discussed above. Whilst the calculations are performed as floating-point operations, the values stored in the definition file are scaled by a factor of 10000; i.e. an offset of -0.5 is encoded as -5000. -Multirotor Mixer -................ +#### Multirotor Mixer #### The multirotor mixer combines four control inputs (roll, pitch, yaw, thrust) into a set of actuator outputs intended to drive motor speed controllers. From a62baf2cb210535e1e7682a860adb5156233f5f0 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sat, 3 Jan 2015 18:48:43 -0500 Subject: [PATCH 350/652] add simple nonsymmetric Matrix testing to test_mathlib --- src/systemcmds/tests/module.mk | 2 +- src/systemcmds/tests/test_mathlib.cpp | 51 ++++++++++++++++++++++++++- 2 files changed, 51 insertions(+), 2 deletions(-) diff --git a/src/systemcmds/tests/module.mk b/src/systemcmds/tests/module.mk index 6eed3922c4..0dc333f0ad 100644 --- a/src/systemcmds/tests/module.mk +++ b/src/systemcmds/tests/module.mk @@ -35,5 +35,5 @@ SRCS = test_adc.c \ test_mount.c \ test_mtd.c -EXTRACXXFLAGS = -Wframe-larger-than=2500 +EXTRACXXFLAGS = -Wframe-larger-than=2500 -Wno-float-equal -Wno-double-promotion diff --git a/src/systemcmds/tests/test_mathlib.cpp b/src/systemcmds/tests/test_mathlib.cpp index 70d173fc91..9fa52aaaa0 100644 --- a/src/systemcmds/tests/test_mathlib.cpp +++ b/src/systemcmds/tests/test_mathlib.cpp @@ -54,6 +54,7 @@ using namespace math; int test_mathlib(int argc, char *argv[]) { + int rc = 0; warnx("testing mathlib"); { @@ -156,5 +157,53 @@ int test_mathlib(int argc, char *argv[]) TEST_OP("Matrix<10, 10> * Matrix<10, 10>", m1 * m2); } - return 0; + { + // test nonsymmetric +, -, +=, -= + + float data1[2][3] = {{1,2,3},{4,5,6}}; + float data2[2][3] = {{2,4,6},{8,10,12}}; + float data3[2][3] = {{3,6,9},{12,15,18}}; + + Matrix<2, 3> m1(data1); + Matrix<2, 3> m2(data2); + Matrix<2, 3> m3(data3); + + if (m1 + m2 != m3) { + warnx("Matrix<2, 3> + Matrix<2, 3> failed!"); + (m1 + m2).print(); + printf("!=\n"); + m3.print(); + rc = 1; + } + + if (m3 - m2 != m1) { + warnx("Matrix<2, 3> - Matrix<2, 3> failed!"); + (m3 - m2).print(); + printf("!=\n"); + m1.print(); + rc = 1; + } + + m1 += m2; + if (m1 != m3) { + warnx("Matrix<2, 3> += Matrix<2, 3> failed!"); + m1.print(); + printf("!=\n"); + m3.print(); + rc = 1; + } + + m1 -= m2; + Matrix<2, 3> m1_orig(data1); + if (m1 != m1_orig) { + warnx("Matrix<2, 3> -= Matrix<2, 3> failed!"); + m1.print(); + printf("!=\n"); + m1_orig.print(); + rc = 1; + } + + } + + return rc; } From 50a00bee8ed56d41d1c3e74eaaa7997191793595 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 4 Jan 2015 10:44:40 +0100 Subject: [PATCH 351/652] GPS driver: UBX time handling for all protocol revisions --- src/drivers/gps/ubx.cpp | 21 +++++++++++---------- 1 file changed, 11 insertions(+), 10 deletions(-) diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index 4563fb65cf..c67965487e 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -748,17 +748,19 @@ UBX::payload_rx_done(void) timeinfo.tm_sec = _buf.payload_rx_nav_pvt.sec; time_t epoch = mktime(&timeinfo); -#ifndef CONFIG_RTC - //Since we lack a hardware RTC, set the system time clock based on GPS UTC - //TODO generalize this by moving into gps.cpp? + // FMUv2+ boards have a hardware RTC, but GPS helps us to configure it + // and control its drift. Since we rely on the HRT for our monotonic + // clock, updating it from time to time is safe. + timespec ts; ts.tv_sec = epoch; ts.tv_nsec = _buf.payload_rx_nav_pvt.nano; - clock_settime(CLOCK_REALTIME, &ts); -#endif + if (clock_settime(CLOCK_REALTIME, &ts)) { + warn("failed setting clock"); + } - _gps_position->time_gps_usec = (uint64_t)epoch * 1000000; //TODO: test this - _gps_position->time_gps_usec += (uint64_t)(_buf.payload_rx_nav_pvt.nano * 1e-3f); + _gps_position->time_gps_usec = static_cast(epoch) * 1000000ULL; + _gps_position->time_gps_usec += _buf.payload_rx_nav_timeutc.nano / 1000; } _gps_position->timestamp_time = hrt_absolute_time(); @@ -818,11 +820,10 @@ UBX::payload_rx_done(void) timeinfo.tm_sec = _buf.payload_rx_nav_timeutc.sec; time_t epoch = mktime(&timeinfo); - // FMUv2+ have a hardware RTC, but GPS helps us to configure it + // FMUv2+ boards have a hardware RTC, but GPS helps us to configure it // and control its drift. Since we rely on the HRT for our monotonic // clock, updating it from time to time is safe. - // TODO generalize this by moving into gps.cpp? timespec ts; ts.tv_sec = epoch; ts.tv_nsec = _buf.payload_rx_nav_timeutc.nano; @@ -830,7 +831,7 @@ UBX::payload_rx_done(void) warn("failed setting clock"); } - _gps_position->time_gps_usec = ((uint64_t)epoch) * 1000000ULL; + _gps_position->time_gps_usec = static_cast(epoch) * 1000000ULL; _gps_position->time_gps_usec += _buf.payload_rx_nav_timeutc.nano / 1000; } From f48abafbc9b9b5f3683fe0c86792f70c0f32bd8e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 4 Jan 2015 10:45:00 +0100 Subject: [PATCH 352/652] GPS driver: Set RTC from Ashtech receivers as well --- src/drivers/gps/ashtech.cpp | 17 +++++++++++++++-- 1 file changed, 15 insertions(+), 2 deletions(-) diff --git a/src/drivers/gps/ashtech.cpp b/src/drivers/gps/ashtech.cpp index a2e292de2a..c2381b8dc7 100644 --- a/src/drivers/gps/ashtech.cpp +++ b/src/drivers/gps/ashtech.cpp @@ -99,8 +99,21 @@ int ASHTECH::handle_message(int len) timeinfo.tm_sec = int(ashtech_sec); time_t epoch = mktime(&timeinfo); - _gps_position->time_gps_usec = (uint64_t)epoch * 1000000; //TODO: test this - _gps_position->time_gps_usec += (uint64_t)((ashtech_sec - int(ashtech_sec)) * 1e6); + uint64_t usecs = static_cast((ashtech_sec - static_cast(ashtech_sec))) * 1e6; + + // FMUv2+ boards have a hardware RTC, but GPS helps us to configure it + // and control its drift. Since we rely on the HRT for our monotonic + // clock, updating it from time to time is safe. + + timespec ts; + ts.tv_sec = epoch; + ts.tv_nsec = usecs * 1000; + if (clock_settime(CLOCK_REALTIME, &ts)) { + warn("failed setting clock"); + } + + _gps_position->time_gps_usec = static_cast(epoch) * 1000000ULL; + _gps_position->time_gps_usec += usecs; _gps_position->timestamp_time = hrt_absolute_time(); } From 17e4e283d86ad07b55c4f2ceaedf801adcfb1a53 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 4 Jan 2015 10:58:45 +0100 Subject: [PATCH 353/652] SMBus battery driver: Stylize according to style guide --- src/drivers/batt_smbus/batt_smbus.cpp | 119 ++++++++++++++------------ 1 file changed, 66 insertions(+), 53 deletions(-) diff --git a/src/drivers/batt_smbus/batt_smbus.cpp b/src/drivers/batt_smbus/batt_smbus.cpp index dd83dacafc..e80c0660e8 100644 --- a/src/drivers/batt_smbus/batt_smbus.cpp +++ b/src/drivers/batt_smbus/batt_smbus.cpp @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012, 2013 PX4 Development Team. All rights reserved. - * Author: Randy Mackay + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -37,6 +36,7 @@ * * Driver for a battery monitor connected via SMBus (I2C). * + * @author Randy Mackay */ #include @@ -77,22 +77,22 @@ #include #include -#define BATT_SMBUS_ADDR_MIN 0x08 /* lowest possible address */ -#define BATT_SMBUS_ADDR_MAX 0x7F /* highest possible address */ +#define BATT_SMBUS_ADDR_MIN 0x08 ///< lowest possible address +#define BATT_SMBUS_ADDR_MAX 0x7F ///< highest possible address -#define BATT_SMBUS_I2C_BUS PX4_I2C_BUS_EXPANSION -#define BATT_SMBUS_ADDR 0x0B /* I2C address */ -#define BATT_SMBUS_TEMP 0x08 /* temperature register */ -#define BATT_SMBUS_VOLTAGE 0x09 /* voltage register */ -#define BATT_SMBUS_DESIGN_CAPACITY 0x18 /* design capacity register */ -#define BATT_SMBUS_DESIGN_VOLTAGE 0x19 /* design voltage register */ -#define BATT_SMBUS_SERIALNUM 0x1c /* serial number register */ -#define BATT_SMBUS_MANUFACTURE_NAME 0x20 /* manufacturer name */ -#define BATT_SMBUS_MANUFACTURE_INFO 0x25 /* cell voltage register */ -#define BATT_SMBUS_CURRENT 0x2a /* current register */ -#define BATT_SMBUS_MEASUREMENT_INTERVAL_MS (1000000 / 10) /* time in microseconds, measure at 10hz */ +#define BATT_SMBUS_I2C_BUS PX4_I2C_BUS_EXPANSION +#define BATT_SMBUS_ADDR 0x0B ///< I2C address +#define BATT_SMBUS_TEMP 0x08 ///< temperature register +#define BATT_SMBUS_VOLTAGE 0x09 ///< voltage register +#define BATT_SMBUS_DESIGN_CAPACITY 0x18 ///< design capacity register +#define BATT_SMBUS_DESIGN_VOLTAGE 0x19 ///< design voltage register +#define BATT_SMBUS_SERIALNUM 0x1c ///< serial number register +#define BATT_SMBUS_MANUFACTURE_NAME 0x20 ///< manufacturer name +#define BATT_SMBUS_MANUFACTURE_INFO 0x25 ///< cell voltage register +#define BATT_SMBUS_CURRENT 0x2a ///< current register +#define BATT_SMBUS_MEASUREMENT_INTERVAL_MS (1000000 / 10) ///< time in microseconds, measure at 10hz -#define BATT_SMBUS_PEC_POLYNOMIAL 0x07 /* Polynomial for calculating PEC */ +#define BATT_SMBUS_PEC_POLYNOMIAL 0x07 ///< Polynomial for calculating PEC #ifndef CONFIG_SCHED_WORKQUEUE # error This requires CONFIG_SCHED_WORKQUEUE. @@ -106,41 +106,62 @@ public: virtual int init(); virtual int test(); - int search(); /* search all possible slave addresses for a smart battery */ + /** + * Search all possible slave addresses for a smart battery + */ + int search(); protected: - virtual int probe(); // check if the device can be contacted + /** + * Check if the device can be contacted + */ + virtual int probe(); private: - // start periodic reads from the battery + /** + * Start periodic reads from the battery + */ void start(); - // stop periodic reads from the battery + /** + * Stop periodic reads from the battery + */ void stop(); - // static function that is called by worker queue + /** + * static function that is called by worker queue + */ static void cycle_trampoline(void *arg); - // perform a read from the battery + /** + * perform a read from the battery + */ void cycle(); - // read_reg - read a word from specified register + /** + * Read a word from specified register + */ int read_reg(uint8_t reg, uint16_t &val); - // read_block - returns number of characters read if successful, zero if unsuccessful + /** + * Read block from bus + * @return returns number of characters read if successful, zero if unsuccessful + */ uint8_t read_block(uint8_t reg, uint8_t* data, uint8_t max_len, bool append_zero); - // get_PEC - calculate PEC for a read or write from the battery - // buff is the data that was read or will be written + /** + * Calculate PEC for a read or write from the battery + * @param buff is the data that was read or will be written + */ uint8_t get_PEC(uint8_t cmd, bool reading, const uint8_t buff[], uint8_t len) const; // internal variables - work_s _work; // work queue for scheduling reads - RingBuffer *_reports; // buffer of recorded voltages, currents - struct battery_status_s _last_report; // last published report, used for test() - orb_advert_t _batt_topic; - orb_id_t _batt_orb_id; + work_s _work; ///< work queue for scheduling reads + RingBuffer *_reports; ///< buffer of recorded voltages, currents + struct battery_status_s _last_report; ///< last published report, used for test() + orb_advert_t _batt_topic; ///< uORB battery topic + orb_id_t _batt_orb_id; ///< uORB battery topic ID }; /* for now, we only support one BATT_SMBUS */ @@ -153,7 +174,6 @@ void batt_smbus_usage(); extern "C" __EXPORT int batt_smbus_main(int argc, char *argv[]); -// constructor BATT_SMBUS::BATT_SMBUS(int bus, uint16_t batt_smbus_addr) : I2C("batt_smbus", BATT_SMBUS_DEVICE_PATH, bus, batt_smbus_addr, 400000), _work{}, @@ -165,10 +185,9 @@ BATT_SMBUS::BATT_SMBUS(int bus, uint16_t batt_smbus_addr) : memset(&_work, 0, sizeof(_work)); } -// destructor BATT_SMBUS::~BATT_SMBUS() { - /* make sure we are truly inactive */ + // make sure we are truly inactive stop(); if (_reports != nullptr) { @@ -188,7 +207,7 @@ BATT_SMBUS::init() errx(1,"failed to init I2C"); return ret; } else { - /* allocate basic report buffers */ + // allocate basic report buffers _reports = new RingBuffer(2, sizeof(struct battery_status_s)); if (_reports == nullptr) { ret = ENOTTY; @@ -230,7 +249,6 @@ BATT_SMBUS::test() return OK; } -/* search all possible slave addresses for a smart battery */ int BATT_SMBUS::search() { @@ -265,25 +283,22 @@ BATT_SMBUS::probe() return OK; } -// start periodic reads from the battery void BATT_SMBUS::start() { - /* reset the report ring and state machine */ + // reset the report ring and state machine _reports->flush(); - /* schedule a cycle to start things */ + // schedule a cycle to start things work_queue(HPWORK, &_work, (worker_t)&BATT_SMBUS::cycle_trampoline, this, 1); } -// stop periodic reads from the battery void BATT_SMBUS::stop() { work_cancel(HPWORK, &_work); } -// static function that is called by worker queue void BATT_SMBUS::cycle_trampoline(void *arg) { @@ -292,7 +307,6 @@ BATT_SMBUS::cycle_trampoline(void *arg) dev->cycle(); } -// perform a read from the battery void BATT_SMBUS::cycle() { @@ -331,14 +345,14 @@ BATT_SMBUS::cycle() // copy report for test() _last_report = new_report; - /* post a report to the ring */ + // post a report to the ring _reports->force(&new_report); - /* notify anyone waiting for data */ + // notify anyone waiting for data poll_notify(POLLIN); } - /* schedule a fresh cycle call when the measurement is done */ + // schedule a fresh cycle call when the measurement is done work_queue(HPWORK, &_work, (worker_t)&BATT_SMBUS::cycle_trampoline, this, USEC2TICK(BATT_SMBUS_MEASUREMENT_INTERVAL_MS)); } @@ -363,8 +377,8 @@ BATT_SMBUS::read_reg(uint8_t reg, uint16_t &val) return ret; } -// read_block - returns number of characters read if successful, zero if unsuccessful -uint8_t BATT_SMBUS::read_block(uint8_t reg, uint8_t* data, uint8_t max_len, bool append_zero) +uint8_t +BATT_SMBUS::read_block(uint8_t reg, uint8_t* data, uint8_t max_len, bool append_zero) { uint8_t buff[max_len+2]; // buffer to hold results @@ -408,9 +422,8 @@ uint8_t BATT_SMBUS::read_block(uint8_t reg, uint8_t* data, uint8_t max_len, bool return bufflen; } -// get_PEC - calculate PEC for a read or write from the battery -// buff is the data that was read or will be written -uint8_t BATT_SMBUS::get_PEC(uint8_t cmd, bool reading, const uint8_t buff[], uint8_t len) const +uint8_t +BATT_SMBUS::get_PEC(uint8_t cmd, bool reading, const uint8_t buff[], uint8_t len) const { // exit immediately if no data if (len <= 0) { @@ -463,11 +476,11 @@ int batt_smbus_main(int argc, char *argv[]) { int i2cdevice = BATT_SMBUS_I2C_BUS; - int batt_smbusadr = BATT_SMBUS_ADDR; /* 7bit */ + int batt_smbusadr = BATT_SMBUS_ADDR; // 7bit address int ch; - /* jump over start/off/etc and look at options first */ + // jump over start/off/etc and look at options first while ((ch = getopt(argc, argv, "a:b:")) != EOF) { switch (ch) { case 'a': @@ -512,7 +525,7 @@ batt_smbus_main(int argc, char *argv[]) exit(0); } - /* need the driver past this point */ + // need the driver past this point if (g_batt_smbus == nullptr) { warnx("not started"); batt_smbus_usage(); From 1106e0bff3173cf480cd63e006ce7e00c962a812 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 4 Jan 2015 11:02:53 +0100 Subject: [PATCH 354/652] SMBus battery driver: further code style fixes --- src/drivers/batt_smbus/batt_smbus.cpp | 78 +++++++++++++++++++-------- 1 file changed, 56 insertions(+), 22 deletions(-) diff --git a/src/drivers/batt_smbus/batt_smbus.cpp b/src/drivers/batt_smbus/batt_smbus.cpp index e80c0660e8..6f9ec63181 100644 --- a/src/drivers/batt_smbus/batt_smbus.cpp +++ b/src/drivers/batt_smbus/batt_smbus.cpp @@ -104,8 +104,22 @@ public: BATT_SMBUS(int bus = PX4_I2C_BUS_EXPANSION, uint16_t batt_smbus_addr = BATT_SMBUS_ADDR); virtual ~BATT_SMBUS(); + /** + * Initialize device + * + * Calls probe() to check for device on bus. + * + * @return 0 on success, error code on failure + */ virtual int init(); + + /** + * Test device + * + * @return 0 on success, error code on failure + */ virtual int test(); + /** * Search all possible slave addresses for a smart battery */ @@ -142,13 +156,13 @@ private: /** * Read a word from specified register */ - int read_reg(uint8_t reg, uint16_t &val); + int read_reg(uint8_t reg, uint16_t &val); /** * Read block from bus * @return returns number of characters read if successful, zero if unsuccessful */ - uint8_t read_block(uint8_t reg, uint8_t* data, uint8_t max_len, bool append_zero); + uint8_t read_block(uint8_t reg, uint8_t *data, uint8_t max_len, bool append_zero); /** * Calculate PEC for a read or write from the battery @@ -164,10 +178,9 @@ private: orb_id_t _batt_orb_id; ///< uORB battery topic ID }; -/* for now, we only support one BATT_SMBUS */ namespace { -BATT_SMBUS *g_batt_smbus; +BATT_SMBUS *g_batt_smbus; ///< device handle. For now, we only support one BATT_SMBUS device } void batt_smbus_usage(); @@ -204,13 +217,16 @@ BATT_SMBUS::init() ret = I2C::init(); if (ret != OK) { - errx(1,"failed to init I2C"); + errx(1, "failed to init I2C"); return ret; + } else { // allocate basic report buffers _reports = new RingBuffer(2, sizeof(struct battery_status_s)); + if (_reports == nullptr) { ret = ENOTTY; + } else { // start work queue start(); @@ -236,6 +252,7 @@ BATT_SMBUS::test() // display new info that has arrived from the orb orb_check(sub, &updated); + if (updated) { if (orb_copy(ORB_ID(battery_status), sub, &status) == OK) { warnx("V=%4.2f C=%4.2f", status.voltage_v, status.current_a); @@ -256,12 +273,14 @@ BATT_SMBUS::search() uint16_t tmp; // search through all valid SMBus addresses - for (uint8_t i=BATT_SMBUS_ADDR_MIN; i<=BATT_SMBUS_ADDR_MAX; i++) { + for (uint8_t i = BATT_SMBUS_ADDR_MIN; i <= BATT_SMBUS_ADDR_MAX; i++) { set_address(i); + if (read_reg(BATT_SMBUS_VOLTAGE, tmp) == OK) { - warnx("battery found at 0x%x",(int)i); + warnx("battery found at 0x%x", (int)i); found_slave = true; } + // short sleep usleep(1); } @@ -269,6 +288,7 @@ BATT_SMBUS::search() // display completion message if (found_slave) { warnx("Done."); + } else { warnx("No smart batteries found."); } @@ -318,6 +338,7 @@ BATT_SMBUS::cycle() // read voltage uint16_t tmp; + if (read_reg(BATT_SMBUS_VOLTAGE, tmp) == OK) { // initialise new_report memset(&new_report, 0, sizeof(new_report)); @@ -328,15 +349,19 @@ BATT_SMBUS::cycle() // read current usleep(1); uint8_t buff[4]; + if (read_block(BATT_SMBUS_CURRENT, buff, 4, false) == 4) { - new_report.current_a = (float)((int32_t)((uint32_t)buff[3]<<24 | (uint32_t)buff[2]<<16 | (uint32_t)buff[1]<<8 | (uint32_t)buff[0])) / 1000.0f; + new_report.current_a = (float)((int32_t)((uint32_t)buff[3] << 24 | (uint32_t)buff[2] << 16 | (uint32_t)buff[1] << 8 | + (uint32_t)buff[0])) / 1000.0f; } // publish to orb if (_batt_topic != -1) { orb_publish(_batt_orb_id, _batt_topic, &new_report); + } else { _batt_topic = orb_advertise(_batt_orb_id, &new_report); + if (_batt_topic < 0) { errx(1, "ADVERT FAIL"); } @@ -353,7 +378,8 @@ BATT_SMBUS::cycle() } // schedule a fresh cycle call when the measurement is done - work_queue(HPWORK, &_work, (worker_t)&BATT_SMBUS::cycle_trampoline, this, USEC2TICK(BATT_SMBUS_MEASUREMENT_INTERVAL_MS)); + work_queue(HPWORK, &_work, (worker_t)&BATT_SMBUS::cycle_trampoline, this, + USEC2TICK(BATT_SMBUS_MEASUREMENT_INTERVAL_MS)); } int @@ -363,11 +389,14 @@ BATT_SMBUS::read_reg(uint8_t reg, uint16_t &val) // read from register int ret = transfer(®, 1, buff, 3); + if (ret == OK) { // check PEC uint8_t pec = get_PEC(reg, true, buff, 2); + if (pec == buff[2]) { val = (uint16_t)buff[1] << 8 | (uint16_t)buff[0]; + } else { ret = ENOTTY; } @@ -378,14 +407,14 @@ BATT_SMBUS::read_reg(uint8_t reg, uint16_t &val) } uint8_t -BATT_SMBUS::read_block(uint8_t reg, uint8_t* data, uint8_t max_len, bool append_zero) +BATT_SMBUS::read_block(uint8_t reg, uint8_t *data, uint8_t max_len, bool append_zero) { - uint8_t buff[max_len+2]; // buffer to hold results + uint8_t buff[max_len + 2]; // buffer to hold results usleep(1); // read bytes including PEC - int ret = transfer(®, 1, buff, max_len+2); + int ret = transfer(®, 1, buff, max_len + 2); // return zero on failure if (ret != OK) { @@ -401,13 +430,15 @@ BATT_SMBUS::read_block(uint8_t reg, uint8_t* data, uint8_t max_len, bool append_ } // check PEC - uint8_t pec = get_PEC(reg, true, buff, bufflen+1); - if (pec != buff[bufflen+1]) { + uint8_t pec = get_PEC(reg, true, buff, bufflen + 1); + + if (pec != buff[bufflen + 1]) { // debug - warnx("CurrPEC:%x vs MyPec:%x",(int)buff[bufflen+1],(int)pec); + warnx("CurrPEC:%x vs MyPec:%x", (int)buff[bufflen + 1], (int)pec); return 0; + } else { - warnx("CurPEC ok: %x",(int)pec); + warnx("CurPEC ok: %x", (int)pec); } // copy data @@ -431,11 +462,11 @@ BATT_SMBUS::get_PEC(uint8_t cmd, bool reading, const uint8_t buff[], uint8_t len } // prepare temp buffer for calcing crc - uint8_t tmp_buff[len+3]; + uint8_t tmp_buff[len + 3]; tmp_buff[0] = (uint8_t)get_address() << 1; tmp_buff[1] = cmd; tmp_buff[2] = tmp_buff[0] | (uint8_t)reading; - memcpy(&tmp_buff[3],buff,len); + memcpy(&tmp_buff[3], buff, len); // initialise crc to zero uint8_t crc = 0; @@ -443,14 +474,16 @@ BATT_SMBUS::get_PEC(uint8_t cmd, bool reading, const uint8_t buff[], uint8_t len bool do_invert; // for each byte in the stream - for (uint8_t i=0; i Date: Sun, 4 Jan 2015 11:04:55 +0100 Subject: [PATCH 355/652] SMBus battery driver: Remove console print on normal block read operation --- src/drivers/batt_smbus/batt_smbus.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/drivers/batt_smbus/batt_smbus.cpp b/src/drivers/batt_smbus/batt_smbus.cpp index 6f9ec63181..2b5fef4d77 100644 --- a/src/drivers/batt_smbus/batt_smbus.cpp +++ b/src/drivers/batt_smbus/batt_smbus.cpp @@ -437,8 +437,6 @@ BATT_SMBUS::read_block(uint8_t reg, uint8_t *data, uint8_t max_len, bool append_ warnx("CurrPEC:%x vs MyPec:%x", (int)buff[bufflen + 1], (int)pec); return 0; - } else { - warnx("CurPEC ok: %x", (int)pec); } // copy data From 4cc7f599d1f29607fae625e26a586d0758f67283 Mon Sep 17 00:00:00 2001 From: Ban Siesta Date: Sun, 4 Jan 2015 10:40:39 +0000 Subject: [PATCH 356/652] ashtech: whitespace --- src/drivers/gps/ashtech.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/drivers/gps/ashtech.cpp b/src/drivers/gps/ashtech.cpp index c2381b8dc7..f1c5ca5495 100644 --- a/src/drivers/gps/ashtech.cpp +++ b/src/drivers/gps/ashtech.cpp @@ -38,7 +38,7 @@ ASHTECH::~ASHTECH() int ASHTECH::handle_message(int len) { char * endp; - + if (len < 7) { return 0; } int uiCalcComma = 0; @@ -624,8 +624,8 @@ void ASHTECH::decode_init(void) } -/* - * ashtech board configuration script +/* + * ashtech board configuration script */ const char comm[] = "$PASHS,POP,20\r\n"\ From 25fc9d791a1faf5d37a6b9a13bd16fafd6a5cf5b Mon Sep 17 00:00:00 2001 From: Ban Siesta Date: Sun, 4 Jan 2015 10:43:28 +0000 Subject: [PATCH 357/652] renaming of gps time to UTC time --- src/drivers/frsky_telemetry/frsky_data.c | 4 ++-- src/drivers/gps/ashtech.cpp | 4 ++-- src/drivers/gps/mtk.cpp | 4 ++-- src/drivers/gps/ubx.cpp | 8 ++++---- .../ekf_att_pos_estimator_main.cpp | 2 +- src/modules/mavlink/mavlink_receiver.cpp | 8 ++++---- .../position_estimator_inav_main.c | 10 +++++----- src/modules/sdlog2/sdlog2.c | 6 +++--- src/modules/uORB/topics/vehicle_global_position.h | 2 +- src/modules/uORB/topics/vehicle_gps_position.h | 2 +- src/modules/uavcan/sensors/gnss.cpp | 2 +- 11 files changed, 26 insertions(+), 26 deletions(-) diff --git a/src/drivers/frsky_telemetry/frsky_data.c b/src/drivers/frsky_telemetry/frsky_data.c index dd9b90fb33..890fada168 100644 --- a/src/drivers/frsky_telemetry/frsky_data.c +++ b/src/drivers/frsky_telemetry/frsky_data.c @@ -223,7 +223,7 @@ void frsky_send_frame2(int uart) char lat_ns = 0, lon_ew = 0; int sec = 0; if (global_pos.timestamp != 0 && hrt_absolute_time() < global_pos.timestamp + 20000) { - time_t time_gps = global_pos.time_gps_usec / 1000000; + time_t time_gps = global_pos.time_utc_usec / 1000000ULL; struct tm *tm_gps = gmtime(&time_gps); course = (global_pos.yaw + M_PI_F) / M_PI_F * 180.0f; @@ -274,7 +274,7 @@ void frsky_send_frame3(int uart) orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_pos); /* send formatted frame */ - time_t time_gps = global_pos.time_gps_usec / 1000000; + time_t time_gps = global_pos.time_utc_usec / 1000000ULL; struct tm *tm_gps = gmtime(&time_gps); uint16_t hour_min = (tm_gps->tm_min << 8) | (tm_gps->tm_hour & 0xff); frsky_send_data(uart, FRSKY_ID_GPS_DAY_MONTH, tm_gps->tm_mday); diff --git a/src/drivers/gps/ashtech.cpp b/src/drivers/gps/ashtech.cpp index f1c5ca5495..99dbe5984f 100644 --- a/src/drivers/gps/ashtech.cpp +++ b/src/drivers/gps/ashtech.cpp @@ -112,8 +112,8 @@ int ASHTECH::handle_message(int len) warn("failed setting clock"); } - _gps_position->time_gps_usec = static_cast(epoch) * 1000000ULL; - _gps_position->time_gps_usec += usecs; + _gps_position->time_utc_usec = static_cast(epoch) * 1000000ULL; + _gps_position->time_utc_usec += usecs; _gps_position->timestamp_time = hrt_absolute_time(); } diff --git a/src/drivers/gps/mtk.cpp b/src/drivers/gps/mtk.cpp index c4f4f7bec9..d808b59a95 100644 --- a/src/drivers/gps/mtk.cpp +++ b/src/drivers/gps/mtk.cpp @@ -282,8 +282,8 @@ MTK::handle_message(gps_mtk_packet_t &packet) timeinfo_conversion_temp -= timeinfo.tm_sec * 1e3; time_t epoch = mktime(&timeinfo); - _gps_position->time_gps_usec = epoch * 1e6; //TODO: test this - _gps_position->time_gps_usec += timeinfo_conversion_temp * 1e3; + _gps_position->time_utc_usec = epoch * 1e6; //TODO: test this + _gps_position->time_utc_usec += timeinfo_conversion_temp * 1e3; _gps_position->timestamp_position = _gps_position->timestamp_time = hrt_absolute_time(); // Position and velocity update always at the same time diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index c67965487e..6b4e1630b0 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -759,8 +759,8 @@ UBX::payload_rx_done(void) warn("failed setting clock"); } - _gps_position->time_gps_usec = static_cast(epoch) * 1000000ULL; - _gps_position->time_gps_usec += _buf.payload_rx_nav_timeutc.nano / 1000; + _gps_position->time_utc_usec = static_cast(epoch) * 1000000ULL; + _gps_position->time_utc_usec += _buf.payload_rx_nav_timeutc.nano / 1000; } _gps_position->timestamp_time = hrt_absolute_time(); @@ -831,8 +831,8 @@ UBX::payload_rx_done(void) warn("failed setting clock"); } - _gps_position->time_gps_usec = static_cast(epoch) * 1000000ULL; - _gps_position->time_gps_usec += _buf.payload_rx_nav_timeutc.nano / 1000; + _gps_position->time_utc_usec = static_cast(epoch) * 1000000ULL; + _gps_position->time_utc_usec += _buf.payload_rx_nav_timeutc.nano / 1000; } _gps_position->timestamp_time = hrt_absolute_time(); diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index e7805daa91..c41777968e 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -1470,7 +1470,7 @@ FixedwingEstimator::task_main() map_projection_reproject(&_pos_ref, _local_pos.x, _local_pos.y, &est_lat, &est_lon); _global_pos.lat = est_lat; _global_pos.lon = est_lon; - _global_pos.time_gps_usec = _gps.time_gps_usec; + _global_pos.time_utc_usec = _gps.time_utc_usec; _global_pos.eph = _gps.eph; _global_pos.epv = _gps.epv; } diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 6f5adb5fe5..f109ceaf12 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -359,7 +359,7 @@ MavlinkReceiver::handle_message_optical_flow_rad(mavlink_message_t *msg) /* optical flow */ mavlink_optical_flow_rad_t flow; mavlink_msg_optical_flow_rad_decode(msg, &flow); - + enum Rotation flow_rot; param_get(param_find("SENS_FLOW_ROT"),&flow_rot); @@ -380,8 +380,8 @@ MavlinkReceiver::handle_message_optical_flow_rad(mavlink_message_t *msg) f.gyro_temperature = flow.temperature; /* rotate measurements according to parameter */ - float zeroval = 0.0f; - rotate_3f(flow_rot, f.pixel_flow_x_integral, f.pixel_flow_y_integral, zeroval); + float zeroval = 0.0f; + rotate_3f(flow_rot, f.pixel_flow_x_integral, f.pixel_flow_y_integral, zeroval); if (_flow_pub < 0) { _flow_pub = orb_advertise(ORB_ID(optical_flow), &f); @@ -1137,7 +1137,7 @@ MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg) memset(&hil_gps, 0, sizeof(hil_gps)); hil_gps.timestamp_time = timestamp; - hil_gps.time_gps_usec = gps.time_usec; + hil_gps.time_utc_usec = gps.time_usec; hil_gps.timestamp_position = timestamp; hil_gps.lat = gps.lat; 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 1962151fae..2f972fc9f2 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -282,13 +282,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) }; float w_gps_xy = 1.0f; float w_gps_z = 1.0f; - + float corr_vision[3][2] = { { 0.0f, 0.0f }, // N (pos, vel) { 0.0f, 0.0f }, // E (pos, vel) { 0.0f, 0.0f }, // D (pos, vel) }; - + float corr_sonar = 0.0f; float corr_sonar_filtered = 0.0f; @@ -650,13 +650,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) x_est[1] = vision.vx; y_est[0] = vision.y; y_est[1] = vision.vy; - /* only reset the z estimate if the z weight parameter is not zero */ + /* only reset the z estimate if the z weight parameter is not zero */ if (params.w_z_vision_p > MIN_VALID_W) { z_est[0] = vision.z; z_est[1] = vision.vz; } - + vision_valid = true; last_vision_x = vision.x; @@ -1166,7 +1166,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (local_pos.xy_global && local_pos.z_global) { /* publish global position */ global_pos.timestamp = t; - global_pos.time_gps_usec = gps.time_gps_usec; + global_pos.time_utc_usec = gps.time_utc_usec; double est_lat, est_lon; map_projection_reproject(&ref, local_pos.x, local_pos.y, &est_lat, &est_lon); diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 2daf73bf9e..2ce3d0097c 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1128,7 +1128,7 @@ int sdlog2_thread_main(int argc, char *argv[]) /* check GPS topic to get GPS time */ if (log_name_timestamp) { if (!orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf_gps_pos)) { - gps_time = buf_gps_pos.time_gps_usec; + gps_time = buf_gps_pos.time_utc_usec; } } @@ -1156,7 +1156,7 @@ int sdlog2_thread_main(int argc, char *argv[]) bool gps_pos_updated = copy_if_updated(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf_gps_pos); if (gps_pos_updated && log_name_timestamp) { - gps_time = buf_gps_pos.time_gps_usec; + gps_time = buf_gps_pos.time_utc_usec; } if (!logging_enabled) { @@ -1186,7 +1186,7 @@ int sdlog2_thread_main(int argc, char *argv[]) if (gps_pos_updated) { log_msg.msg_type = LOG_GPS_MSG; - log_msg.body.log_GPS.gps_time = buf_gps_pos.time_gps_usec; + log_msg.body.log_GPS.gps_time = buf_gps_pos.time_utc_usec; log_msg.body.log_GPS.fix_type = buf_gps_pos.fix_type; log_msg.body.log_GPS.eph = buf_gps_pos.eph; log_msg.body.log_GPS.epv = buf_gps_pos.epv; diff --git a/src/modules/uORB/topics/vehicle_global_position.h b/src/modules/uORB/topics/vehicle_global_position.h index c3bb3b8936..bc7046690b 100644 --- a/src/modules/uORB/topics/vehicle_global_position.h +++ b/src/modules/uORB/topics/vehicle_global_position.h @@ -62,7 +62,7 @@ */ struct vehicle_global_position_s { uint64_t timestamp; /**< Time of this estimate, in microseconds since system start */ - uint64_t time_gps_usec; /**< GPS timestamp in microseconds */ + uint64_t time_utc_usec; /**< GPS UTC timestamp in microseconds */ double lat; /**< Latitude in degrees */ double lon; /**< Longitude in degrees */ float alt; /**< Altitude AMSL in meters */ diff --git a/src/modules/uORB/topics/vehicle_gps_position.h b/src/modules/uORB/topics/vehicle_gps_position.h index 7888a50f4c..102914bbbb 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. It might be unavailable right after cold start, indicated by a value of 0 */ + uint64_t time_utc_usec; /**< Timestamp (microseconds, UTC), 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 */ }; diff --git a/src/modules/uavcan/sensors/gnss.cpp b/src/modules/uavcan/sensors/gnss.cpp index a375db37f8..571a6f1cd7 100644 --- a/src/modules/uavcan/sensors/gnss.cpp +++ b/src/modules/uavcan/sensors/gnss.cpp @@ -159,7 +159,7 @@ void UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure Date: Sun, 4 Jan 2015 11:52:32 +0100 Subject: [PATCH 358/652] Fix integer constants in mavlink app --- src/modules/mavlink/mavlink_receiver.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 24f1391760..1b24b9c52e 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -947,12 +947,12 @@ MavlinkReceiver::handle_message_system_time(mavlink_message_t *msg) clock_gettime(CLOCK_REALTIME, &tv); // date -d @1234567890: Sat Feb 14 02:31:30 MSK 2009 - bool onb_unix_valid = tv.tv_sec > 1234567890L; - bool ofb_unix_valid = time.time_unix_usec > 1234567890L * 1000; + bool onb_unix_valid = tv.tv_sec > 1234567890ULL; + bool ofb_unix_valid = time.time_unix_usec > 1234567890ULL * 1000ULL; if (!onb_unix_valid && ofb_unix_valid) { - tv.tv_sec = time.time_unix_usec / 1000000; - tv.tv_nsec = (time.time_unix_usec % 1000000) * 1000; + tv.tv_sec = time.time_unix_usec / 1000000ULL; + tv.tv_nsec = (time.time_unix_usec % 1000000ULL) * 1000ULL; clock_settime(CLOCK_REALTIME, &tv); warnx("[timesync] Set system time from SYSTEM_TIME message"); } From eefad6217dbcae56e44d142815f7559b17eaa9a8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 4 Jan 2015 12:05:40 +0100 Subject: [PATCH 359/652] GPS driver feedback cleanup --- src/drivers/gps/gps.cpp | 8 ++++---- src/drivers/gps/gps_helper.cpp | 11 +++++------ src/drivers/gps/mtk.cpp | 22 ++++++++++------------ src/drivers/gps/ubx.cpp | 2 +- 4 files changed, 20 insertions(+), 23 deletions(-) diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 47c907bd33..8d71767916 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -552,7 +552,7 @@ start(const char *uart_path, bool fake_gps, bool enable_sat_info) fd = open(GPS_DEVICE_PATH, O_RDONLY); if (fd < 0) { - errx(1, "Could not open device path: %s\n", GPS_DEVICE_PATH); + errx(1, "open: %s\n", GPS_DEVICE_PATH); goto fail; } @@ -565,7 +565,7 @@ fail: g_dev = nullptr; } - errx(1, "driver start failed"); + errx(1, "start failed"); } /** @@ -604,7 +604,7 @@ reset() err(1, "failed "); if (ioctl(fd, SENSORIOCRESET, 0) < 0) - err(1, "driver reset failed"); + err(1, "reset failed"); exit(0); } @@ -616,7 +616,7 @@ void info() { if (g_dev == nullptr) - errx(1, "driver not running"); + errx(1, "not running"); g_dev->print_info(); diff --git a/src/drivers/gps/gps_helper.cpp b/src/drivers/gps/gps_helper.cpp index 3b92f1bf45..05dfc99b76 100644 --- a/src/drivers/gps/gps_helper.cpp +++ b/src/drivers/gps/gps_helper.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -91,7 +91,7 @@ GPS_Helper::set_baudrate(const int &fd, unsigned baud) warnx("try baudrate: %d\n", speed); default: - warnx("ERROR: Unsupported baudrate: %d\n", baud); + warnx("ERR: baudrate: %d\n", baud); return -EINVAL; } @@ -109,20 +109,19 @@ GPS_Helper::set_baudrate(const int &fd, unsigned baud) /* set baud rate */ if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) { - warnx("ERROR setting config: %d (cfsetispeed)\n", termios_state); + warnx("ERR: %d (cfsetispeed)\n", termios_state); return -1; } if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) { - warnx("ERROR setting config: %d (cfsetospeed)\n", termios_state); + warnx("ERR: %d (cfsetospeed)\n", termios_state); return -1; } if ((termios_state = tcsetattr(fd, TCSANOW, &uart_config)) < 0) { - warnx("ERROR setting baudrate (tcsetattr)\n"); + warnx("ERR: %d (tcsetattr)\n", termios_state); return -1; } - /* XXX if resetting the parser here, ensure it does exist (check for null pointer) */ return 0; } diff --git a/src/drivers/gps/mtk.cpp b/src/drivers/gps/mtk.cpp index d808b59a95..c0c47073bc 100644 --- a/src/drivers/gps/mtk.cpp +++ b/src/drivers/gps/mtk.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -73,39 +73,38 @@ MTK::configure(unsigned &baudrate) /* Write config messages, don't wait for an answer */ if (strlen(MTK_OUTPUT_5HZ) != write(_fd, MTK_OUTPUT_5HZ, strlen(MTK_OUTPUT_5HZ))) { - warnx("mtk: config write failed"); - return -1; + goto errout; } usleep(10000); if (strlen(MTK_SET_BINARY) != write(_fd, MTK_SET_BINARY, strlen(MTK_SET_BINARY))) { - warnx("mtk: config write failed"); - return -1; + goto errout; } usleep(10000); if (strlen(SBAS_ON) != write(_fd, SBAS_ON, strlen(SBAS_ON))) { - warnx("mtk: config write failed"); - return -1; + goto errout; } usleep(10000); if (strlen(WAAS_ON) != write(_fd, WAAS_ON, strlen(WAAS_ON))) { - warnx("mtk: config write failed"); - return -1; + goto errout; } usleep(10000); if (strlen(MTK_NAVTHRES_OFF) != write(_fd, MTK_NAVTHRES_OFF, strlen(MTK_NAVTHRES_OFF))) { - warnx("mtk: config write failed"); - return -1; + goto errout; } return 0; + +errout: + warnx("mtk: config write failed"); + return -1; } int @@ -222,7 +221,6 @@ MTK::parse_char(uint8_t b, gps_mtk_packet_t &packet) ret = 1; } else { - warnx("MTK Checksum invalid"); ret = -1; } diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index 6b4e1630b0..05809b3617 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012, 2013, 2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions From 06aa3fceee670ca510cea3d148634c0daf3c7911 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 4 Jan 2015 12:05:59 +0100 Subject: [PATCH 360/652] Hott driver: Feedback cleanup --- src/drivers/hott/hott_sensors/hott_sensors.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/hott/hott_sensors/hott_sensors.cpp b/src/drivers/hott/hott_sensors/hott_sensors.cpp index 4b8e0c0b0c..1aade450b1 100644 --- a/src/drivers/hott/hott_sensors/hott_sensors.cpp +++ b/src/drivers/hott/hott_sensors/hott_sensors.cpp @@ -157,7 +157,7 @@ hott_sensors_thread_main(int argc, char *argv[]) /* enable UART, writes potentially an empty buffer, but multiplexing is disabled */ const int uart = open_uart(device); if (uart < 0) { - errx(1, "Failed opening HoTT UART, exiting."); + errx(1, "Open fail, exiting."); thread_running = false; } From 313af1760baf35a73201d3526405d81c4123e625 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 4 Jan 2015 12:06:21 +0100 Subject: [PATCH 361/652] mavlink: reduce verbosity --- src/modules/mavlink/mavlink_receiver.cpp | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 090b629750..93a2972855 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -263,7 +263,7 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg) //check for MAVLINK terminate command if (cmd_mavlink.command == MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN && ((int)cmd_mavlink.param1) == 3) { /* This is the link shutdown command, terminate mavlink */ - warnx("terminated by remote command"); + warnx("terminated by remote"); fflush(stdout); usleep(50000); @@ -273,7 +273,7 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg) } else { if (msg->sysid == mavlink_system.sysid && msg->compid == mavlink_system.compid) { - warnx("ignoring CMD spoofed with same SYS/COMP (%d/%d) ID", + warnx("ignoring CMD with same SYS/COMP (%d/%d) ID", mavlink_system.sysid, mavlink_system.compid); return; } @@ -319,7 +319,7 @@ MavlinkReceiver::handle_message_command_int(mavlink_message_t *msg) //check for MAVLINK terminate command if (cmd_mavlink.command == MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN && ((int)cmd_mavlink.param1) == 3) { /* This is the link shutdown command, terminate mavlink */ - warnx("terminated by remote command"); + warnx("terminated by remote"); fflush(stdout); usleep(50000); @@ -329,7 +329,7 @@ MavlinkReceiver::handle_message_command_int(mavlink_message_t *msg) } else { if (msg->sysid == mavlink_system.sysid && msg->compid == mavlink_system.compid) { - warnx("ignoring CMD spoofed with same SYS/COMP (%d/%d) ID", + warnx("ignoring CMD with same SYS/COMP (%d/%d) ID", mavlink_system.sysid, mavlink_system.compid); return; } @@ -880,7 +880,7 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg) manual.r = man.r / 1000.0f; manual.z = man.z / 1000.0f; - warnx("pitch: %.2f, roll: %.2f, yaw: %.2f, throttle: %.2f", (double)manual.x, (double)manual.y, (double)manual.r, (double)manual.z); + // warnx("pitch: %.2f, roll: %.2f, yaw: %.2f, throttle: %.2f", (double)manual.x, (double)manual.y, (double)manual.r, (double)manual.z); if (_manual_pub < 0) { _manual_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual); @@ -954,7 +954,7 @@ MavlinkReceiver::handle_message_system_time(mavlink_message_t *msg) tv.tv_sec = time.time_unix_usec / 1000000ULL; tv.tv_nsec = (time.time_unix_usec % 1000000ULL) * 1000ULL; clock_settime(CLOCK_REALTIME, &tv); - warnx("[timesync] Set system time from SYSTEM_TIME message"); + warnx("[timesync] synced.."); } } @@ -985,7 +985,7 @@ MavlinkReceiver::handle_message_timesync(mavlink_message_t *msg) if (dt > 10000000 || dt < -1000000) { // 10 millisecond skew _time_offset = (tsync.ts1 + now_ns - tsync.tc1*2)/2; - warnx("[timesync] Companion clock offset is skewed. Hard-setting offset"); + warnx("[timesync] Resetting."); } else { smooth_time_offset(offset_ns); @@ -1186,7 +1186,7 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) /* print HIL sensors rate */ if ((timestamp - _old_timestamp) > 10000000) { - printf("receiving HIL sensors at %d hz\n", _hil_frames / 10); + // printf("receiving HIL sensors at %d hz\n", _hil_frames / 10); _old_timestamp = timestamp; _hil_frames = 0; } From b6f6a99799514ad884f27ebc4b6d925952812633 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 4 Jan 2015 12:08:25 +0100 Subject: [PATCH 362/652] FW attitude control: Build with space optimization --- src/modules/fw_att_control/module.mk | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/fw_att_control/module.mk b/src/modules/fw_att_control/module.mk index 89c6494c53..3661a171fc 100644 --- a/src/modules/fw_att_control/module.mk +++ b/src/modules/fw_att_control/module.mk @@ -41,3 +41,5 @@ SRCS = fw_att_control_main.cpp \ fw_att_control_params.c MODULE_STACKSIZE = 1200 + +MAXOPTIMIZATION = -Os From bccc37cfb9175cf38f2769c1654f5b7ec3a88618 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 4 Jan 2015 12:31:21 +0100 Subject: [PATCH 363/652] send MANUAL_CONTROL stream over usb --- ROMFS/px4fmu_common/init.d/rc.usb | 2 ++ 1 file changed, 2 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/rc.usb b/ROMFS/px4fmu_common/init.d/rc.usb index b3de8e5907..2dfe11ce61 100644 --- a/ROMFS/px4fmu_common/init.d/rc.usb +++ b/ROMFS/px4fmu_common/init.d/rc.usb @@ -26,6 +26,8 @@ usleep 100000 mavlink stream -d /dev/ttyACM0 -s POSITION_TARGET_GLOBAL_INT -r 10 usleep 100000 mavlink stream -d /dev/ttyACM0 -s LOCAL_POSITION_NED -r 30 +usleep 100000 +mavlink stream -d /dev/ttyACM0 -s MANUAL_CONTROL -r 5 # Exit shell to make it available to MAVLink exit From 3616c83f88078ecf827fb844ffdbacdd442f9bad Mon Sep 17 00:00:00 2001 From: "M.H.Kabir" Date: Sun, 4 Jan 2015 17:38:19 +0530 Subject: [PATCH 364/652] missing zero --- src/modules/mavlink/mavlink_receiver.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 93a2972855..9b3274431c 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -983,7 +983,7 @@ MavlinkReceiver::handle_message_timesync(mavlink_message_t *msg) int64_t offset_ns = (9*_time_offset + (tsync.ts1 + now_ns - tsync.tc1*2)/2 )/10; // average offset int64_t dt = _time_offset - offset_ns; - if (dt > 10000000 || dt < -1000000) { // 10 millisecond skew + if (dt > 10000000 || dt < -10000000) { // 10 millisecond skew _time_offset = (tsync.ts1 + now_ns - tsync.tc1*2)/2; warnx("[timesync] Resetting."); From 460402e07ac84315a32cc42dae295c88a6297924 Mon Sep 17 00:00:00 2001 From: Ban Siesta Date: Sun, 4 Jan 2015 13:31:18 +0000 Subject: [PATCH 365/652] sdlog2: checks and warnings to make sure there is space left on the SD card --- src/modules/sdlog2/sdlog2.c | 50 +++++++++++++++++++++++++++++++++++++ 1 file changed, 50 insertions(+) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 2ce3d0097c..da181dad5c 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -39,12 +39,14 @@ * * @author Lorenz Meier * @author Anton Babushkin + * @author Ban Siesta */ #include #include #include #include +#include #include #include #include @@ -158,6 +160,7 @@ static const int MIN_BYTES_TO_WRITE = 512; static bool _extended_logging = false; +static const char *mountpoint = "/fs/microsd"; static const char *log_root = "/fs/microsd/log"; static int mavlink_fd = -1; struct logbuffer_s lb; @@ -185,6 +188,9 @@ static bool log_name_timestamp = false; /* helper flag to track system state changes */ static bool flag_system_armed = false; +/* flag if warning about MicroSD card being almost full has already been sent */ +static bool space_warning_sent = false; + static pthread_t logwriter_pthread = 0; static pthread_attr_t logwriter_attr; @@ -244,6 +250,11 @@ static bool file_exist(const char *filename); static int file_copy(const char *file_old, const char *file_new); +/** + * Check if there is still free space available + */ +static int check_free_space(void); + static void handle_command(struct vehicle_command_s *cmd); static void handle_status(struct vehicle_status_s *cmd); @@ -547,6 +558,7 @@ static void *logwriter_thread(void *arg) pthread_mutex_unlock(&logbuffer_mutex); if (available > 0) { + /* do heavy IO here */ if (available > MAX_WRITE_CHUNK) { n = MAX_WRITE_CHUNK; @@ -584,6 +596,12 @@ static void *logwriter_thread(void *arg) if (++poll_count == 10) { fsync(log_fd); poll_count = 0; + + /* check if space is available, if not stop everything */ + if (check_free_space() != OK) { + logwriter_should_exit = true; + main_thread_should_exit = true; + } } } @@ -607,6 +625,7 @@ void sdlog2_start_log() errx(1, "error creating log dir"); } + /* initialize statistics counter */ log_bytes_written = 0; start_time = hrt_absolute_time(); @@ -899,6 +918,12 @@ int sdlog2_thread_main(int argc, char *argv[]) } + + if (check_free_space() != OK) { + errx(1, "error MicroSD almost full"); + } + + /* create log root dir */ int mkdir_ret = mkdir(log_root, S_IRWXU | S_IRWXG | S_IRWXO); @@ -1822,6 +1847,31 @@ int file_copy(const char *file_old, const char *file_new) return OK; } +int check_free_space() +{ + /* use statfs to determine the number of blocks left */ + FAR struct statfs statfs_buf; + if (statfs(mountpoint, &statfs_buf) != OK) { + warnx("could not determine statfs"); + return ERROR; + } + + /* use a threshold of 4 MiB */ + if (statfs_buf.f_bavail < (int)(4*1024*1024/statfs_buf.f_bsize)) { + warnx("no more space on MicroSD (less than 4 MiB)"); + mavlink_log_critical(mavlink_fd, "[sdlog2] no more space left on MicroSD"); + return ERROR; + + /* use a threshold of 100 MiB to send a warning */ + } else if (!space_warning_sent && statfs_buf.f_bavail < (int)(100*1024*1024/statfs_buf.f_bsize)) { + warnx("space on MicroSD running out (less than 100MiB)"); + mavlink_log_critical(mavlink_fd, "[sdlog2] space on MicroSD running out"); + space_warning_sent = true; + } + + return OK; +} + void handle_command(struct vehicle_command_s *cmd) { int param; From 3b418a5a595f0e48c4365217815372d9b278f64c Mon Sep 17 00:00:00 2001 From: Ban Siesta Date: Sun, 4 Jan 2015 13:48:11 +0000 Subject: [PATCH 366/652] sdlog2: concatenate path strings --- src/modules/sdlog2/sdlog2.c | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index da181dad5c..3b3b5c7a5d 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -160,8 +160,9 @@ static const int MIN_BYTES_TO_WRITE = 512; static bool _extended_logging = false; -static const char *mountpoint = "/fs/microsd"; -static const char *log_root = "/fs/microsd/log"; +#define MOUNTPOINT "/fs/microsd" +static const char *mountpoint = MOUNTPOINT; +static const char *log_root = MOUNTPOINT "/log"; static int mavlink_fd = -1; struct logbuffer_s lb; From 04c273bca6c99f31fd04741234d9c8efa849b553 Mon Sep 17 00:00:00 2001 From: Ban Siesta Date: Sun, 4 Jan 2015 14:06:33 +0000 Subject: [PATCH 367/652] sdlog2: slow down the free space check a bit more --- src/modules/sdlog2/sdlog2.c | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 3b3b5c7a5d..8537d03b36 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -594,16 +594,19 @@ static void *logwriter_thread(void *arg) should_wait = true; } - if (++poll_count == 10) { + /* slow down fsync */ + if (poll_count % 10 == 0) { fsync(log_fd); - poll_count = 0; + /* slow down the check even more, and don't do both at the same time */ + } else if (poll_count % 1000 == 5) { /* check if space is available, if not stop everything */ if (check_free_space() != OK) { logwriter_should_exit = true; main_thread_should_exit = true; } } + poll_count++; } fsync(log_fd); From 7ce0a56d75f210a947db13b61c1a3c447244850e Mon Sep 17 00:00:00 2001 From: Ban Siesta Date: Sun, 4 Jan 2015 14:06:52 +0000 Subject: [PATCH 368/652] sdlog2: some comments --- src/modules/sdlog2/sdlog2.c | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 8537d03b36..16ca728913 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1864,12 +1864,14 @@ int check_free_space() if (statfs_buf.f_bavail < (int)(4*1024*1024/statfs_buf.f_bsize)) { warnx("no more space on MicroSD (less than 4 MiB)"); mavlink_log_critical(mavlink_fd, "[sdlog2] no more space left on MicroSD"); + /* we do not need a flag to remember that we sent this warning because we will exit anyway */ return ERROR; /* use a threshold of 100 MiB to send a warning */ } else if (!space_warning_sent && statfs_buf.f_bavail < (int)(100*1024*1024/statfs_buf.f_bsize)) { warnx("space on MicroSD running out (less than 100MiB)"); mavlink_log_critical(mavlink_fd, "[sdlog2] space on MicroSD running out"); + /* we don't want to flood the user with warnings */ space_warning_sent = true; } From abc8bf2e84d566b8760896e3c2fc60a871264c71 Mon Sep 17 00:00:00 2001 From: Ban Siesta Date: Sun, 4 Jan 2015 14:07:52 +0000 Subject: [PATCH 369/652] sdlog2: adapt threshold --- src/modules/sdlog2/sdlog2.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 16ca728913..efd6594b58 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1860,9 +1860,9 @@ int check_free_space() return ERROR; } - /* use a threshold of 4 MiB */ - if (statfs_buf.f_bavail < (int)(4*1024*1024/statfs_buf.f_bsize)) { - warnx("no more space on MicroSD (less than 4 MiB)"); + /* use a threshold of 10 MiB */ + if (statfs_buf.f_bavail < (int)(10*1024*1024/statfs_buf.f_bsize)) { + warnx("no more space on MicroSD (less than 10 MiB)"); mavlink_log_critical(mavlink_fd, "[sdlog2] no more space left on MicroSD"); /* we do not need a flag to remember that we sent this warning because we will exit anyway */ return ERROR; From 0142002737637e80071487a632b151d47a11a2a0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 4 Jan 2015 15:33:32 +0100 Subject: [PATCH 370/652] mavlink log: Macro added to log to mavlink and console in one go --- src/include/mavlink/mavlink_log.h | 36 +++++++++++++++++++++++++++++-- src/modules/sdlog2/sdlog2.c | 3 +-- 2 files changed, 35 insertions(+), 4 deletions(-) diff --git a/src/include/mavlink/mavlink_log.h b/src/include/mavlink/mavlink_log.h index 6d56c546a7..4962d029d5 100644 --- a/src/include/mavlink/mavlink_log.h +++ b/src/include/mavlink/mavlink_log.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -35,7 +35,7 @@ * @file mavlink_log.h * MAVLink text logging. * - * @author Lorenz Meier + * @author Lorenz Meier */ #ifndef MAVLINK_LOG @@ -99,6 +99,38 @@ __EXPORT void mavlink_vasprintf(int _fd, int severity, const char *fmt, ...); */ #define mavlink_log_info(_fd, _text, ...) mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_INFO, _text, ##__VA_ARGS__); +/** + * Send a mavlink emergency message and print to console. + * + * @param _fd A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0); + * @param _text The text to log; + */ +#define mavlink_and_console_log_emergency(_fd, _text, ...) mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_EMERGENCY, _text, ##__VA_ARGS__); \ + fprintf(stderr, "telem> "); \ + fprintf(stderr, _text, ##__VA_ARGS__); \ + fprintf(stderr, "\n"); + +/** + * Send a mavlink critical message and print to console. + * + * @param _fd A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0); + * @param _text The text to log; + */ +#define mavlink_and_console_log_critical(_fd, _text, ...) mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_CRITICAL, _text, ##__VA_ARGS__); \ + fprintf(stderr, "telem> "); \ + fprintf(stderr, _text, ##__VA_ARGS__); \ + fprintf(stderr, "\n"); + +/** + * Send a mavlink emergency message and print to console. + * + * @param _fd A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0); + * @param _text The text to log; + */ +#define mavlink_and_console_log_info(_fd, _text, ...) mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_INFO, _text, ##__VA_ARGS__); \ + fprintf(stderr, "telem> "); \ + fprintf(stderr, _text, ##__VA_ARGS__); \ + fprintf(stderr, "\n"); struct mavlink_logmessage { char text[MAVLINK_LOG_MAXLEN + 1]; diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index efd6594b58..218145809a 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -396,8 +396,7 @@ int create_log_dir() } /* print logging path, important to find log file later */ - warnx("log dir: %s", log_dir); - mavlink_log_info(mavlink_fd, "[sdlog2] log dir: %s", log_dir); + mavlink_and_console_log_info(mavlink_fd, "[sdlog2] log dir: %s", log_dir); return 0; } From 7d0c89ded78161d622d40c9dd7cd7b4ac7a8c8d6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 4 Jan 2015 15:53:07 +0100 Subject: [PATCH 371/652] mavlink app: Abort writing to text log on microSD once SD is not writeabele any more after 5 tries. Fix to first message write. --- src/modules/mavlink/mavlink_messages.cpp | 25 ++++++++++++++++++++---- 1 file changed, 21 insertions(+), 4 deletions(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 89a40d0325..6765100c70 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -342,6 +342,8 @@ private: MavlinkStreamStatustext(MavlinkStreamStatustext &); MavlinkStreamStatustext& operator = (const MavlinkStreamStatustext &); FILE *fp = nullptr; + unsigned write_err_count = 0; + static const unsigned write_err_threshold = 5; protected: explicit MavlinkStreamStatustext(Mavlink *mavlink) : MavlinkStream(mavlink) @@ -370,10 +372,21 @@ protected: /* write log messages in first instance to disk */ if (_mavlink->get_instance_id() == 0) { if (fp) { - fputs(msg.text, fp); - fputs("\n", fp); - fsync(fileno(fp)); - } else { + if (EOF == fputs(msg.text, fp)) { + write_err_count++; + } else { + write_err_count = 0; + } + + if (write_err_count >= write_err_threshold) { + (void)fclose(fp); + fp = nullptr; + } else { + (void)fputs("\n", fp); + (void)fsync(fileno(fp)); + } + + } else if (write_err_count < write_err_threshold) { /* string to hold the path to the log */ char log_file_name[32] = ""; char log_file_path[64] = ""; @@ -389,6 +402,10 @@ protected: strftime(log_file_name, sizeof(log_file_name), "msgs_%Y_%m_%d_%H_%M_%S.txt", &tt); snprintf(log_file_path, sizeof(log_file_path), "/fs/microsd/%s", log_file_name); fp = fopen(log_file_path, "ab"); + + /* write first message */ + fputs(msg.text, fp); + fputs("\n", fp); } } } From 1f181fb03f5eb467d5e1f0933164363269986844 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 4 Jan 2015 16:14:41 +0100 Subject: [PATCH 372/652] SDLOG2: Use RTC, not GPS time for file naming --- src/modules/sdlog2/sdlog2.c | 48 +++++++++++++++++++++++-------------- 1 file changed, 30 insertions(+), 18 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 2ce3d0097c..10b6c903f1 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -103,6 +103,8 @@ #include "sdlog2_format.h" #include "sdlog2_messages.h" +#define PX4_EPOCH_SECS 1234567890ULL + /** * Logging rate. * @@ -338,13 +340,16 @@ int create_log_dir() uint16_t dir_number = 1; // start with dir sess001 int mkdir_ret; - if (log_name_timestamp && gps_time != 0) { - /* use GPS date for log dir naming: e.g. /fs/microsd/2014-01-19 */ - time_t gps_time_sec = gps_time / 1000000; - struct tm t; - gmtime_r(&gps_time_sec, &t); + timespec ts; + clock_gettime(CLOCK_REALTIME, &ts); + /* use RTC time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.bin */ + time_t utc_time_sec = ts.tv_sec + (ts.tv_nsec / 1e9); + struct tm tt; + struct tm *ttp = gmtime_r(&utc_time_sec, &tt); + + if (log_name_timestamp && ttp && (utc_time_sec > PX4_EPOCH_SECS)) { int n = snprintf(log_dir, sizeof(log_dir), "%s/", log_root); - strftime(log_dir + n, sizeof(log_dir) - n, "%Y-%m-%d", &t); + strftime(log_dir + n, sizeof(log_dir) - n, "%Y-%m-%d", &tt); mkdir_ret = mkdir(log_dir, S_IRWXU | S_IRWXG | S_IRWXO); if (mkdir_ret == OK) { @@ -395,12 +400,16 @@ int open_log_file() char log_file_name[32] = ""; char log_file_path[64] = ""; - if (log_name_timestamp && gps_time != 0) { - /* use GPS time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.bin */ - time_t gps_time_sec = gps_time / 1000000; - struct tm t; - gmtime_r(&gps_time_sec, &t); - strftime(log_file_name, sizeof(log_file_name), "%H_%M_%S.bin", &t); + timespec ts; + clock_gettime(CLOCK_REALTIME, &ts); + /* use RTC time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.bin */ + time_t utc_time_sec = ts.tv_sec + (ts.tv_nsec / 1e9); + struct tm tt; + struct tm *ttp = gmtime_r(&utc_time_sec, &tt); + + /* start logging if we have a valid time and the time is not in the past */ + if (log_name_timestamp && ttp && (utc_time_sec > PX4_EPOCH_SECS)) { + strftime(log_file_name, sizeof(log_file_name), "%H_%M_%S.bin", &tt); snprintf(log_file_path, sizeof(log_file_path), "%s/%s", log_dir, log_file_name); } else { @@ -446,12 +455,15 @@ int open_perf_file(const char* str) char log_file_name[32] = ""; char log_file_path[64] = ""; - if (log_name_timestamp && gps_time != 0) { - /* use GPS time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.bin */ - time_t gps_time_sec = gps_time / 1000000; - struct tm t; - gmtime_r(&gps_time_sec, &t); - strftime(log_file_name, sizeof(log_file_name), "perf%H_%M_%S.txt", &t); + timespec ts; + clock_gettime(CLOCK_REALTIME, &ts); + /* use RTC time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.bin */ + time_t utc_time_sec = ts.tv_sec + (ts.tv_nsec / 1e9); + struct tm tt; + struct tm *ttp = gmtime_r(&utc_time_sec, &tt); + + if (log_name_timestamp && ttp && (utc_time_sec > PX4_EPOCH_SECS)) { + strftime(log_file_name, sizeof(log_file_name), "perf%H_%M_%S.txt", &tt); snprintf(log_file_path, sizeof(log_file_path), "%s/%s_%s", log_dir, str, log_file_name); } else { From 0d103dcb383f29d77753ee1e7ab49cc7a0f76fb3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 4 Jan 2015 16:24:02 +0100 Subject: [PATCH 373/652] sdlog2: Use correct struct --- src/modules/sdlog2/sdlog2.c | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 10b6c903f1..c3952b77db 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -57,6 +57,7 @@ #include #include #include +#include #include @@ -340,7 +341,7 @@ int create_log_dir() uint16_t dir_number = 1; // start with dir sess001 int mkdir_ret; - timespec ts; + struct timespec ts; clock_gettime(CLOCK_REALTIME, &ts); /* use RTC time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.bin */ time_t utc_time_sec = ts.tv_sec + (ts.tv_nsec / 1e9); @@ -400,7 +401,7 @@ int open_log_file() char log_file_name[32] = ""; char log_file_path[64] = ""; - timespec ts; + struct timespec ts; clock_gettime(CLOCK_REALTIME, &ts); /* use RTC time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.bin */ time_t utc_time_sec = ts.tv_sec + (ts.tv_nsec / 1e9); @@ -455,7 +456,7 @@ int open_perf_file(const char* str) char log_file_name[32] = ""; char log_file_path[64] = ""; - timespec ts; + struct timespec ts; clock_gettime(CLOCK_REALTIME, &ts); /* use RTC time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.bin */ time_t utc_time_sec = ts.tv_sec + (ts.tv_nsec / 1e9); From 1cc92c03612c504414092bc6742bb17c812ce9c6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 4 Jan 2015 16:24:43 +0100 Subject: [PATCH 374/652] GPS driver: Require valid minimum time to allow setting the wall clock. Protection against nulled time fields --- src/drivers/gps/ashtech.cpp | 27 ++++++++++-------- src/drivers/gps/gps_helper.h | 2 ++ src/drivers/gps/ubx.cpp | 54 +++++++++++++++++++++--------------- 3 files changed, 50 insertions(+), 33 deletions(-) diff --git a/src/drivers/gps/ashtech.cpp b/src/drivers/gps/ashtech.cpp index 99dbe5984f..a50767c22c 100644 --- a/src/drivers/gps/ashtech.cpp +++ b/src/drivers/gps/ashtech.cpp @@ -99,21 +99,26 @@ int ASHTECH::handle_message(int len) timeinfo.tm_sec = int(ashtech_sec); time_t epoch = mktime(&timeinfo); - uint64_t usecs = static_cast((ashtech_sec - static_cast(ashtech_sec))) * 1e6; + if (epoch > GPS_EPOCH_SECS) { + uint64_t usecs = static_cast((ashtech_sec - static_cast(ashtech_sec))) * 1e6; - // FMUv2+ boards have a hardware RTC, but GPS helps us to configure it - // and control its drift. Since we rely on the HRT for our monotonic - // clock, updating it from time to time is safe. + // FMUv2+ boards have a hardware RTC, but GPS helps us to configure it + // and control its drift. Since we rely on the HRT for our monotonic + // clock, updating it from time to time is safe. - timespec ts; - ts.tv_sec = epoch; - ts.tv_nsec = usecs * 1000; - if (clock_settime(CLOCK_REALTIME, &ts)) { - warn("failed setting clock"); + timespec ts; + ts.tv_sec = epoch; + ts.tv_nsec = usecs * 1000; + if (clock_settime(CLOCK_REALTIME, &ts)) { + warn("failed setting clock"); + } + + _gps_position->time_utc_usec = static_cast(epoch) * 1000000ULL; + _gps_position->time_utc_usec += usecs; + } else { + _gps_position->time_utc_usec = 0; } - _gps_position->time_utc_usec = static_cast(epoch) * 1000000ULL; - _gps_position->time_utc_usec += usecs; _gps_position->timestamp_time = hrt_absolute_time(); } diff --git a/src/drivers/gps/gps_helper.h b/src/drivers/gps/gps_helper.h index 3623397b2d..0ce05fcb50 100644 --- a/src/drivers/gps/gps_helper.h +++ b/src/drivers/gps/gps_helper.h @@ -43,6 +43,8 @@ #include #include +#define GPS_EPOCH_SECS 1234567890ULL + class GPS_Helper { public: diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index 05809b3617..e197059db1 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -748,19 +748,23 @@ UBX::payload_rx_done(void) timeinfo.tm_sec = _buf.payload_rx_nav_pvt.sec; time_t epoch = mktime(&timeinfo); - // FMUv2+ boards have a hardware RTC, but GPS helps us to configure it - // and control its drift. Since we rely on the HRT for our monotonic - // clock, updating it from time to time is safe. + if (epoch > GPS_EPOCH_SECS) { + // FMUv2+ boards have a hardware RTC, but GPS helps us to configure it + // and control its drift. Since we rely on the HRT for our monotonic + // clock, updating it from time to time is safe. - timespec ts; - ts.tv_sec = epoch; - ts.tv_nsec = _buf.payload_rx_nav_pvt.nano; - if (clock_settime(CLOCK_REALTIME, &ts)) { - warn("failed setting clock"); + timespec ts; + ts.tv_sec = epoch; + ts.tv_nsec = _buf.payload_rx_nav_pvt.nano; + if (clock_settime(CLOCK_REALTIME, &ts)) { + warn("failed setting clock"); + } + + _gps_position->time_utc_usec = static_cast(epoch) * 1000000ULL; + _gps_position->time_utc_usec += _buf.payload_rx_nav_timeutc.nano / 1000; + } else { + _gps_position->time_utc_usec = 0; } - - _gps_position->time_utc_usec = static_cast(epoch) * 1000000ULL; - _gps_position->time_utc_usec += _buf.payload_rx_nav_timeutc.nano / 1000; } _gps_position->timestamp_time = hrt_absolute_time(); @@ -820,19 +824,25 @@ UBX::payload_rx_done(void) timeinfo.tm_sec = _buf.payload_rx_nav_timeutc.sec; time_t epoch = mktime(&timeinfo); - // FMUv2+ boards have a hardware RTC, but GPS helps us to configure it - // and control its drift. Since we rely on the HRT for our monotonic - // clock, updating it from time to time is safe. + // only set the time if it makes sense - timespec ts; - ts.tv_sec = epoch; - ts.tv_nsec = _buf.payload_rx_nav_timeutc.nano; - if (clock_settime(CLOCK_REALTIME, &ts)) { - warn("failed setting clock"); + if (epoch > GPS_EPOCH_SECS) { + // FMUv2+ boards have a hardware RTC, but GPS helps us to configure it + // and control its drift. Since we rely on the HRT for our monotonic + // clock, updating it from time to time is safe. + + timespec ts; + ts.tv_sec = epoch; + ts.tv_nsec = _buf.payload_rx_nav_timeutc.nano; + if (clock_settime(CLOCK_REALTIME, &ts)) { + warn("failed setting clock"); + } + + _gps_position->time_utc_usec = static_cast(epoch) * 1000000ULL; + _gps_position->time_utc_usec += _buf.payload_rx_nav_timeutc.nano / 1000; + } else { + _gps_position->time_utc_usec = 0; } - - _gps_position->time_utc_usec = static_cast(epoch) * 1000000ULL; - _gps_position->time_utc_usec += _buf.payload_rx_nav_timeutc.nano / 1000; } _gps_position->timestamp_time = hrt_absolute_time(); From c47e8c22688ad3ac358c71337ef8cb0feeeba409 Mon Sep 17 00:00:00 2001 From: "M.H.Kabir" Date: Sun, 4 Jan 2015 21:29:05 +0530 Subject: [PATCH 375/652] Add suffixes, constant --- src/modules/mavlink/mavlink_receiver.cpp | 16 ++++++++++------ src/modules/mavlink/mavlink_receiver.h | 2 ++ 2 files changed, 12 insertions(+), 6 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 9b3274431c..d84b70e46a 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -947,14 +947,18 @@ MavlinkReceiver::handle_message_system_time(mavlink_message_t *msg) clock_gettime(CLOCK_REALTIME, &tv); // date -d @1234567890: Sat Feb 14 02:31:30 MSK 2009 - bool onb_unix_valid = tv.tv_sec > 1234567890ULL; - bool ofb_unix_valid = time.time_unix_usec > 1234567890ULL * 1000ULL; + bool onb_unix_valid = tv.tv_sec > PX4_EPOCH_SECS; + bool ofb_unix_valid = time.time_unix_usec > PX4_EPOCH_SECS * 1000ULL; if (!onb_unix_valid && ofb_unix_valid) { tv.tv_sec = time.time_unix_usec / 1000000ULL; tv.tv_nsec = (time.time_unix_usec % 1000000ULL) * 1000ULL; - clock_settime(CLOCK_REALTIME, &tv); - warnx("[timesync] synced.."); + if(clock_settime(CLOCK_REALTIME, &tv)) { + warn("failed setting clock"); + } + else { + warnx("[timesync] UTC time synced."); + } } } @@ -983,9 +987,9 @@ MavlinkReceiver::handle_message_timesync(mavlink_message_t *msg) int64_t offset_ns = (9*_time_offset + (tsync.ts1 + now_ns - tsync.tc1*2)/2 )/10; // average offset int64_t dt = _time_offset - offset_ns; - if (dt > 10000000 || dt < -10000000) { // 10 millisecond skew + if (dt > 10000000LL || dt < -10000000LL) { // 10 millisecond skew _time_offset = (tsync.ts1 + now_ns - tsync.tc1*2)/2; - warnx("[timesync] Resetting."); + warnx("[timesync] Resetting offset sync."); } else { smooth_time_offset(offset_ns); diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index a677e75cd9..4afc9b3721 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -75,6 +75,8 @@ #include "mavlink_ftp.h" +#define PX4_EPOCH_SECS 1234567890ULL + class Mavlink; class MavlinkReceiver From 3df73cde3e3bd092cdf28990c0736001681a9289 Mon Sep 17 00:00:00 2001 From: "M.H.Kabir" Date: Sun, 4 Jan 2015 22:38:47 +0530 Subject: [PATCH 376/652] Fix offset calculation. --- src/modules/mavlink/mavlink_receiver.cpp | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index d84b70e46a..97108270c5 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -969,7 +969,7 @@ MavlinkReceiver::handle_message_timesync(mavlink_message_t *msg) mavlink_timesync_t tsync; mavlink_msg_timesync_decode(msg, &tsync); - uint64_t now_ns = hrt_absolute_time() * 1000 ; + uint64_t now_ns = hrt_absolute_time() * 1000LL ; if (tsync.tc1 == 0) { @@ -984,13 +984,12 @@ MavlinkReceiver::handle_message_timesync(mavlink_message_t *msg) } else if (tsync.tc1 > 0) { - int64_t offset_ns = (9*_time_offset + (tsync.ts1 + now_ns - tsync.tc1*2)/2 )/10; // average offset + int64_t offset_ns = (tsync.ts1 + now_ns - tsync.tc1*2)/2 ; int64_t dt = _time_offset - offset_ns; if (dt > 10000000LL || dt < -10000000LL) { // 10 millisecond skew - _time_offset = (tsync.ts1 + now_ns - tsync.tc1*2)/2; - warnx("[timesync] Resetting offset sync."); - + _time_offset = offset_ns; + warnx("[timesync] Hard setting offset."); } else { smooth_time_offset(offset_ns); } @@ -1473,7 +1472,7 @@ uint64_t MavlinkReceiver::to_hrt(uint64_t usec) void MavlinkReceiver::smooth_time_offset(uint64_t offset_ns) { - /* alpha = 0.75 fixed for now. The closer alpha is to 1.0, + /* alpha = 0.6 fixed for now. The closer alpha is to 1.0, * the faster the moving average updates in response to * new offset samples. */ From 55741be473e1aafddffbd621ccbbc90e88fbe669 Mon Sep 17 00:00:00 2001 From: Anton Matosov Date: Mon, 17 Nov 2014 11:29:20 +0200 Subject: [PATCH 377/652] Made it possible to specify yaw scale for the copter --- src/modules/systemlib/mixer/multi_tables | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/modules/systemlib/mixer/multi_tables b/src/modules/systemlib/mixer/multi_tables index 18c8285786..8978345010 100755 --- a/src/modules/systemlib/mixer/multi_tables +++ b/src/modules/systemlib/mixer/multi_tables @@ -102,8 +102,10 @@ foreach table $tables { foreach {angle dir} $angles { if {$dir == "CW"} { set dd 1.0 - } else { + } else if {$dir == "CCW"} { set dd -1.0 + } else { + set dd $dir } factors $angle $dd } From 8612189c65e5af73699e482d42950570dd298fd3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 4 Jan 2015 18:42:36 +0100 Subject: [PATCH 378/652] Moved text feedback to new API --- src/modules/sdlog2/sdlog2.c | 57 ++++++++++++++++--------------------- 1 file changed, 25 insertions(+), 32 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 218145809a..22b178310a 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -432,7 +432,7 @@ int open_log_file() if (file_number > MAX_NO_LOGFILE) { /* we should not end up here, either we have more than MAX_NO_LOGFILE on the SD card, or another problem */ - mavlink_log_critical(mavlink_fd, "[sdlog2] ERR: max files %d", MAX_NO_LOGFILE); + mavlink_and_console_log_critical(mavlink_fd, "[sdlog2] ERR: max files %d", MAX_NO_LOGFILE); return -1; } } @@ -440,12 +440,10 @@ int open_log_file() int fd = open(log_file_path, O_CREAT | O_WRONLY | O_DSYNC); if (fd < 0) { - warn("failed opening log: %s", log_file_name); - mavlink_log_critical(mavlink_fd, "[sdlog2] failed opening log: %s", log_file_name); + mavlink_and_console_log_critical(mavlink_fd, "[sdlog2] failed opening log: %s", log_file_name); } else { - warnx("log file: %s", log_file_name); - mavlink_log_info(mavlink_fd, "[sdlog2] log file: %s", log_file_name); + mavlink_and_console_log_info(mavlink_fd, "[sdlog2] log file: %s", log_file_name); } return fd; @@ -483,7 +481,7 @@ int open_perf_file(const char* str) if (file_number > MAX_NO_LOGFILE) { /* we should not end up here, either we have more than MAX_NO_LOGFILE on the SD card, or another problem */ - mavlink_log_critical(mavlink_fd, "[sdlog2] ERR: max files %d", MAX_NO_LOGFILE); + mavlink_and_console_log_critical(mavlink_fd, "[sdlog2] ERR: max files %d", MAX_NO_LOGFILE); return -1; } } @@ -491,12 +489,10 @@ int open_perf_file(const char* str) int fd = open(log_file_path, O_CREAT | O_WRONLY | O_DSYNC); if (fd < 0) { - warn("failed opening log: %s", log_file_name); - mavlink_log_critical(mavlink_fd, "[sdlog2] failed opening log: %s", log_file_name); + mavlink_and_console_log_critical(mavlink_fd, "[sdlog2] failed opening log: %s", log_file_name); } else { - warnx("log file: %s", log_file_name); - mavlink_log_info(mavlink_fd, "[sdlog2] log file: %s", log_file_name); + mavlink_and_console_log_info(mavlink_fd, "[sdlog2] log file: %s", log_file_name); } return fd; @@ -619,13 +615,12 @@ static void *logwriter_thread(void *arg) void sdlog2_start_log() { - warnx("start logging"); - mavlink_log_info(mavlink_fd, "[sdlog2] start logging"); + mavlink_and_console_log_info(mavlink_fd, "[sdlog2] start logging"); /* create log dir if needed */ if (create_log_dir() != 0) { - mavlink_log_critical(mavlink_fd, "[sdlog2] error creating log dir"); - errx(1, "error creating log dir"); + mavlink_and_console_log_critical(mavlink_fd, "[sdlog2] error creating log dir"); + exit(1); } @@ -666,8 +661,7 @@ void sdlog2_start_log() void sdlog2_stop_log() { - warnx("stop logging"); - mavlink_log_info(mavlink_fd, "[sdlog2] stop logging"); + mavlink_and_console_log_info(mavlink_fd, "[sdlog2] stop logging"); logging_enabled = false; @@ -793,7 +787,7 @@ int sdlog2_thread_main(int argc, char *argv[]) mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); if (mavlink_fd < 0) { - warnx("failed to open MAVLink log stream, start mavlink app first"); + warnx("ERR: log stream, start mavlink app first"); } /* delay = 1 / rate (rate defined by -r option), default log rate: 50 Hz */ @@ -923,7 +917,7 @@ int sdlog2_thread_main(int argc, char *argv[]) if (check_free_space() != OK) { - errx(1, "error MicroSD almost full"); + errx(1, "ERR: MicroSD almost full"); } @@ -931,7 +925,7 @@ int sdlog2_thread_main(int argc, char *argv[]) int mkdir_ret = mkdir(log_root, S_IRWXU | S_IRWXG | S_IRWXO); if (mkdir_ret != 0 && errno != EEXIST) { - err(1, "failed creating log root dir: %s", log_root); + err(1, "ERR: failed creating log dir: %s", log_root); } /* copy conversion scripts */ @@ -1783,8 +1777,6 @@ int sdlog2_thread_main(int argc, char *argv[]) free(lb.data); - warnx("exiting"); - thread_running = false; return 0; @@ -1817,7 +1809,7 @@ int file_copy(const char *file_old, const char *file_new) int ret = 0; if (source == NULL) { - warnx("failed opening input file to copy"); + warnx("ERR: open in"); return 1; } @@ -1825,7 +1817,7 @@ int file_copy(const char *file_old, const char *file_new) if (target == NULL) { fclose(source); - warnx("failed to open output file to copy"); + warnx("ERR: open out"); return 1; } @@ -1855,21 +1847,22 @@ int check_free_space() /* use statfs to determine the number of blocks left */ FAR struct statfs statfs_buf; if (statfs(mountpoint, &statfs_buf) != OK) { - warnx("could not determine statfs"); - return ERROR; + errx(ERROR, "ERR: statfs"); } /* use a threshold of 10 MiB */ - if (statfs_buf.f_bavail < (int)(10*1024*1024/statfs_buf.f_bsize)) { - warnx("no more space on MicroSD (less than 10 MiB)"); - mavlink_log_critical(mavlink_fd, "[sdlog2] no more space left on MicroSD"); + if (statfs_buf.f_bavail < (int)(10 * 1024 * 1024 / statfs_buf.f_bsize)) { + mavlink_and_console_log_critical(mavlink_fd, + "[sdlog2] no space on MicroSD: %u MiB", + (unsigned int)(statfs_buf.f_bavail * statfs_buf.f_bsize) / (1024U * 1024U)); /* we do not need a flag to remember that we sent this warning because we will exit anyway */ return ERROR; /* use a threshold of 100 MiB to send a warning */ - } else if (!space_warning_sent && statfs_buf.f_bavail < (int)(100*1024*1024/statfs_buf.f_bsize)) { - warnx("space on MicroSD running out (less than 100MiB)"); - mavlink_log_critical(mavlink_fd, "[sdlog2] space on MicroSD running out"); + } else if (!space_warning_sent && statfs_buf.f_bavail < (int)(100 * 1024 * 1024 / statfs_buf.f_bsize)) { + mavlink_and_console_log_critical(mavlink_fd, + "[sdlog2] space on MicroSD low: %u MiB", + (unsigned int)(statfs_buf.f_bavail * statfs_buf.f_bsize) / (1024U * 1024U)); /* we don't want to flood the user with warnings */ space_warning_sent = true; } From c4471d77d7d6a20709240ab933c32d87ba7c07fd Mon Sep 17 00:00:00 2001 From: Anton Matosov Date: Sun, 4 Jan 2015 18:58:57 +0200 Subject: [PATCH 379/652] Moved quad_v and twin_engine to the multi_tables in order to make all the tables been generated automatically --- .../systemlib/mixer/mixer_multirotor.cpp | 126 +++++++++--------- src/modules/systemlib/mixer/multi_tables | 19 ++- 2 files changed, 76 insertions(+), 69 deletions(-) diff --git a/src/modules/systemlib/mixer/mixer_multirotor.cpp b/src/modules/systemlib/mixer/mixer_multirotor.cpp index 24187c9bc5..eb1aef6c1c 100644 --- a/src/modules/systemlib/mixer/mixer_multirotor.cpp +++ b/src/modules/systemlib/mixer/mixer_multirotor.cpp @@ -78,89 +78,87 @@ float constrain(float val, float min, float max) */ const MultirotorMixer::Rotor _config_quad_x[] = { - { -0.707107, 0.707107, 1.00 }, - { 0.707107, -0.707107, 1.00 }, - { 0.707107, 0.707107, -1.00 }, - { -0.707107, -0.707107, -1.00 }, + { -0.707107, 0.707107, 1.000000 }, + { 0.707107, -0.707107, 1.000000 }, + { 0.707107, 0.707107, -1.000000 }, + { -0.707107, -0.707107, -1.000000 }, }; const MultirotorMixer::Rotor _config_quad_plus[] = { - { -1.000000, 0.000000, 1.00 }, - { 1.000000, 0.000000, 1.00 }, - { 0.000000, 1.000000, -1.00 }, - { -0.000000, -1.000000, -1.00 }, + { -1.000000, 0.000000, 1.000000 }, + { 1.000000, 0.000000, 1.000000 }, + { 0.000000, 1.000000, -1.000000 }, + { -0.000000, -1.000000, -1.000000 }, }; -//Add table for quad in V configuration, which is not generated by multi_tables! const MultirotorMixer::Rotor _config_quad_v[] = { - { -0.3223, 0.9466, 0.4242 }, - { 0.3223, -0.9466, 1.0000 }, - { 0.3223, 0.9466, -0.4242 }, - { -0.3223, -0.9466, -1.0000 }, + { -0.322266, 0.946649, 0.424200 }, + { 0.322266, 0.946649, 1.000000 }, + { 0.322266, 0.946649, -0.424200 }, + { -0.322266, 0.946649, -1.000000 }, }; const MultirotorMixer::Rotor _config_quad_wide[] = { - { -0.927184, 0.374607, 1.00 }, - { 0.777146, -0.629320, 1.00 }, - { 0.927184, 0.374607, -1.00 }, - { -0.777146, -0.629320, -1.00 }, + { -0.927184, 0.374607, 1.000000 }, + { 0.777146, -0.629320, 1.000000 }, + { 0.927184, 0.374607, -1.000000 }, + { -0.777146, -0.629320, -1.000000 }, }; const MultirotorMixer::Rotor _config_hex_x[] = { - { -1.000000, 0.000000, -1.00 }, - { 1.000000, 0.000000, 1.00 }, - { 0.500000, 0.866025, -1.00 }, - { -0.500000, -0.866025, 1.00 }, - { -0.500000, 0.866025, 1.00 }, - { 0.500000, -0.866025, -1.00 }, + { -1.000000, 0.000000, -1.000000 }, + { 1.000000, 0.000000, 1.000000 }, + { 0.500000, 0.866025, -1.000000 }, + { -0.500000, -0.866025, 1.000000 }, + { -0.500000, 0.866025, 1.000000 }, + { 0.500000, -0.866025, -1.000000 }, }; const MultirotorMixer::Rotor _config_hex_plus[] = { - { 0.000000, 1.000000, -1.00 }, - { -0.000000, -1.000000, 1.00 }, - { 0.866025, -0.500000, -1.00 }, - { -0.866025, 0.500000, 1.00 }, - { 0.866025, 0.500000, 1.00 }, - { -0.866025, -0.500000, -1.00 }, + { 0.000000, 1.000000, -1.000000 }, + { -0.000000, -1.000000, 1.000000 }, + { 0.866025, -0.500000, -1.000000 }, + { -0.866025, 0.500000, 1.000000 }, + { 0.866025, 0.500000, 1.000000 }, + { -0.866025, -0.500000, -1.000000 }, }; const MultirotorMixer::Rotor _config_hex_cox[] = { - { -0.866025, 0.500000, -1.00 }, - { -0.866025, 0.500000, 1.00 }, - { -0.000000, -1.000000, -1.00 }, - { -0.000000, -1.000000, 1.00 }, - { 0.866025, 0.500000, -1.00 }, - { 0.866025, 0.500000, 1.00 }, + { -0.866025, 0.500000, -1.000000 }, + { -0.866025, 0.500000, 1.000000 }, + { -0.000000, -1.000000, -1.000000 }, + { -0.000000, -1.000000, 1.000000 }, + { 0.866025, 0.500000, -1.000000 }, + { 0.866025, 0.500000, 1.000000 }, }; const MultirotorMixer::Rotor _config_octa_x[] = { - { -0.382683, 0.923880, -1.00 }, - { 0.382683, -0.923880, -1.00 }, - { -0.923880, 0.382683, 1.00 }, - { -0.382683, -0.923880, 1.00 }, - { 0.382683, 0.923880, 1.00 }, - { 0.923880, -0.382683, 1.00 }, - { 0.923880, 0.382683, -1.00 }, - { -0.923880, -0.382683, -1.00 }, + { -0.382683, 0.923880, -1.000000 }, + { 0.382683, -0.923880, -1.000000 }, + { -0.923880, 0.382683, 1.000000 }, + { -0.382683, -0.923880, 1.000000 }, + { 0.382683, 0.923880, 1.000000 }, + { 0.923880, -0.382683, 1.000000 }, + { 0.923880, 0.382683, -1.000000 }, + { -0.923880, -0.382683, -1.000000 }, }; const MultirotorMixer::Rotor _config_octa_plus[] = { - { 0.000000, 1.000000, -1.00 }, - { -0.000000, -1.000000, -1.00 }, - { -0.707107, 0.707107, 1.00 }, - { -0.707107, -0.707107, 1.00 }, - { 0.707107, 0.707107, 1.00 }, - { 0.707107, -0.707107, 1.00 }, - { 1.000000, 0.000000, -1.00 }, - { -1.000000, 0.000000, -1.00 }, + { 0.000000, 1.000000, -1.000000 }, + { -0.000000, -1.000000, -1.000000 }, + { -0.707107, 0.707107, 1.000000 }, + { -0.707107, -0.707107, 1.000000 }, + { 0.707107, 0.707107, 1.000000 }, + { 0.707107, -0.707107, 1.000000 }, + { 1.000000, 0.000000, -1.000000 }, + { -1.000000, 0.000000, -1.000000 }, }; const MultirotorMixer::Rotor _config_octa_cox[] = { - { -0.707107, 0.707107, 1.00 }, - { 0.707107, 0.707107, -1.00 }, - { 0.707107, -0.707107, 1.00 }, - { -0.707107, -0.707107, -1.00 }, - { 0.707107, 0.707107, 1.00 }, - { -0.707107, 0.707107, -1.00 }, - { -0.707107, -0.707107, 1.00 }, - { 0.707107, -0.707107, -1.00 }, + { -0.707107, 0.707107, 1.000000 }, + { 0.707107, 0.707107, -1.000000 }, + { 0.707107, -0.707107, 1.000000 }, + { -0.707107, -0.707107, -1.000000 }, + { 0.707107, 0.707107, 1.000000 }, + { -0.707107, 0.707107, -1.000000 }, + { -0.707107, -0.707107, 1.000000 }, + { 0.707107, -0.707107, -1.000000 }, }; -const MultirotorMixer::Rotor _config_duorotor[] = { - { -1.000000, 0.000000, 0.00 }, - { 1.000000, 0.000000, 0.00 }, +const MultirotorMixer::Rotor _config_twin_engine[] = { + { -1.000000, 0.000000, 0.000000 }, + { 1.000000, 0.000000, 0.000000 }, }; - const MultirotorMixer::Rotor *_config_index[MultirotorMixer::MAX_GEOMETRY] = { &_config_quad_x[0], &_config_quad_plus[0], @@ -172,7 +170,7 @@ const MultirotorMixer::Rotor *_config_index[MultirotorMixer::MAX_GEOMETRY] = { &_config_octa_x[0], &_config_octa_plus[0], &_config_octa_cox[0], - &_config_duorotor[0], + &_config_twin_engine[0], }; const unsigned _config_rotor_count[MultirotorMixer::MAX_GEOMETRY] = { 4, /* quad_x */ diff --git a/src/modules/systemlib/mixer/multi_tables b/src/modules/systemlib/mixer/multi_tables index 8978345010..bdb62f812d 100755 --- a/src/modules/systemlib/mixer/multi_tables +++ b/src/modules/systemlib/mixer/multi_tables @@ -21,6 +21,12 @@ set quad_plus { 180 CW } +set quad_v { + 18.8 0.4242 + -18.8 1.0 + -18.8 -0.4242 + 18.8 -1.0 +} set quad_wide { 68 CCW @@ -89,11 +95,14 @@ set octa_cox { -135 CW } +set twin_engine { + 90 0.0 + -90 0.0 +} -set tables {quad_x quad_plus quad_wide hex_x hex_plus hex_cox octa_x octa_plus octa_cox} +set tables {quad_x quad_plus quad_v quad_wide hex_x hex_plus hex_cox octa_x octa_plus octa_cox twin_engine} - -proc factors {a d} { puts [format "\t{ %9.6f, %9.6f, %5.2f }," [rcos [expr $a + 90]] [rcos $a] [expr -$d]]} +proc factors {a d} { puts [format "\t{ %9.6f, %9.6f, %9.6f }," [rcos [expr $a + 90]] [rcos $a] [expr $d]]} foreach table $tables { puts [format "const MultirotorMixer::Rotor _config_%s\[\] = {" $table] @@ -101,9 +110,9 @@ foreach table $tables { upvar #0 $table angles foreach {angle dir} $angles { if {$dir == "CW"} { - set dd 1.0 - } else if {$dir == "CCW"} { set dd -1.0 + } elseif {$dir == "CCW"} { + set dd 1.0 } else { set dd $dir } From 552ff809693d340ba6f5fed6837b99effe8bf2c3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 4 Jan 2015 18:53:34 +0100 Subject: [PATCH 380/652] Fix NSH timeout logic --- src/systemcmds/nshterm/nshterm.c | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/src/systemcmds/nshterm/nshterm.c b/src/systemcmds/nshterm/nshterm.c index edeb5c6248..c2ea2a1cc0 100644 --- a/src/systemcmds/nshterm/nshterm.c +++ b/src/systemcmds/nshterm/nshterm.c @@ -66,20 +66,18 @@ nshterm_main(int argc, char *argv[]) int fd = -1; int armed_fd = orb_subscribe(ORB_ID(actuator_armed)); struct actuator_armed_s armed; - /* we assume the system does not provide arming status feedback */ - bool armed_updated = false; - /* try the first 30 seconds or if arming system is ready */ - while ((retries < 300) || armed_updated) { + /* try to bring up the console - stop doing so if the system gets armed */ + while (true) { /* abort if an arming topic is published and system is armed */ bool updated = false; - if (orb_check(armed_fd, &updated)) { + orb_check(armed_fd, &updated) + if (updated) { /* the system is now providing arming status feedback. * instead of timing out, we resort to abort bringing * up the terminal. */ - armed_updated = true; orb_copy(ORB_ID(actuator_armed), armed_fd, &armed); if (armed.armed) { From c451fdd1986fd36bd075e9a67387a13be8f505a0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 4 Jan 2015 19:04:15 +0100 Subject: [PATCH 381/652] sdlog2: Output cleanup --- src/modules/sdlog2/sdlog2.c | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 9631497798..9dd80a18e1 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -502,10 +502,8 @@ int open_perf_file(const char* str) int fd = open(log_file_path, O_CREAT | O_WRONLY | O_DSYNC); if (fd < 0) { - mavlink_and_console_log_critical(mavlink_fd, "[sdlog2] failed opening log: %s", log_file_name); + mavlink_and_console_log_critical(mavlink_fd, "[sdlog2] failed opening: %s", log_file_name); - } else { - mavlink_and_console_log_info(mavlink_fd, "[sdlog2] log file: %s", log_file_name); } return fd; From 1b7bdcb07a27c1710cf6e47168db3152a0d26143 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 4 Jan 2015 19:04:42 +0100 Subject: [PATCH 382/652] NSH term fix --- src/systemcmds/nshterm/nshterm.c | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/systemcmds/nshterm/nshterm.c b/src/systemcmds/nshterm/nshterm.c index c2ea2a1cc0..ceaea35b63 100644 --- a/src/systemcmds/nshterm/nshterm.c +++ b/src/systemcmds/nshterm/nshterm.c @@ -72,7 +72,7 @@ nshterm_main(int argc, char *argv[]) /* abort if an arming topic is published and system is armed */ bool updated = false; - orb_check(armed_fd, &updated) + orb_check(armed_fd, &updated); if (updated) { /* the system is now providing arming status feedback. * instead of timing out, we resort to abort bringing @@ -90,6 +90,7 @@ nshterm_main(int argc, char *argv[]) /* which may not be ready immediately. */ fd = open(argv[1], O_RDWR); if (fd != -1) { + close(armed_fd); break; } usleep(100000); @@ -114,7 +115,7 @@ nshterm_main(int argc, char *argv[]) } /* Set ONLCR flag (which appends a CR for every LF) */ - uart_config.c_oflag |= (ONLCR | OPOST/* | OCRNL*/); + uart_config.c_oflag |= (ONLCR | OPOST); if ((termios_state = tcsetattr(fd, TCSANOW, &uart_config)) < 0) { warnx("ERR set config %s\n", argv[1]); From d70ed8ad123b44667a636056fe5783944157d735 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 4 Jan 2015 19:04:42 +0100 Subject: [PATCH 383/652] NSH term fix --- src/systemcmds/nshterm/nshterm.c | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/systemcmds/nshterm/nshterm.c b/src/systemcmds/nshterm/nshterm.c index c2ea2a1cc0..ceaea35b63 100644 --- a/src/systemcmds/nshterm/nshterm.c +++ b/src/systemcmds/nshterm/nshterm.c @@ -72,7 +72,7 @@ nshterm_main(int argc, char *argv[]) /* abort if an arming topic is published and system is armed */ bool updated = false; - orb_check(armed_fd, &updated) + orb_check(armed_fd, &updated); if (updated) { /* the system is now providing arming status feedback. * instead of timing out, we resort to abort bringing @@ -90,6 +90,7 @@ nshterm_main(int argc, char *argv[]) /* which may not be ready immediately. */ fd = open(argv[1], O_RDWR); if (fd != -1) { + close(armed_fd); break; } usleep(100000); @@ -114,7 +115,7 @@ nshterm_main(int argc, char *argv[]) } /* Set ONLCR flag (which appends a CR for every LF) */ - uart_config.c_oflag |= (ONLCR | OPOST/* | OCRNL*/); + uart_config.c_oflag |= (ONLCR | OPOST); if ((termios_state = tcsetattr(fd, TCSANOW, &uart_config)) < 0) { warnx("ERR set config %s\n", argv[1]); From e8eff3061f5e9c451c94d081932cac0e62e1a9b9 Mon Sep 17 00:00:00 2001 From: Ban Siesta Date: Sun, 4 Jan 2015 18:59:53 +0000 Subject: [PATCH 384/652] Revert "sdlog2: slow down the free space check a bit more" This reverts commit 04c273bca6c99f31fd04741234d9c8efa849b553. --- src/modules/sdlog2/sdlog2.c | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 9dd80a18e1..1bf7d71986 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -600,19 +600,16 @@ static void *logwriter_thread(void *arg) should_wait = true; } - /* slow down fsync */ - if (poll_count % 10 == 0) { + if (++poll_count == 10) { fsync(log_fd); + poll_count = 0; - /* slow down the check even more, and don't do both at the same time */ - } else if (poll_count % 1000 == 5) { /* check if space is available, if not stop everything */ if (check_free_space() != OK) { logwriter_should_exit = true; main_thread_should_exit = true; } } - poll_count++; } fsync(log_fd); From 0265885a39282cf9aa34cf1647cf9b260f9f41cc Mon Sep 17 00:00:00 2001 From: Ban Siesta Date: Sun, 4 Jan 2015 19:13:27 +0000 Subject: [PATCH 385/652] sdlog2: check every 20MiB if we're running out of space, moved the threshold to 50MiB --- src/modules/sdlog2/sdlog2.c | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 1bf7d71986..31cdff3069 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -178,6 +178,7 @@ static char log_dir[32]; /* statistics counters */ static uint64_t start_time = 0; static unsigned long log_bytes_written = 0; +static unsigned long last_checked_bytes_written = 0; static unsigned long log_msgs_written = 0; static unsigned long log_msgs_skipped = 0; @@ -604,11 +605,15 @@ static void *logwriter_thread(void *arg) fsync(log_fd); poll_count = 0; + } + + if (log_bytes_written - last_checked_bytes_written > 20*1024*1024) { /* check if space is available, if not stop everything */ if (check_free_space() != OK) { logwriter_should_exit = true; main_thread_should_exit = true; } + last_checked_bytes_written = log_bytes_written; } } @@ -1858,8 +1863,8 @@ int check_free_space() errx(ERROR, "ERR: statfs"); } - /* use a threshold of 10 MiB */ - if (statfs_buf.f_bavail < (int)(10 * 1024 * 1024 / statfs_buf.f_bsize)) { + /* use a threshold of 50 MiB */ + if (statfs_buf.f_bavail < (int)(50 * 1024 * 1024 / statfs_buf.f_bsize)) { mavlink_and_console_log_critical(mavlink_fd, "[sdlog2] no space on MicroSD: %u MiB", (unsigned int)(statfs_buf.f_bavail * statfs_buf.f_bsize) / (1024U * 1024U)); From 2e1ac72b598184ef618beceb10de53606929edf7 Mon Sep 17 00:00:00 2001 From: Kevin Hester Date: Sat, 10 Aug 2013 12:57:52 -1000 Subject: [PATCH 386/652] Speed up stack dumping by looking for freespace 4 bytes at a time Conflicts: mavlink/include/mavlink/v1.0 --- Debug/NuttX | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/Debug/NuttX b/Debug/NuttX index 4b9f4b5a1b..20b5793105 100644 --- a/Debug/NuttX +++ b/Debug/NuttX @@ -178,9 +178,10 @@ define showtaskstack printf "can't measure idle stack\n" else set $stack_free = 0 - while ($stack_free < $task->adj_stack_size) && *(uint8_t *)($task->stack_alloc_ptr + $stack_free) + while ($stack_free < $task->adj_stack_size) && ((uint32_t *)($task->stack_alloc_ptr))[$stack_free] == 0xffffffff set $stack_free = $stack_free + 1 end + set $stack_free = $stack_free * 4 printf" stack 0x%08x-0x%08x (%d) %d free\n", $task->stack_alloc_ptr, $task->adj_stack_ptr, $task->adj_stack_size, $stack_free end end @@ -278,4 +279,4 @@ define my_mem set $cursor = $cursor - 4 end end -end \ No newline at end of file +end From 0e1a532720a80660b2d4a3b24c1a071a517ec82e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 4 Jan 2015 20:57:25 +0100 Subject: [PATCH 387/652] sdlog2: Use .px4log as file format. Fixes #1243 --- src/modules/sdlog2/sdlog2.c | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index c3952b77db..a88b6045ab 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -343,7 +343,7 @@ int create_log_dir() struct timespec ts; clock_gettime(CLOCK_REALTIME, &ts); - /* use RTC time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.bin */ + /* use RTC time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.px4log */ time_t utc_time_sec = ts.tv_sec + (ts.tv_nsec / 1e9); struct tm tt; struct tm *ttp = gmtime_r(&utc_time_sec, &tt); @@ -403,14 +403,14 @@ int open_log_file() struct timespec ts; clock_gettime(CLOCK_REALTIME, &ts); - /* use RTC time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.bin */ + /* use RTC time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.px4log */ time_t utc_time_sec = ts.tv_sec + (ts.tv_nsec / 1e9); struct tm tt; struct tm *ttp = gmtime_r(&utc_time_sec, &tt); /* start logging if we have a valid time and the time is not in the past */ if (log_name_timestamp && ttp && (utc_time_sec > PX4_EPOCH_SECS)) { - strftime(log_file_name, sizeof(log_file_name), "%H_%M_%S.bin", &tt); + strftime(log_file_name, sizeof(log_file_name), "%H_%M_%S.px4log", &tt); snprintf(log_file_path, sizeof(log_file_path), "%s/%s", log_dir, log_file_name); } else { @@ -418,8 +418,8 @@ int open_log_file() /* look for the next file that does not exist */ while (file_number <= MAX_NO_LOGFILE) { - /* format log file path: e.g. /fs/microsd/sess001/log001.bin */ - snprintf(log_file_name, sizeof(log_file_name), "log%03u.bin", file_number); + /* format log file path: e.g. /fs/microsd/sess001/log001.px4log */ + snprintf(log_file_name, sizeof(log_file_name), "log%03u.px4log", file_number); snprintf(log_file_path, sizeof(log_file_path), "%s/%s", log_dir, log_file_name); if (!file_exist(log_file_path)) { @@ -458,7 +458,7 @@ int open_perf_file(const char* str) struct timespec ts; clock_gettime(CLOCK_REALTIME, &ts); - /* use RTC time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.bin */ + /* use RTC time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.txt */ time_t utc_time_sec = ts.tv_sec + (ts.tv_nsec / 1e9); struct tm tt; struct tm *ttp = gmtime_r(&utc_time_sec, &tt); @@ -472,7 +472,7 @@ int open_perf_file(const char* str) /* look for the next file that does not exist */ while (file_number <= MAX_NO_LOGFILE) { - /* format log file path: e.g. /fs/microsd/sess001/log001.bin */ + /* format log file path: e.g. /fs/microsd/sess001/log001.txt */ snprintf(log_file_name, sizeof(log_file_name), "perf%03u.txt", file_number); snprintf(log_file_path, sizeof(log_file_path), "%s/%s_%s", log_dir, str, log_file_name); From c3ed35f5cc8e2617e61747e904dbaa652d1cc2c8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 5 Jan 2015 06:42:43 +0100 Subject: [PATCH 388/652] Travis CI: Comment out mail command --- .travis.yml | 26 +++++++++++++------------- 1 file changed, 13 insertions(+), 13 deletions(-) diff --git a/.travis.yml b/.travis.yml index 02882ac96e..b201d5130c 100644 --- a/.travis.yml +++ b/.travis.yml @@ -49,19 +49,19 @@ script: - zip Firmware.zip Images/*.px4 # We use an encrypted env variable to ensure this only executes when artifacts are uploaded. -after_script: - - 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 - - /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/ +#after_script: +# - 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 +# - /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: provider: releases From 59fa170e54a4a88b4bdad86622e75183e9fbdc51 Mon Sep 17 00:00:00 2001 From: Anton Matosov Date: Mon, 10 Nov 2014 00:09:27 +0200 Subject: [PATCH 389/652] Automated updates of the C++ code generated by the multi_tables script, now it is placed into the mixer_multirotor.generated.h file which is generated by makefile --- .gitignore | 1 + .../systemlib/mixer/mixer_multirotor.cpp | 114 +----------------- src/modules/systemlib/mixer/module.mk | 7 +- src/modules/systemlib/mixer/multi_tables | 9 ++ 4 files changed, 18 insertions(+), 113 deletions(-) diff --git a/.gitignore b/.gitignore index 9959229f5f..8ea2698e72 100644 --- a/.gitignore +++ b/.gitignore @@ -40,3 +40,4 @@ tags *.orig Firmware.zip unittests/build +*.generated.h diff --git a/src/modules/systemlib/mixer/mixer_multirotor.cpp b/src/modules/systemlib/mixer/mixer_multirotor.cpp index eb1aef6c1c..bb3868c34b 100644 --- a/src/modules/systemlib/mixer/mixer_multirotor.cpp +++ b/src/modules/systemlib/mixer/mixer_multirotor.cpp @@ -73,118 +73,8 @@ float constrain(float val, float min, float max) return (val < min) ? min : ((val > max) ? max : val); } -/* - * These tables automatically generated by multi_tables - do not edit. - */ - -const MultirotorMixer::Rotor _config_quad_x[] = { - { -0.707107, 0.707107, 1.000000 }, - { 0.707107, -0.707107, 1.000000 }, - { 0.707107, 0.707107, -1.000000 }, - { -0.707107, -0.707107, -1.000000 }, -}; -const MultirotorMixer::Rotor _config_quad_plus[] = { - { -1.000000, 0.000000, 1.000000 }, - { 1.000000, 0.000000, 1.000000 }, - { 0.000000, 1.000000, -1.000000 }, - { -0.000000, -1.000000, -1.000000 }, -}; -const MultirotorMixer::Rotor _config_quad_v[] = { - { -0.322266, 0.946649, 0.424200 }, - { 0.322266, 0.946649, 1.000000 }, - { 0.322266, 0.946649, -0.424200 }, - { -0.322266, 0.946649, -1.000000 }, -}; -const MultirotorMixer::Rotor _config_quad_wide[] = { - { -0.927184, 0.374607, 1.000000 }, - { 0.777146, -0.629320, 1.000000 }, - { 0.927184, 0.374607, -1.000000 }, - { -0.777146, -0.629320, -1.000000 }, -}; -const MultirotorMixer::Rotor _config_hex_x[] = { - { -1.000000, 0.000000, -1.000000 }, - { 1.000000, 0.000000, 1.000000 }, - { 0.500000, 0.866025, -1.000000 }, - { -0.500000, -0.866025, 1.000000 }, - { -0.500000, 0.866025, 1.000000 }, - { 0.500000, -0.866025, -1.000000 }, -}; -const MultirotorMixer::Rotor _config_hex_plus[] = { - { 0.000000, 1.000000, -1.000000 }, - { -0.000000, -1.000000, 1.000000 }, - { 0.866025, -0.500000, -1.000000 }, - { -0.866025, 0.500000, 1.000000 }, - { 0.866025, 0.500000, 1.000000 }, - { -0.866025, -0.500000, -1.000000 }, -}; -const MultirotorMixer::Rotor _config_hex_cox[] = { - { -0.866025, 0.500000, -1.000000 }, - { -0.866025, 0.500000, 1.000000 }, - { -0.000000, -1.000000, -1.000000 }, - { -0.000000, -1.000000, 1.000000 }, - { 0.866025, 0.500000, -1.000000 }, - { 0.866025, 0.500000, 1.000000 }, -}; -const MultirotorMixer::Rotor _config_octa_x[] = { - { -0.382683, 0.923880, -1.000000 }, - { 0.382683, -0.923880, -1.000000 }, - { -0.923880, 0.382683, 1.000000 }, - { -0.382683, -0.923880, 1.000000 }, - { 0.382683, 0.923880, 1.000000 }, - { 0.923880, -0.382683, 1.000000 }, - { 0.923880, 0.382683, -1.000000 }, - { -0.923880, -0.382683, -1.000000 }, -}; -const MultirotorMixer::Rotor _config_octa_plus[] = { - { 0.000000, 1.000000, -1.000000 }, - { -0.000000, -1.000000, -1.000000 }, - { -0.707107, 0.707107, 1.000000 }, - { -0.707107, -0.707107, 1.000000 }, - { 0.707107, 0.707107, 1.000000 }, - { 0.707107, -0.707107, 1.000000 }, - { 1.000000, 0.000000, -1.000000 }, - { -1.000000, 0.000000, -1.000000 }, -}; -const MultirotorMixer::Rotor _config_octa_cox[] = { - { -0.707107, 0.707107, 1.000000 }, - { 0.707107, 0.707107, -1.000000 }, - { 0.707107, -0.707107, 1.000000 }, - { -0.707107, -0.707107, -1.000000 }, - { 0.707107, 0.707107, 1.000000 }, - { -0.707107, 0.707107, -1.000000 }, - { -0.707107, -0.707107, 1.000000 }, - { 0.707107, -0.707107, -1.000000 }, -}; -const MultirotorMixer::Rotor _config_twin_engine[] = { - { -1.000000, 0.000000, 0.000000 }, - { 1.000000, 0.000000, 0.000000 }, -}; -const MultirotorMixer::Rotor *_config_index[MultirotorMixer::MAX_GEOMETRY] = { - &_config_quad_x[0], - &_config_quad_plus[0], - &_config_quad_v[0], - &_config_quad_wide[0], - &_config_hex_x[0], - &_config_hex_plus[0], - &_config_hex_cox[0], - &_config_octa_x[0], - &_config_octa_plus[0], - &_config_octa_cox[0], - &_config_twin_engine[0], -}; -const unsigned _config_rotor_count[MultirotorMixer::MAX_GEOMETRY] = { - 4, /* quad_x */ - 4, /* quad_plus */ - 4, /* quad_v */ - 4, /* quad_wide */ - 6, /* hex_x */ - 6, /* hex_plus */ - 6, /* hex_cox */ - 8, /* octa_x */ - 8, /* octa_plus */ - 8, /* octa_cox */ - 2, /* twin_engine */ -}; +// This file is generated by the multi_tables script which is invoked during the build process +#include "mixer_multirotor.generated.h" } diff --git a/src/modules/systemlib/mixer/module.mk b/src/modules/systemlib/mixer/module.mk index fc7485e202..6836313247 100644 --- a/src/modules/systemlib/mixer/module.mk +++ b/src/modules/systemlib/mixer/module.mk @@ -31,13 +31,18 @@ # ############################################################################ + # # mixer library # LIBNAME = mixerlib - + SRCS = mixer.cpp \ mixer_group.cpp \ mixer_multirotor.cpp \ mixer_simple.cpp \ mixer_load.c + +SELF_DIR := $(dir $(lastword $(MAKEFILE_LIST))) +$(SELF_DIR)mixer_multirotor.generated.h : $(SELF_DIR)multi_tables + $(SELF_DIR)multi_tables > $(SELF_DIR)mixer_multirotor.generated.h diff --git a/src/modules/systemlib/mixer/multi_tables b/src/modules/systemlib/mixer/multi_tables index bdb62f812d..1f47ca63d5 100755 --- a/src/modules/systemlib/mixer/multi_tables +++ b/src/modules/systemlib/mixer/multi_tables @@ -3,6 +3,12 @@ # Generate multirotor mixer scale tables compatible with the ArduCopter layout # + +puts "/*" +puts "* These tables automatically generated by multi_tables - do not edit." +puts "*/" +puts "" + proc rad {a} { expr ($a / 360.0) * 2 * acos(-1) } proc rcos {a} { expr cos([rad $a])} @@ -133,3 +139,6 @@ foreach table $tables { puts [format "\t%u, /* %s */" [expr [llength $angles] / 2] $table] } puts "};" + +# Newline at the end of file +puts "" From 388833a1fa56b40034333ba6bbef376fabcc1320 Mon Sep 17 00:00:00 2001 From: Anton Matosov Date: Sun, 4 Jan 2015 23:58:17 +0200 Subject: [PATCH 390/652] Added explicit dependency into makefile, as implicit one doesn't work often. --- src/modules/systemlib/mixer/module.mk | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/modules/systemlib/mixer/module.mk b/src/modules/systemlib/mixer/module.mk index 6836313247..6f4befb576 100644 --- a/src/modules/systemlib/mixer/module.mk +++ b/src/modules/systemlib/mixer/module.mk @@ -44,5 +44,9 @@ SRCS = mixer.cpp \ mixer_load.c SELF_DIR := $(dir $(lastword $(MAKEFILE_LIST))) + +# Add explicit dependency, as implicit one doesn't work often. +mixer_multirotor.cpp : $(SELF_DIR)mixer_multirotor.generated.h + $(SELF_DIR)mixer_multirotor.generated.h : $(SELF_DIR)multi_tables $(SELF_DIR)multi_tables > $(SELF_DIR)mixer_multirotor.generated.h From 8fadbdcf2fee463498228abeac038260f51460b1 Mon Sep 17 00:00:00 2001 From: Anton Matosov Date: Mon, 5 Jan 2015 00:30:49 +0200 Subject: [PATCH 391/652] Automated generation of the Geometry enum to make addition of the new multirotor a really simple task --- src/modules/systemlib/mixer/mixer.h | 18 +- .../systemlib/mixer/mixer_multirotor.cpp | 7 +- src/modules/systemlib/mixer/multi_tables | 164 ++++++++++-------- 3 files changed, 93 insertions(+), 96 deletions(-) diff --git a/src/modules/systemlib/mixer/mixer.h b/src/modules/systemlib/mixer/mixer.h index 1fe4380ade..a0197df480 100644 --- a/src/modules/systemlib/mixer/mixer.h +++ b/src/modules/systemlib/mixer/mixer.h @@ -453,23 +453,9 @@ public: /** * Supported multirotor geometries. * - * XXX add more + * Values are generated by the multi_tables script and placed to mixer_multirotor.generated.h */ - enum Geometry { - QUAD_X = 0, /**< quad in X configuration */ - QUAD_PLUS, /**< quad in + configuration */ - QUAD_V, /**< quad in V configuration */ - QUAD_WIDE, /**< quad in wide configuration */ - HEX_X, /**< hex in X configuration */ - HEX_PLUS, /**< hex in + configuration */ - HEX_COX, - OCTA_X, - OCTA_PLUS, - OCTA_COX, - TWIN_ENGINE, /**< VTOL: one engine on each wing */ - - MAX_GEOMETRY - }; + enum Geometry : unsigned int; /** * Precalculated rotor mix. diff --git a/src/modules/systemlib/mixer/mixer_multirotor.cpp b/src/modules/systemlib/mixer/mixer_multirotor.cpp index bb3868c34b..42e3513f4d 100644 --- a/src/modules/systemlib/mixer/mixer_multirotor.cpp +++ b/src/modules/systemlib/mixer/mixer_multirotor.cpp @@ -55,6 +55,9 @@ #include "mixer.h" +// This file is generated by the multi_tables script which is invoked during the build process +#include "mixer_multirotor.generated.h" + #define debug(fmt, args...) do { } while(0) //#define debug(fmt, args...) do { printf("[mixer] " fmt "\n", ##args); } while(0) //#include @@ -72,10 +75,6 @@ float constrain(float val, float min, float max) { return (val < min) ? min : ((val > max) ? max : val); } - -// This file is generated by the multi_tables script which is invoked during the build process -#include "mixer_multirotor.generated.h" - } MultirotorMixer::MultirotorMixer(ControlCallback control_cb, diff --git a/src/modules/systemlib/mixer/multi_tables b/src/modules/systemlib/mixer/multi_tables index 1f47ca63d5..b29bf540ae 100755 --- a/src/modules/systemlib/mixer/multi_tables +++ b/src/modules/systemlib/mixer/multi_tables @@ -3,9 +3,8 @@ # Generate multirotor mixer scale tables compatible with the ArduCopter layout # - puts "/*" -puts "* These tables automatically generated by multi_tables - do not edit." +puts "* These file is automatically generated by multi_tables - do not edit." puts "*/" puts "" @@ -14,17 +13,17 @@ proc rcos {a} { expr cos([rad $a])} set quad_x { - 45 CCW - -135 CCW - -45 CW - 135 CW + 45 CCW + -135 CCW + -45 CW + 135 CW } set quad_plus { - 90 CCW - -90 CCW - 0 CW - 180 CW + 90 CCW + -90 CCW + 0 CW + 180 CW } set quad_v { @@ -35,70 +34,70 @@ set quad_v { } set quad_wide { - 68 CCW - -129 CCW - -68 CW - 129 CW + 68 CCW + -129 CCW + -68 CW + 129 CW } set hex_x { - 90 CW - -90 CCW - -30 CW - 150 CCW - 30 CCW - -150 CW + 90 CW + -90 CCW + -30 CW + 150 CCW + 30 CCW + -150 CW } set hex_plus { - 0 CW - 180 CCW - -120 CW - 60 CCW - -60 CCW - 120 CW + 0 CW + 180 CCW + -120 CW + 60 CCW + -60 CCW + 120 CW } set hex_cox { - 60 CW - 60 CCW - 180 CW - 180 CCW - -60 CW - -60 CCW + 60 CW + 60 CCW + 180 CW + 180 CCW + -60 CW + -60 CCW } set octa_x { - 22.5 CW - -157.5 CW - 67.5 CCW - 157.5 CCW - -22.5 CCW - -112.5 CCW - -67.5 CW - 112.5 CW + 22.5 CW + -157.5 CW + 67.5 CCW + 157.5 CCW + -22.5 CCW + -112.5 CCW + -67.5 CW + 112.5 CW } set octa_plus { - 0 CW - 180 CW - 45 CCW - 135 CCW - -45 CCW - -135 CCW - -90 CW - 90 CW + 0 CW + 180 CW + 45 CCW + 135 CCW + -45 CCW + -135 CCW + -90 CW + 90 CW } set octa_cox { - 45 CCW - -45 CW - -135 CCW - 135 CW - -45 CCW - 45 CW - 135 CCW - -135 CW + 45 CCW + -45 CW + -135 CCW + 135 CW + -45 CCW + 45 CW + 135 CCW + -135 CW } set twin_engine { @@ -106,39 +105,52 @@ set twin_engine { -90 0.0 } + set tables {quad_x quad_plus quad_v quad_wide hex_x hex_plus hex_cox octa_x octa_plus octa_cox twin_engine} +puts "enum MultirotorMixer::Geometry : unsigned int {" +foreach table $tables { + puts [format "\t%s," [string toupper $table]] +} +puts "\n\tMAX_GEOMETRY" +puts "}; // enum MultirotorMixer::Geometry\n" + + +puts "namespace {" + proc factors {a d} { puts [format "\t{ %9.6f, %9.6f, %9.6f }," [rcos [expr $a + 90]] [rcos $a] [expr $d]]} foreach table $tables { - puts [format "const MultirotorMixer::Rotor _config_%s\[\] = {" $table] + puts [format "const MultirotorMixer::Rotor _config_%s\[\] = {" $table] - upvar #0 $table angles - foreach {angle dir} $angles { - if {$dir == "CW"} { - set dd -1.0 - } elseif {$dir == "CCW"} { - set dd 1.0 - } else { - set dd $dir - } - factors $angle $dd - } - puts "};" + upvar #0 $table angles + foreach {angle dir} $angles { + if {$dir == "CW"} { + set dd -1.0 + } elseif {$dir == "CCW"} { + set dd 1.0 + } else { + set dd $dir + } + factors $angle $dd + } + puts "};\n" } -puts "const MultirotorMixer::Rotor *_config_index\[MultirotorMixer::MAX_GEOMETRY\] = {" +puts "const MultirotorMixer::Rotor *_config_index\[\] = {" foreach table $tables { - puts [format "\t&_config_%s\[0\]," $table] + puts [format "\t&_config_%s\[0\]," $table] } -puts "};" +puts "};\n" -puts "const unsigned _config_rotor_count\[MultirotorMixer::MAX_GEOMETRY\] = {" +puts "const unsigned _config_rotor_count\[\] = {" foreach table $tables { - upvar #0 $table angles - puts [format "\t%u, /* %s */" [expr [llength $angles] / 2] $table] + upvar #0 $table angles + puts [format "\t%u, /* %s */" [expr [llength $angles] / 2] $table] } -puts "};" +puts "};\n" + +puts "} // anonymous namespace\n" # Newline at the end of file puts "" From a4cf9be8e0afdc2674c482791179d70e7312ae5d Mon Sep 17 00:00:00 2001 From: Anton Matosov Date: Mon, 5 Jan 2015 01:06:57 +0200 Subject: [PATCH 392/652] Enabled C++11 for tests target --- unittests/CMakeLists.txt | 10 ++++++++++ unittests/Makefile | 2 +- 2 files changed, 11 insertions(+), 1 deletion(-) diff --git a/unittests/CMakeLists.txt b/unittests/CMakeLists.txt index d264ae8cdc..ec3e4104ec 100644 --- a/unittests/CMakeLists.txt +++ b/unittests/CMakeLists.txt @@ -19,6 +19,16 @@ function(add_gtest) endforeach() endfunction() +include(CheckCXXCompilerFlag) +CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11) +CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X) +if(COMPILER_SUPPORTS_CXX11) + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") +elseif(COMPILER_SUPPORTS_CXX0X) + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x") +else() + message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.") +endif() # add each test # todo: add mixer_test sbus2_test st24_test sf0x_test diff --git a/unittests/Makefile b/unittests/Makefile index 5faa50bb5f..038e8b8f7f 100644 --- a/unittests/Makefile +++ b/unittests/Makefile @@ -1,7 +1,7 @@ CC=g++ CFLAGS=-I. -I../src/modules -I ../src/include -I../src/drivers \ - -I../src -I../src/lib -D__EXPORT="" -Dnullptr="0" -lm + -I../src -I../src/lib -D__EXPORT="" -Dnullptr="0" -lm -std=c++11 # Points to the root of Google Test, relative to where this file is. # Remember to tweak this if you move this file. From 0e4268a80450c3cc9915bdf823fb78981149318b Mon Sep 17 00:00:00 2001 From: Anton Matosov Date: Mon, 5 Jan 2015 01:18:29 +0200 Subject: [PATCH 393/652] Enabled C++11 for tests target (trying other compiler flag) --- unittests/Makefile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/unittests/Makefile b/unittests/Makefile index 038e8b8f7f..7846638d40 100644 --- a/unittests/Makefile +++ b/unittests/Makefile @@ -1,7 +1,7 @@ CC=g++ CFLAGS=-I. -I../src/modules -I ../src/include -I../src/drivers \ - -I../src -I../src/lib -D__EXPORT="" -Dnullptr="0" -lm -std=c++11 + -I../src -I../src/lib -D__EXPORT="" -Dnullptr="0" -lm -std=gnu++0x # Points to the root of Google Test, relative to where this file is. # Remember to tweak this if you move this file. From 2daf30fb251c7dc8d59f0302ecb444b116081931 Mon Sep 17 00:00:00 2001 From: Anton Matosov Date: Mon, 5 Jan 2015 02:39:13 +0200 Subject: [PATCH 394/652] Rewrote multi_tables with python --- src/modules/systemlib/mixer/multi_tables | 156 ------------------- src/modules/systemlib/mixer/multi_tables.py | 159 ++++++++++++++++++++ 2 files changed, 159 insertions(+), 156 deletions(-) delete mode 100755 src/modules/systemlib/mixer/multi_tables create mode 100755 src/modules/systemlib/mixer/multi_tables.py diff --git a/src/modules/systemlib/mixer/multi_tables b/src/modules/systemlib/mixer/multi_tables deleted file mode 100755 index b29bf540ae..0000000000 --- a/src/modules/systemlib/mixer/multi_tables +++ /dev/null @@ -1,156 +0,0 @@ -#!/usr/bin/tclsh -# -# Generate multirotor mixer scale tables compatible with the ArduCopter layout -# - -puts "/*" -puts "* These file is automatically generated by multi_tables - do not edit." -puts "*/" -puts "" - -proc rad {a} { expr ($a / 360.0) * 2 * acos(-1) } -proc rcos {a} { expr cos([rad $a])} - - -set quad_x { - 45 CCW - -135 CCW - -45 CW - 135 CW -} - -set quad_plus { - 90 CCW - -90 CCW - 0 CW - 180 CW -} - -set quad_v { - 18.8 0.4242 - -18.8 1.0 - -18.8 -0.4242 - 18.8 -1.0 -} - -set quad_wide { - 68 CCW - -129 CCW - -68 CW - 129 CW -} - -set hex_x { - 90 CW - -90 CCW - -30 CW - 150 CCW - 30 CCW - -150 CW -} - -set hex_plus { - 0 CW - 180 CCW - -120 CW - 60 CCW - -60 CCW - 120 CW -} - -set hex_cox { - 60 CW - 60 CCW - 180 CW - 180 CCW - -60 CW - -60 CCW -} - -set octa_x { - 22.5 CW - -157.5 CW - 67.5 CCW - 157.5 CCW - -22.5 CCW - -112.5 CCW - -67.5 CW - 112.5 CW -} - -set octa_plus { - 0 CW - 180 CW - 45 CCW - 135 CCW - -45 CCW - -135 CCW - -90 CW - 90 CW -} - -set octa_cox { - 45 CCW - -45 CW - -135 CCW - 135 CW - -45 CCW - 45 CW - 135 CCW - -135 CW -} - -set twin_engine { - 90 0.0 - -90 0.0 -} - - -set tables {quad_x quad_plus quad_v quad_wide hex_x hex_plus hex_cox octa_x octa_plus octa_cox twin_engine} - -puts "enum MultirotorMixer::Geometry : unsigned int {" -foreach table $tables { - puts [format "\t%s," [string toupper $table]] -} -puts "\n\tMAX_GEOMETRY" -puts "}; // enum MultirotorMixer::Geometry\n" - - -puts "namespace {" - -proc factors {a d} { puts [format "\t{ %9.6f, %9.6f, %9.6f }," [rcos [expr $a + 90]] [rcos $a] [expr $d]]} - -foreach table $tables { - puts [format "const MultirotorMixer::Rotor _config_%s\[\] = {" $table] - - upvar #0 $table angles - foreach {angle dir} $angles { - if {$dir == "CW"} { - set dd -1.0 - } elseif {$dir == "CCW"} { - set dd 1.0 - } else { - set dd $dir - } - factors $angle $dd - } - puts "};\n" -} - -puts "const MultirotorMixer::Rotor *_config_index\[\] = {" -foreach table $tables { - puts [format "\t&_config_%s\[0\]," $table] -} -puts "};\n" - -puts "const unsigned _config_rotor_count\[\] = {" -foreach table $tables { - upvar #0 $table angles - puts [format "\t%u, /* %s */" [expr [llength $angles] / 2] $table] -} -puts "};\n" - -puts "} // anonymous namespace\n" - -# Newline at the end of file -puts "" diff --git a/src/modules/systemlib/mixer/multi_tables.py b/src/modules/systemlib/mixer/multi_tables.py new file mode 100755 index 0000000000..07594ee722 --- /dev/null +++ b/src/modules/systemlib/mixer/multi_tables.py @@ -0,0 +1,159 @@ +#!/usr/bin/python +# +# Generate multirotor mixer scale tables compatible with the ArduCopter layout +# + +import math + +print "/*" +print "* These file is automatically generated by multi_tables.py - do not edit." +print "*/" +print "" + +def rcos(angleInRadians): + return math.cos(math.radians(angleInRadians)) + +CCW = 1.0 +CW = -CCW + +quad_x = [ + [ 45, CCW], + [-135, CCW], + [-45, CW], + [135, CW], +] + +quad_plus = [ + [ 90, CCW], + [ -90, CCW], + [ 0, CW], + [ 180, CW], +] + +quad_v = [ + [ 18.8, 0.4242], + [ -18.8, 1.0], + [ -18.8, -0.4242], + [ 18.8, -1.0], +] + +quad_wide = [ + [ 68, CCW], + [ -129, CCW], + [ -68, CW], + [ 129, CW], +] + +hex_x = [ + [ 90, CW], + [ -90, CCW], + [ -30, CW], + [ 150, CCW], + [ 30, CCW], + [-150, CW], +] + +hex_plus = [ + [ 0, CW], + [ 180, CCW], + [-120, CW], + [ 60, CCW], + [ -60, CCW], + [ 120, CW], +] + +hex_cox = [ + [ 60, CW], + [ 60, CCW], + [ 180, CW], + [ 180, CCW], + [ -60, CW], + [ -60, CCW], +] + +octa_x = [ + [ 22.5, CW], + [-157.5, CW], + [ 67.5, CCW], + [ 157.5, CCW], + [ -22.5, CCW], + [-112.5, CCW], + [ -67.5, CW], + [ 112.5, CW], +] + +octa_plus = [ + [ 0, CW], + [ 180, CW], + [ 45, CCW], + [ 135, CCW], + [ -45, CCW], + [-135, CCW], + [ -90, CW], + [ 90, CW], +] + +octa_cox = [ + [ 45, CCW], + [ -45, CW], + [-135, CCW], + [ 135, CW], + [ -45, CCW], + [ 45, CW], + [ 135, CCW], + [-135, CW], +] + +twin_engine = [ + [ 90, 0.0], + [-90, 0.0], +] + +def variableName(variable): + for variableName, value in list(globals().iteritems()): + if value is variable: + return variableName + +tables = [quad_x, quad_plus, quad_v, quad_wide, hex_x, hex_plus, hex_cox, octa_x, octa_plus, octa_cox, twin_engine] + + +def printEnum(): + print "enum MultirotorMixer::Geometry : unsigned int {" + for table in tables: + print "\t{},".format(variableName(table).upper()) + + print "\n\tMAX_GEOMETRY" + print "}; // enum MultirotorMixer::Geometry\n" + +def printScaleTables(): + for table in tables: + print "const MultirotorMixer::Rotor _config_{}[] = {{".format(variableName(table)) + for (angle, yawScale) in table: + rollScale = rcos(angle + 90) + pitchScale = rcos(angle) + print "\t{{ {:9f}, {:9f}, {:9f} }},".format(rollScale, pitchScale, yawScale) + print "};\n" + +def printScaleTablesIndex(): + print "const MultirotorMixer::Rotor *_config_index[] = {" + for table in tables: + print "\t&_config_{}[0],".format(variableName(table)) + print "};\n" + + +def printScaleTablesCounts(): + print "const unsigned _config_rotor_count[] = {" + for table in tables: + print "\t{}, /* {} */".format(len(table), variableName(table)) + print "};\n" + + + +printEnum() + +print "namespace {" +printScaleTables() +printScaleTablesIndex() +printScaleTablesCounts() + +print "} // anonymous namespace\n" From 21b45ae86b1525e052ea9b37e78c960498a95c72 Mon Sep 17 00:00:00 2001 From: Anton Matosov Date: Mon, 5 Jan 2015 03:18:14 +0200 Subject: [PATCH 395/652] Removed extension for multi_tables to not affect its users Fixed dependencies for the mixer_multirotor.cpp from all the modules --- src/modules/px4iofirmware/module.mk | 11 ++++++++++- src/modules/systemlib/mixer/module.mk | 4 ++-- .../systemlib/mixer/{multi_tables.py => multi_tables} | 0 3 files changed, 12 insertions(+), 3 deletions(-) rename src/modules/systemlib/mixer/{multi_tables.py => multi_tables} (100%) diff --git a/src/modules/px4iofirmware/module.mk b/src/modules/px4iofirmware/module.mk index eb99e8a966..bfcbaa9f86 100644 --- a/src/modules/px4iofirmware/module.mk +++ b/src/modules/px4iofirmware/module.mk @@ -1,5 +1,4 @@ - SRCS = adc.c \ controls.c \ dsm.c \ @@ -24,3 +23,13 @@ ifeq ($(BOARD),px4io-v2) SRCS += serial.c \ ../systemlib/hx_stream.c endif + +SELF_DIR := $(dir $(lastword $(MAKEFILE_LIST))) +MIXER_DIR:=$(SELF_DIR)../systemlib/mixer/ + +# Add explicit dependency, as implicit one doesn't work often. +$(MIXER_DIR)mixer_multirotor.cpp : $(MIXER_DIR)mixer_multirotor.generated.h + +$(MIXER_DIR)mixer_multirotor.generated.h : $(MIXER_DIR)multi_tables + $(MIXER_DIR)multi_tables > $(MIXER_DIR)mixer_multirotor.generated.h + \ No newline at end of file diff --git a/src/modules/systemlib/mixer/module.mk b/src/modules/systemlib/mixer/module.mk index 6f4befb576..5e818ff03f 100644 --- a/src/modules/systemlib/mixer/module.mk +++ b/src/modules/systemlib/mixer/module.mk @@ -46,7 +46,7 @@ SRCS = mixer.cpp \ SELF_DIR := $(dir $(lastword $(MAKEFILE_LIST))) # Add explicit dependency, as implicit one doesn't work often. -mixer_multirotor.cpp : $(SELF_DIR)mixer_multirotor.generated.h +$(SELF_DIR)mixer_multirotor.cpp : $(SELF_DIR)mixer_multirotor.generated.h $(SELF_DIR)mixer_multirotor.generated.h : $(SELF_DIR)multi_tables - $(SELF_DIR)multi_tables > $(SELF_DIR)mixer_multirotor.generated.h + $(SELF_DIR)multi_tables> $(SELF_DIR)mixer_multirotor.generated.h diff --git a/src/modules/systemlib/mixer/multi_tables.py b/src/modules/systemlib/mixer/multi_tables similarity index 100% rename from src/modules/systemlib/mixer/multi_tables.py rename to src/modules/systemlib/mixer/multi_tables From 1bc6c44a0e30059620bc5fec85c5d096cd911df0 Mon Sep 17 00:00:00 2001 From: Anton Matosov Date: Mon, 5 Jan 2015 12:37:05 +0200 Subject: [PATCH 396/652] Fixed compilation of tests target in clean checkout Moved mixer_multirotor.generated.h generation to the standalone makefile to prevent copypaste --- src/modules/px4iofirmware/module.mk | 8 +--- src/modules/systemlib/mixer/module.mk | 8 +--- src/modules/systemlib/mixer/multi_tables.mk | 41 +++++++++++++++++++++ unittests/Makefile | 2 + 4 files changed, 45 insertions(+), 14 deletions(-) create mode 100644 src/modules/systemlib/mixer/multi_tables.mk diff --git a/src/modules/px4iofirmware/module.mk b/src/modules/px4iofirmware/module.mk index bfcbaa9f86..cc3de09e9b 100644 --- a/src/modules/px4iofirmware/module.mk +++ b/src/modules/px4iofirmware/module.mk @@ -24,12 +24,6 @@ SRCS += serial.c \ ../systemlib/hx_stream.c endif -SELF_DIR := $(dir $(lastword $(MAKEFILE_LIST))) -MIXER_DIR:=$(SELF_DIR)../systemlib/mixer/ -# Add explicit dependency, as implicit one doesn't work often. -$(MIXER_DIR)mixer_multirotor.cpp : $(MIXER_DIR)mixer_multirotor.generated.h - -$(MIXER_DIR)mixer_multirotor.generated.h : $(MIXER_DIR)multi_tables - $(MIXER_DIR)multi_tables > $(MIXER_DIR)mixer_multirotor.generated.h +include ../systemlib/mixer/multi_tables.mk \ No newline at end of file diff --git a/src/modules/systemlib/mixer/module.mk b/src/modules/systemlib/mixer/module.mk index 5e818ff03f..7ec77cd2ff 100644 --- a/src/modules/systemlib/mixer/module.mk +++ b/src/modules/systemlib/mixer/module.mk @@ -43,10 +43,4 @@ SRCS = mixer.cpp \ mixer_simple.cpp \ mixer_load.c -SELF_DIR := $(dir $(lastword $(MAKEFILE_LIST))) - -# Add explicit dependency, as implicit one doesn't work often. -$(SELF_DIR)mixer_multirotor.cpp : $(SELF_DIR)mixer_multirotor.generated.h - -$(SELF_DIR)mixer_multirotor.generated.h : $(SELF_DIR)multi_tables - $(SELF_DIR)multi_tables> $(SELF_DIR)mixer_multirotor.generated.h +include multi_tables.mk diff --git a/src/modules/systemlib/mixer/multi_tables.mk b/src/modules/systemlib/mixer/multi_tables.mk new file mode 100644 index 0000000000..02216c3d44 --- /dev/null +++ b/src/modules/systemlib/mixer/multi_tables.mk @@ -0,0 +1,41 @@ +############################################################################ +# +# 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. +# +############################################################################ + + +SELF_DIR := $(dir $(lastword $(MAKEFILE_LIST))) + +# Add explicit dependency, as implicit one doesn't work often. +$(SELF_DIR)mixer_multirotor.cpp : $(SELF_DIR)mixer_multirotor.generated.h + +$(SELF_DIR)mixer_multirotor.generated.h : $(SELF_DIR)multi_tables + $(SELF_DIR)multi_tables> $(SELF_DIR)mixer_multirotor.generated.h \ No newline at end of file diff --git a/unittests/Makefile b/unittests/Makefile index 7846638d40..719db2ca2e 100644 --- a/unittests/Makefile +++ b/unittests/Makefile @@ -53,6 +53,8 @@ MIXER_FILES=../src/systemcmds/tests/test_mixer.cpp \ hrt.cpp \ mixer_test.cpp +include ../src/modules/systemlib/mixer/multi_tables.mk + SBUS2_FILES=../src/modules/px4iofirmware/sbus.c \ hrt.cpp \ sbus2_test.cpp From 6ba0b758045261906d60d772ac001d8b46d0b4c7 Mon Sep 17 00:00:00 2001 From: Anton Matosov Date: Mon, 5 Jan 2015 12:41:25 +0200 Subject: [PATCH 397/652] Replaced tabs with spaces as it is critical for some python builds --- src/modules/systemlib/mixer/multi_tables | 64 ++++++++++++------------ 1 file changed, 32 insertions(+), 32 deletions(-) diff --git a/src/modules/systemlib/mixer/multi_tables b/src/modules/systemlib/mixer/multi_tables index 07594ee722..786b63b023 100755 --- a/src/modules/systemlib/mixer/multi_tables +++ b/src/modules/systemlib/mixer/multi_tables @@ -17,14 +17,14 @@ CCW = 1.0 CW = -CCW quad_x = [ - [ 45, CCW], - [-135, CCW], - [-45, CW], + [ 45, CCW], + [-135, CCW], + [-45, CW], [135, CW], ] quad_plus = [ - [ 90, CCW], + [ 90, CCW], [ -90, CCW], [ 0, CW], [ 180, CW], @@ -38,14 +38,14 @@ quad_v = [ ] quad_wide = [ - [ 68, CCW], + [ 68, CCW], [ -129, CCW], [ -68, CW], [ 129, CW], ] hex_x = [ - [ 90, CW], + [ 90, CW], [ -90, CCW], [ -30, CW], [ 150, CCW], @@ -110,44 +110,44 @@ twin_engine = [ ] def variableName(variable): - for variableName, value in list(globals().iteritems()): - if value is variable: - return variableName + for variableName, value in list(globals().iteritems()): + if value is variable: + return variableName tables = [quad_x, quad_plus, quad_v, quad_wide, hex_x, hex_plus, hex_cox, octa_x, octa_plus, octa_cox, twin_engine] def printEnum(): - print "enum MultirotorMixer::Geometry : unsigned int {" - for table in tables: - print "\t{},".format(variableName(table).upper()) + print "enum MultirotorMixer::Geometry : unsigned int {" + for table in tables: + print "\t{},".format(variableName(table).upper()) - print "\n\tMAX_GEOMETRY" - print "}; // enum MultirotorMixer::Geometry\n" + print "\n\tMAX_GEOMETRY" + print "}; // enum MultirotorMixer::Geometry\n" def printScaleTables(): - for table in tables: - print "const MultirotorMixer::Rotor _config_{}[] = {{".format(variableName(table)) - for (angle, yawScale) in table: - rollScale = rcos(angle + 90) - pitchScale = rcos(angle) - print "\t{{ {:9f}, {:9f}, {:9f} }},".format(rollScale, pitchScale, yawScale) - print "};\n" - + for table in tables: + print "const MultirotorMixer::Rotor _config_{}[] = {{".format(variableName(table)) + for (angle, yawScale) in table: + rollScale = rcos(angle + 90) + pitchScale = rcos(angle) + print "\t{{ {:9f}, {:9f}, {:9f} }},".format(rollScale, pitchScale, yawScale) + print "};\n" + def printScaleTablesIndex(): - print "const MultirotorMixer::Rotor *_config_index[] = {" - for table in tables: - print "\t&_config_{}[0],".format(variableName(table)) - print "};\n" + print "const MultirotorMixer::Rotor *_config_index[] = {" + for table in tables: + print "\t&_config_{}[0],".format(variableName(table)) + print "};\n" def printScaleTablesCounts(): - print "const unsigned _config_rotor_count[] = {" - for table in tables: - print "\t{}, /* {} */".format(len(table), variableName(table)) - print "};\n" - - + print "const unsigned _config_rotor_count[] = {" + for table in tables: + print "\t{}, /* {} */".format(len(table), variableName(table)) + print "};\n" + + printEnum() From 3bf5dd416e58631285373a0d88a6e85406726043 Mon Sep 17 00:00:00 2001 From: Anton Matosov Date: Mon, 5 Jan 2015 13:22:53 +0200 Subject: [PATCH 398/652] Fixed compilation --- src/modules/px4iofirmware/module.mk | 4 ++-- src/modules/systemlib/mixer/module.mk | 3 ++- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/src/modules/px4iofirmware/module.mk b/src/modules/px4iofirmware/module.mk index cc3de09e9b..844e493cd4 100644 --- a/src/modules/px4iofirmware/module.mk +++ b/src/modules/px4iofirmware/module.mk @@ -24,6 +24,6 @@ SRCS += serial.c \ ../systemlib/hx_stream.c endif - -include ../systemlib/mixer/multi_tables.mk +SELF_DIR := $(dir $(lastword $(MAKEFILE_LIST))) +include $(SELF_DIR)../systemlib/mixer/multi_tables.mk \ No newline at end of file diff --git a/src/modules/systemlib/mixer/module.mk b/src/modules/systemlib/mixer/module.mk index 7ec77cd2ff..3fd07f5ba1 100644 --- a/src/modules/systemlib/mixer/module.mk +++ b/src/modules/systemlib/mixer/module.mk @@ -43,4 +43,5 @@ SRCS = mixer.cpp \ mixer_simple.cpp \ mixer_load.c -include multi_tables.mk +SELF_DIR := $(dir $(lastword $(MAKEFILE_LIST))) +include $(SELF_DIR)multi_tables.mk From 68978b814aaf9e355434ef2a71305a1a850a826c Mon Sep 17 00:00:00 2001 From: Anton Matosov Date: Mon, 5 Jan 2015 13:23:47 +0200 Subject: [PATCH 399/652] Fixed autoconf variable check --- 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 2d5e272bdd..668026c616 100644 --- a/ROMFS/px4fmu_common/init.d/10018_tbs_endurance +++ b/ROMFS/px4fmu_common/init.d/10018_tbs_endurance @@ -9,7 +9,7 @@ sh /etc/init.d/rc.mc_defaults -if [ $DO_AUTOCONFIG == yes ] +if [ $AUTOCNF == yes ] then param set MC_ROLL_P 7.0 param set MC_ROLLRATE_P 0.07 From 18bf501992afa8b561822615104c3868091429b9 Mon Sep 17 00:00:00 2001 From: Anton Matosov Date: Mon, 5 Jan 2015 21:59:56 +0200 Subject: [PATCH 400/652] Added output of the generated multi_tables to the terminal as well as to the file Changed compiler C++0x enabling flag to check if it is a cuase Updated copyrights --- src/modules/systemlib/mixer/multi_tables | 35 ++++++++++++++++++++- src/modules/systemlib/mixer/multi_tables.mk | 6 ++-- unittests/Makefile | 2 +- 3 files changed, 39 insertions(+), 4 deletions(-) diff --git a/src/modules/systemlib/mixer/multi_tables b/src/modules/systemlib/mixer/multi_tables index 786b63b023..2752165cfb 100755 --- a/src/modules/systemlib/mixer/multi_tables +++ b/src/modules/systemlib/mixer/multi_tables @@ -1,4 +1,37 @@ #!/usr/bin/python +############################################################################ +# +# 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. +# +############################################################################ + # # Generate multirotor mixer scale tables compatible with the ArduCopter layout # @@ -6,7 +39,7 @@ import math print "/*" -print "* These file is automatically generated by multi_tables.py - do not edit." +print "* These file is automatically generated by multi_tables - do not edit." print "*/" print "" diff --git a/src/modules/systemlib/mixer/multi_tables.mk b/src/modules/systemlib/mixer/multi_tables.mk index 02216c3d44..6429961cc8 100644 --- a/src/modules/systemlib/mixer/multi_tables.mk +++ b/src/modules/systemlib/mixer/multi_tables.mk @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. +# Copyright (c) 2014 Anton Matosov . All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions @@ -38,4 +38,6 @@ SELF_DIR := $(dir $(lastword $(MAKEFILE_LIST))) $(SELF_DIR)mixer_multirotor.cpp : $(SELF_DIR)mixer_multirotor.generated.h $(SELF_DIR)mixer_multirotor.generated.h : $(SELF_DIR)multi_tables - $(SELF_DIR)multi_tables> $(SELF_DIR)mixer_multirotor.generated.h \ No newline at end of file + $(SELF_DIR)multi_tables > $(SELF_DIR)mixer_multirotor.generated.h + $(SELF_DIR)multi_tables + \ No newline at end of file diff --git a/unittests/Makefile b/unittests/Makefile index 719db2ca2e..cb08aa8fcd 100644 --- a/unittests/Makefile +++ b/unittests/Makefile @@ -1,7 +1,7 @@ CC=g++ CFLAGS=-I. -I../src/modules -I ../src/include -I../src/drivers \ - -I../src -I../src/lib -D__EXPORT="" -Dnullptr="0" -lm -std=gnu++0x + -I../src -I../src/lib -D__EXPORT="" -Dnullptr="0" -lm -std=c++0x # Points to the root of Google Test, relative to where this file is. # Remember to tweak this if you move this file. From 117d43067f3c16c596a41cf0af5feaee1b11716b Mon Sep 17 00:00:00 2001 From: Anton Matosov Date: Mon, 5 Jan 2015 23:15:12 +0200 Subject: [PATCH 401/652] Switched to enum class out of class definition --- src/modules/systemlib/mixer/mixer.h | 17 +++++----- .../systemlib/mixer/mixer_multirotor.cpp | 33 ++++++++++--------- src/modules/systemlib/mixer/multi_tables | 4 +-- 3 files changed, 29 insertions(+), 25 deletions(-) diff --git a/src/modules/systemlib/mixer/mixer.h b/src/modules/systemlib/mixer/mixer.h index a0197df480..55c4ce6afd 100644 --- a/src/modules/systemlib/mixer/mixer.h +++ b/src/modules/systemlib/mixer/mixer.h @@ -441,6 +441,14 @@ private: SimpleMixer operator=(const SimpleMixer&); }; + +/** + * Supported multirotor geometries. + * + * Values are generated by the multi_tables script and placed to mixer_multirotor.generated.h + */ +enum class MultirotorGeometry : unsigned int; + /** * Multi-rotor mixer for pre-defined vehicle geometries. * @@ -450,13 +458,6 @@ private: class __EXPORT MultirotorMixer : public Mixer { public: - /** - * Supported multirotor geometries. - * - * Values are generated by the multi_tables script and placed to mixer_multirotor.generated.h - */ - enum Geometry : unsigned int; - /** * Precalculated rotor mix. */ @@ -484,7 +485,7 @@ public: */ MultirotorMixer(ControlCallback control_cb, uintptr_t cb_handle, - Geometry geometry, + MultirotorGeometry geometry, float roll_scale, float pitch_scale, float yaw_scale, diff --git a/src/modules/systemlib/mixer/mixer_multirotor.cpp b/src/modules/systemlib/mixer/mixer_multirotor.cpp index 42e3513f4d..d4bceaa274 100644 --- a/src/modules/systemlib/mixer/mixer_multirotor.cpp +++ b/src/modules/systemlib/mixer/mixer_multirotor.cpp @@ -52,6 +52,7 @@ #include #include #include +#include #include "mixer.h" @@ -77,9 +78,11 @@ float constrain(float val, float min, float max) } } +using GeometryType = typename std::underlying_type::type; + MultirotorMixer::MultirotorMixer(ControlCallback control_cb, uintptr_t cb_handle, - Geometry geometry, + MultirotorGeometry geometry, float roll_scale, float pitch_scale, float yaw_scale, @@ -89,8 +92,8 @@ MultirotorMixer::MultirotorMixer(ControlCallback control_cb, _pitch_scale(pitch_scale), _yaw_scale(yaw_scale), _idle_speed(-1.0f + idle_speed * 2.0f), /* shift to output range here to avoid runtime calculation */ - _rotor_count(_config_rotor_count[geometry]), - _rotors(_config_index[geometry]) + _rotor_count(_config_rotor_count[(GeometryType)geometry]), + _rotors(_config_index[(GeometryType)geometry]) { } @@ -101,7 +104,7 @@ MultirotorMixer::~MultirotorMixer() MultirotorMixer * MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, const char *buf, unsigned &buflen) { - MultirotorMixer::Geometry geometry; + MultirotorGeometry geometry; char geomname[8]; int s[4]; int used; @@ -141,37 +144,37 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl debug("remaining in buf: %d, first char: %c", buflen, buf[0]); if (!strcmp(geomname, "4+")) { - geometry = MultirotorMixer::QUAD_PLUS; + geometry = MultirotorGeometry::QUAD_PLUS; } else if (!strcmp(geomname, "4x")) { - geometry = MultirotorMixer::QUAD_X; + geometry = MultirotorGeometry::QUAD_X; } else if (!strcmp(geomname, "4v")) { - geometry = MultirotorMixer::QUAD_V; + geometry = MultirotorGeometry::QUAD_V; } else if (!strcmp(geomname, "4w")) { - geometry = MultirotorMixer::QUAD_WIDE; + geometry = MultirotorGeometry::QUAD_WIDE; } else if (!strcmp(geomname, "6+")) { - geometry = MultirotorMixer::HEX_PLUS; + geometry = MultirotorGeometry::HEX_PLUS; } else if (!strcmp(geomname, "6x")) { - geometry = MultirotorMixer::HEX_X; + geometry = MultirotorGeometry::HEX_X; } else if (!strcmp(geomname, "6c")) { - geometry = MultirotorMixer::HEX_COX; + geometry = MultirotorGeometry::HEX_COX; } else if (!strcmp(geomname, "8+")) { - geometry = MultirotorMixer::OCTA_PLUS; + geometry = MultirotorGeometry::OCTA_PLUS; } else if (!strcmp(geomname, "8x")) { - geometry = MultirotorMixer::OCTA_X; + geometry = MultirotorGeometry::OCTA_X; } else if (!strcmp(geomname, "8c")) { - geometry = MultirotorMixer::OCTA_COX; + geometry = MultirotorGeometry::OCTA_COX; } else if (!strcmp(geomname, "2-")) { - geometry = MultirotorMixer::TWIN_ENGINE; + geometry = MultirotorGeometry::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 2752165cfb..5d746fcfaf 100755 --- a/src/modules/systemlib/mixer/multi_tables +++ b/src/modules/systemlib/mixer/multi_tables @@ -151,12 +151,12 @@ tables = [quad_x, quad_plus, quad_v, quad_wide, hex_x, hex_plus, hex_cox, octa_x def printEnum(): - print "enum MultirotorMixer::Geometry : unsigned int {" + print "enum class MultirotorGeometry : unsigned int {" for table in tables: print "\t{},".format(variableName(table).upper()) print "\n\tMAX_GEOMETRY" - print "}; // enum MultirotorMixer::Geometry\n" + print "}; // enum class MultirotorGeometry\n" def printScaleTables(): for table in tables: From 29d0754df633b50bc456922ff68d3e6c020f1d81 Mon Sep 17 00:00:00 2001 From: Anton Matosov Date: Mon, 5 Jan 2015 23:22:43 +0200 Subject: [PATCH 402/652] std::underlying_type is not supported by the g++ 4.6 used by CI server --- src/modules/systemlib/mixer/mixer.h | 3 ++- src/modules/systemlib/mixer/mixer_multirotor.cpp | 6 ++---- src/modules/systemlib/mixer/multi_tables | 2 +- 3 files changed, 5 insertions(+), 6 deletions(-) diff --git a/src/modules/systemlib/mixer/mixer.h b/src/modules/systemlib/mixer/mixer.h index 55c4ce6afd..6494a314e7 100644 --- a/src/modules/systemlib/mixer/mixer.h +++ b/src/modules/systemlib/mixer/mixer.h @@ -447,7 +447,8 @@ private: * * Values are generated by the multi_tables script and placed to mixer_multirotor.generated.h */ -enum class MultirotorGeometry : unsigned int; +using MultirotorGeometryUnderlyingType = unsigned int; +enum class MultirotorGeometry : MultirotorGeometryUnderlyingType; /** * Multi-rotor mixer for pre-defined vehicle geometries. diff --git a/src/modules/systemlib/mixer/mixer_multirotor.cpp b/src/modules/systemlib/mixer/mixer_multirotor.cpp index d4bceaa274..bee9d0edca 100644 --- a/src/modules/systemlib/mixer/mixer_multirotor.cpp +++ b/src/modules/systemlib/mixer/mixer_multirotor.cpp @@ -78,8 +78,6 @@ float constrain(float val, float min, float max) } } -using GeometryType = typename std::underlying_type::type; - MultirotorMixer::MultirotorMixer(ControlCallback control_cb, uintptr_t cb_handle, MultirotorGeometry geometry, @@ -92,8 +90,8 @@ MultirotorMixer::MultirotorMixer(ControlCallback control_cb, _pitch_scale(pitch_scale), _yaw_scale(yaw_scale), _idle_speed(-1.0f + idle_speed * 2.0f), /* shift to output range here to avoid runtime calculation */ - _rotor_count(_config_rotor_count[(GeometryType)geometry]), - _rotors(_config_index[(GeometryType)geometry]) + _rotor_count(_config_rotor_count[(MultirotorGeometryUnderlyingType)geometry]), + _rotors(_config_index[(MultirotorGeometryUnderlyingType)geometry]) { } diff --git a/src/modules/systemlib/mixer/multi_tables b/src/modules/systemlib/mixer/multi_tables index 5d746fcfaf..01ff1b719e 100755 --- a/src/modules/systemlib/mixer/multi_tables +++ b/src/modules/systemlib/mixer/multi_tables @@ -151,7 +151,7 @@ tables = [quad_x, quad_plus, quad_v, quad_wide, hex_x, hex_plus, hex_cox, octa_x def printEnum(): - print "enum class MultirotorGeometry : unsigned int {" + print "enum class MultirotorGeometry : MultirotorGeometryUnderlyingType {" for table in tables: print "\t{},".format(variableName(table).upper()) From e466e10d234c142c28b5d5a7661eb508a514ffb9 Mon Sep 17 00:00:00 2001 From: Anton Matosov Date: Mon, 5 Jan 2015 23:33:19 +0200 Subject: [PATCH 403/652] using type aliases are not supported by g++ 4.6 --- src/modules/systemlib/mixer/mixer.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/systemlib/mixer/mixer.h b/src/modules/systemlib/mixer/mixer.h index 6494a314e7..864ce21a58 100644 --- a/src/modules/systemlib/mixer/mixer.h +++ b/src/modules/systemlib/mixer/mixer.h @@ -447,7 +447,7 @@ private: * * Values are generated by the multi_tables script and placed to mixer_multirotor.generated.h */ -using MultirotorGeometryUnderlyingType = unsigned int; +typedef unsigned int MultirotorGeometryUnderlyingType; enum class MultirotorGeometry : MultirotorGeometryUnderlyingType; /** From b04559e5fa856aad48d7c371806557dab9ba12a3 Mon Sep 17 00:00:00 2001 From: Anton Matosov Date: Mon, 5 Jan 2015 23:48:18 +0200 Subject: [PATCH 404/652] Removed include that couses compilation failure --- src/modules/systemlib/mixer/mixer_multirotor.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/modules/systemlib/mixer/mixer_multirotor.cpp b/src/modules/systemlib/mixer/mixer_multirotor.cpp index bee9d0edca..5cfbe47f07 100644 --- a/src/modules/systemlib/mixer/mixer_multirotor.cpp +++ b/src/modules/systemlib/mixer/mixer_multirotor.cpp @@ -52,7 +52,6 @@ #include #include #include -#include #include "mixer.h" From a73108d116f950331a20f308c33ff8e5c6cda8d7 Mon Sep 17 00:00:00 2001 From: Anton Matosov Date: Mon, 5 Jan 2015 13:23:47 +0200 Subject: [PATCH 405/652] Fixed autoconf variable check --- 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 2d5e272bdd..668026c616 100644 --- a/ROMFS/px4fmu_common/init.d/10018_tbs_endurance +++ b/ROMFS/px4fmu_common/init.d/10018_tbs_endurance @@ -9,7 +9,7 @@ sh /etc/init.d/rc.mc_defaults -if [ $DO_AUTOCONFIG == yes ] +if [ $AUTOCNF == yes ] then param set MC_ROLL_P 7.0 param set MC_ROLLRATE_P 0.07 From 6e144e6e42232b1ecc7a972f1d4e532694efb15a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 5 Jan 2015 20:54:44 +0100 Subject: [PATCH 406/652] Multi-tables: Adjust shebang, add missing include guards --- src/modules/systemlib/mixer/multi_tables | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/src/modules/systemlib/mixer/multi_tables b/src/modules/systemlib/mixer/multi_tables index 786b63b023..46dfc5bffc 100755 --- a/src/modules/systemlib/mixer/multi_tables +++ b/src/modules/systemlib/mixer/multi_tables @@ -1,4 +1,4 @@ -#!/usr/bin/python +#!/usr/bin/env python # # Generate multirotor mixer scale tables compatible with the ArduCopter layout # @@ -6,9 +6,12 @@ import math print "/*" -print "* These file is automatically generated by multi_tables.py - do not edit." +print "* This file is automatically generated by multi_tables.py - do not edit." print "*/" print "" +print "#ifndef _MIXER_MULTI_TABLES" +print "#define _MIXER_MULTI_TABLES" +print "" def rcos(angleInRadians): return math.cos(math.radians(angleInRadians)) @@ -157,3 +160,5 @@ printScaleTablesIndex() printScaleTablesCounts() print "} // anonymous namespace\n" +print "#endif /* _MIXER_MULTI_TABLES */" +print "" From a6ad8b3cb74bf02c269c18187fdd938391581970 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 5 Jan 2015 20:54:57 +0100 Subject: [PATCH 407/652] Travis CI: Output generated mixer table. --- .travis.yml | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/.travis.yml b/.travis.yml index b201d5130c..586c0c756a 100644 --- a/.travis.yml +++ b/.travis.yml @@ -2,9 +2,12 @@ # http://travis-ci.org language: cpp +compiler: gcc before_script: - - sudo apt-get update -q + - sudo add-apt-repository --yes ppa:ubuntu-toolchain-r/test + - sudo apt-get update -qq + - sudo apt-get install -qq g++-4.8 libstdc++-4.8-dev # Travis specific tools - sudo apt-get install s3cmd grep zip mailutils # General toolchain dependencies @@ -39,6 +42,7 @@ script: - arm-none-eabi-gcc --version - echo 'Running Tests..' && echo -en 'travis_fold:start:script.1\\r' - make tests + - cat src/modules/systemlib/mixer/mixer_multirotor.generated.h - echo -en 'travis_fold:end:script.1\\r' - echo 'Building NuttX..' && echo -en 'travis_fold:start:script.2\\r' - make archives From 3fb61cf41e054fc27946281b8962d3ca9e3b0cba Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 5 Jan 2015 23:13:03 +0100 Subject: [PATCH 408/652] Travis CI: Install GCC 4.8 as well --- .travis.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.travis.yml b/.travis.yml index 586c0c756a..957ab7f148 100644 --- a/.travis.yml +++ b/.travis.yml @@ -7,7 +7,7 @@ compiler: gcc before_script: - sudo add-apt-repository --yes ppa:ubuntu-toolchain-r/test - sudo apt-get update -qq - - sudo apt-get install -qq g++-4.8 libstdc++-4.8-dev + - sudo apt-get install -qq gcc-4.8 g++-4.8 libstdc++-4.8-dev # Travis specific tools - sudo apt-get install s3cmd grep zip mailutils # General toolchain dependencies From 3f7b9b45eaa15fc9b14b0afe07726fcc8b61e6d7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 5 Jan 2015 23:23:20 +0100 Subject: [PATCH 409/652] Travis CI: More fixes to really move to GCC 4.8 --- .travis.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.travis.yml b/.travis.yml index 957ab7f148..fd2a6b6d1c 100644 --- a/.travis.yml +++ b/.travis.yml @@ -2,12 +2,12 @@ # http://travis-ci.org language: cpp -compiler: gcc before_script: - sudo add-apt-repository --yes ppa:ubuntu-toolchain-r/test - sudo apt-get update -qq - - sudo apt-get install -qq gcc-4.8 g++-4.8 libstdc++-4.8-dev + - if [ "$CXX" = "g++" ]; then sudo apt-get install -qq g++-4.8 gcc-4.8 libstdc++-4.8-dev; fi + - if [ "$CXX" = "g++" ]; then export CXX="g++-4.8" CC="gcc-4.8"; fi # Travis specific tools - sudo apt-get install s3cmd grep zip mailutils # General toolchain dependencies From 9767dd6c30472a06bbfce8d95e83d37c7061ed06 Mon Sep 17 00:00:00 2001 From: Anton Matosov Date: Tue, 6 Jan 2015 00:54:17 +0200 Subject: [PATCH 410/652] Switched to full C++11 feature set --- unittests/Makefile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/unittests/Makefile b/unittests/Makefile index cb08aa8fcd..4dbde55f10 100644 --- a/unittests/Makefile +++ b/unittests/Makefile @@ -1,7 +1,7 @@ CC=g++ CFLAGS=-I. -I../src/modules -I ../src/include -I../src/drivers \ - -I../src -I../src/lib -D__EXPORT="" -Dnullptr="0" -lm -std=c++0x + -I../src -I../src/lib -D__EXPORT="" -Dnullptr="0" -lm -std=c++11 # Points to the root of Google Test, relative to where this file is. # Remember to tweak this if you move this file. From 3efb1be48e7f5b3608a4bfef87a09748b3458c54 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 5 Jan 2015 18:18:47 -0500 Subject: [PATCH 411/652] move mixer_test and conversion_test to gtest --- unittests/CMakeLists.txt | 23 +++++++++++++++++++++-- unittests/Makefile | 21 +++------------------ unittests/conversion_test.cpp | 9 +++++++++ unittests/mixer_test.cpp | 21 +++++++++------------ 4 files changed, 42 insertions(+), 32 deletions(-) create mode 100644 unittests/conversion_test.cpp diff --git a/unittests/CMakeLists.txt b/unittests/CMakeLists.txt index d264ae8cdc..7a7b2a731d 100644 --- a/unittests/CMakeLists.txt +++ b/unittests/CMakeLists.txt @@ -11,16 +11,35 @@ include_directories(${CMAKE_SOURCE_DIR}/../src/modules) include_directories(${CMAKE_SOURCE_DIR}/../src/lib) add_definitions(-D__EXPORT=) +set(CMAKE_C_FLAGS "-std=c99") +set(CMAKE_CXX_FLAGS "-std=c++11 -Wno-write-strings") function(add_gtest) foreach(test_name ${ARGN}) target_link_libraries(${test_name} gtest_main) - add_test(${test_name}Test ${test_name}) + add_test(${test_name} ${test_name}) endforeach() endfunction() # add each test -# todo: add mixer_test sbus2_test st24_test sf0x_test +# todo: add sbus2_test st24_test sf0x_test add_executable(autodeclination_test autodeclination_test.cpp ${CMAKE_SOURCE_DIR}/../src/lib/geo_lookup/geo_mag_declination.c) add_gtest(autodeclination_test) + + +# mixer_test +add_executable(mixer_test mixer_test.cpp hrt.cpp + ${CMAKE_SOURCE_DIR}/../src/modules/systemlib/mixer/mixer.cpp + ${CMAKE_SOURCE_DIR}/../src/modules/systemlib/mixer/mixer_group.cpp + ${CMAKE_SOURCE_DIR}/../src/modules/systemlib/mixer/mixer_load.c + ${CMAKE_SOURCE_DIR}/../src/modules/systemlib/mixer/mixer_multirotor.cpp + ${CMAKE_SOURCE_DIR}/../src/modules/systemlib/mixer/mixer_simple.cpp + ${CMAKE_SOURCE_DIR}/../src/modules/systemlib/pwm_limit/pwm_limit.c + ${CMAKE_SOURCE_DIR}/../src/systemcmds/tests/test_mixer.cpp) +add_gtest(mixer_test) + +# conversion_test +add_executable(conversion_test conversion_test.cpp + ${CMAKE_SOURCE_DIR}/../src/systemcmds/tests/test_conv.cpp) +add_gtest(conversion_test) diff --git a/unittests/Makefile b/unittests/Makefile index 5faa50bb5f..0c50b38146 100644 --- a/unittests/Makefile +++ b/unittests/Makefile @@ -40,18 +40,7 @@ gtest_main.a: gtest-all.o gtest_main.o $(AR) $(ARFLAGS) $@ $^ -all: mixer_test sbus2_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 \ - hrt.cpp \ - mixer_test.cpp +all: sbus2_test st24_test sf0x_test SBUS2_FILES=../src/modules/px4iofirmware/sbus.c \ hrt.cpp \ @@ -66,9 +55,6 @@ SF0X_FILES= \ sf0x_test.cpp \ ../src/drivers/sf0x/sf0x_parser.cpp -mixer_test: $(MIXER_FILES) - $(CC) -o mixer_test $(MIXER_FILES) $(CFLAGS) - sbus2_test: $(SBUS2_FILES) $(CC) -o sbus2_test $(SBUS2_FILES) $(CFLAGS) @@ -82,12 +68,11 @@ cmake_gtests: mkdir -p build cd build && CC=gcc cmake .. && $(MAKE) && $(MAKE) test -unittests: clean mixer_test sbus2_test sf0x_test st24_test cmake_gtests - ./mixer_test +unittests: clean sbus2_test sf0x_test st24_test cmake_gtests ./sbus2_test ./sf0x_test ./st24_test .PHONY: clean clean: - rm -rf gtest.a gtest_main.a *.o $(ODIR)/*.o *~ core $(INCDIR)/*~ sample_unittest mixer_test sbus2_test st24_test sf0x_test build + rm -rf gtest.a gtest_main.a *.o $(ODIR)/*.o *~ core $(INCDIR)/*~ sample_unittest sbus2_test st24_test sf0x_test build diff --git a/unittests/conversion_test.cpp b/unittests/conversion_test.cpp new file mode 100644 index 0000000000..99e1c7721d --- /dev/null +++ b/unittests/conversion_test.cpp @@ -0,0 +1,9 @@ +#include +#include +#include "../../src/systemcmds/tests/tests.h" + +#include "gtest/gtest.h" + +TEST(ConversionTest, FMU_quad_w) { + ASSERT_EQ(test_conv(0, NULL), 0) << "Conversion test failed"; +} diff --git a/unittests/mixer_test.cpp b/unittests/mixer_test.cpp index 4919e325c8..1c808e052a 100644 --- a/unittests/mixer_test.cpp +++ b/unittests/mixer_test.cpp @@ -2,18 +2,15 @@ #include #include "../../src/systemcmds/tests/tests.h" -int main(int argc, char *argv[]) { +#include "gtest/gtest.h" - 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"}; - - if (ret = test_mixer(3, args)); - - test_conv(1, args); - - return 0; +TEST(MixerTest, IO_pass) { + char* args[] = {"mixer", "../../ROMFS/px4fmu_common/mixers/IO_pass.mix"}; + ASSERT_EQ(test_mixer(2, args), 0) << "IO_pass.mix failed"; } + +TEST(MixerTest, FMU_quad_w) { + char* args[] = {"mixer", "../../ROMFS/px4fmu_common/mixers/FMU_quad_w.mix"}; + ASSERT_EQ(test_mixer(2, args), 0) << "FMU_quad_w.mix failed"; +} From 5841686a2e17d0c0070da174dfa8ff729db081b5 Mon Sep 17 00:00:00 2001 From: Anton Matosov Date: Tue, 6 Jan 2015 01:27:09 +0200 Subject: [PATCH 412/652] Changed the way CC is reset to CXX, used a real value instead of hardcoded g++. --- unittests/Makefile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/unittests/Makefile b/unittests/Makefile index 4dbde55f10..f25a56fc44 100644 --- a/unittests/Makefile +++ b/unittests/Makefile @@ -1,5 +1,5 @@ -CC=g++ +CC=$(CXX) CFLAGS=-I. -I../src/modules -I ../src/include -I../src/drivers \ -I../src -I../src/lib -D__EXPORT="" -Dnullptr="0" -lm -std=c++11 From 6287231fecded6b3329e28994d133c0246817864 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 5 Jan 2015 18:34:13 -0500 Subject: [PATCH 413/652] cmake test harness output everything on failure --- unittests/CMakeLists.txt | 12 ++++++++---- unittests/Makefile | 2 +- 2 files changed, 9 insertions(+), 5 deletions(-) diff --git a/unittests/CMakeLists.txt b/unittests/CMakeLists.txt index 7a7b2a731d..b4a4ec423e 100644 --- a/unittests/CMakeLists.txt +++ b/unittests/CMakeLists.txt @@ -14,20 +14,22 @@ add_definitions(-D__EXPORT=) set(CMAKE_C_FLAGS "-std=c99") set(CMAKE_CXX_FLAGS "-std=c++11 -Wno-write-strings") +# check +add_custom_target(check COMMAND ${CMAKE_CTEST_COMMAND} --output-on-failure) + function(add_gtest) foreach(test_name ${ARGN}) target_link_libraries(${test_name} gtest_main) add_test(${test_name} ${test_name}) + add_dependencies(check ${test_name}) endforeach() endfunction() # add each test -# todo: add sbus2_test st24_test sf0x_test add_executable(autodeclination_test autodeclination_test.cpp ${CMAKE_SOURCE_DIR}/../src/lib/geo_lookup/geo_mag_declination.c) add_gtest(autodeclination_test) - # mixer_test add_executable(mixer_test mixer_test.cpp hrt.cpp ${CMAKE_SOURCE_DIR}/../src/modules/systemlib/mixer/mixer.cpp @@ -40,6 +42,8 @@ add_executable(mixer_test mixer_test.cpp hrt.cpp add_gtest(mixer_test) # conversion_test -add_executable(conversion_test conversion_test.cpp - ${CMAKE_SOURCE_DIR}/../src/systemcmds/tests/test_conv.cpp) +add_executable(conversion_test conversion_test.cpp ${CMAKE_SOURCE_DIR}/../src/systemcmds/tests/test_conv.cpp) add_gtest(conversion_test) + + +# todo: add sbus2_test st24_test sf0x_test diff --git a/unittests/Makefile b/unittests/Makefile index 0c50b38146..e1d2acacc0 100644 --- a/unittests/Makefile +++ b/unittests/Makefile @@ -66,7 +66,7 @@ st24_test: $(ST24_FILES) cmake_gtests: mkdir -p build - cd build && CC=gcc cmake .. && $(MAKE) && $(MAKE) test + cd build && CC=gcc cmake .. && $(MAKE) check unittests: clean sbus2_test sf0x_test st24_test cmake_gtests ./sbus2_test From efb3240bd27fa037f9f68d21208453ab6361deca Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 5 Jan 2015 18:38:28 -0500 Subject: [PATCH 414/652] gtest mixer_test match original --- unittests/mixer_test.cpp | 11 +++-------- 1 file changed, 3 insertions(+), 8 deletions(-) diff --git a/unittests/mixer_test.cpp b/unittests/mixer_test.cpp index 1c808e052a..29595c0671 100644 --- a/unittests/mixer_test.cpp +++ b/unittests/mixer_test.cpp @@ -5,12 +5,7 @@ #include "gtest/gtest.h" -TEST(MixerTest, IO_pass) { - char* args[] = {"mixer", "../../ROMFS/px4fmu_common/mixers/IO_pass.mix"}; - ASSERT_EQ(test_mixer(2, args), 0) << "IO_pass.mix failed"; +TEST(MixerTest, Mixer) { + char* args[] = {"empty", "../../ROMFS/px4fmu_common/mixers/IO_pass.mix", "../../ROMFS/px4fmu_common/mixers/FMU_quad_w.mix"}; + ASSERT_EQ(test_mixer(3, args), 0) << "IO_pass.mix failed"; } - -TEST(MixerTest, FMU_quad_w) { - char* args[] = {"mixer", "../../ROMFS/px4fmu_common/mixers/FMU_quad_w.mix"}; - ASSERT_EQ(test_mixer(2, args), 0) << "FMU_quad_w.mix failed"; -} From 4a09e6370a9d730af001a3c5f2736e69e8c170c2 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 5 Jan 2015 18:43:26 -0500 Subject: [PATCH 415/652] move sbus2_test to cmake --- unittests/CMakeLists.txt | 7 +++++-- unittests/Makefile | 14 +++----------- 2 files changed, 8 insertions(+), 13 deletions(-) diff --git a/unittests/CMakeLists.txt b/unittests/CMakeLists.txt index b4a4ec423e..644609a103 100644 --- a/unittests/CMakeLists.txt +++ b/unittests/CMakeLists.txt @@ -20,7 +20,7 @@ add_custom_target(check COMMAND ${CMAKE_CTEST_COMMAND} --output-on-failure) function(add_gtest) foreach(test_name ${ARGN}) target_link_libraries(${test_name} gtest_main) - add_test(${test_name} ${test_name}) + add_test(NAME ${test_name} COMMAND ${test_name} WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}) add_dependencies(check ${test_name}) endforeach() endfunction() @@ -45,5 +45,8 @@ add_gtest(mixer_test) add_executable(conversion_test conversion_test.cpp ${CMAKE_SOURCE_DIR}/../src/systemcmds/tests/test_conv.cpp) add_gtest(conversion_test) +# sbus2_test +add_executable(sbus2_test sbus2_test.cpp hrt.cpp) -# todo: add sbus2_test st24_test sf0x_test + +# todo: add st24_test sf0x_test diff --git a/unittests/Makefile b/unittests/Makefile index e1d2acacc0..1d6db6817c 100644 --- a/unittests/Makefile +++ b/unittests/Makefile @@ -40,11 +40,7 @@ gtest_main.a: gtest-all.o gtest_main.o $(AR) $(ARFLAGS) $@ $^ -all: sbus2_test st24_test sf0x_test - -SBUS2_FILES=../src/modules/px4iofirmware/sbus.c \ - hrt.cpp \ - sbus2_test.cpp +all: st24_test sf0x_test ST24_FILES=../src/lib/rc/st24.c \ hrt.cpp \ @@ -55,9 +51,6 @@ SF0X_FILES= \ sf0x_test.cpp \ ../src/drivers/sf0x/sf0x_parser.cpp -sbus2_test: $(SBUS2_FILES) - $(CC) -o sbus2_test $(SBUS2_FILES) $(CFLAGS) - sf0x_test: $(SF0X_FILES) $(CC) -o sf0x_test $(SF0X_FILES) $(CFLAGS) @@ -68,11 +61,10 @@ cmake_gtests: mkdir -p build cd build && CC=gcc cmake .. && $(MAKE) check -unittests: clean sbus2_test sf0x_test st24_test cmake_gtests - ./sbus2_test +unittests: clean sf0x_test st24_test cmake_gtests ./sf0x_test ./st24_test .PHONY: clean clean: - rm -rf gtest.a gtest_main.a *.o $(ODIR)/*.o *~ core $(INCDIR)/*~ sample_unittest sbus2_test st24_test sf0x_test build + rm -rf gtest.a gtest_main.a *.o $(ODIR)/*.o *~ core $(INCDIR)/*~ sample_unittest st24_test sf0x_test build From 322362d5e0adac332c1fab968831f0569460afd5 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 5 Jan 2015 18:50:26 -0500 Subject: [PATCH 416/652] move st24_test to cmake and run tests from original unittests working directory --- unittests/CMakeLists.txt | 11 +++++++++-- unittests/Makefile | 14 +++----------- unittests/mixer_test.cpp | 2 +- 3 files changed, 13 insertions(+), 14 deletions(-) diff --git a/unittests/CMakeLists.txt b/unittests/CMakeLists.txt index 644609a103..1657797e95 100644 --- a/unittests/CMakeLists.txt +++ b/unittests/CMakeLists.txt @@ -12,7 +12,7 @@ include_directories(${CMAKE_SOURCE_DIR}/../src/lib) add_definitions(-D__EXPORT=) set(CMAKE_C_FLAGS "-std=c99") -set(CMAKE_CXX_FLAGS "-std=c++11 -Wno-write-strings") +set(CMAKE_CXX_FLAGS "-std=c++11") # check add_custom_target(check COMMAND ${CMAKE_CTEST_COMMAND} --output-on-failure) @@ -46,7 +46,14 @@ add_executable(conversion_test conversion_test.cpp ${CMAKE_SOURCE_DIR}/../src/sy add_gtest(conversion_test) # sbus2_test +# TODO: move to gtest add_executable(sbus2_test sbus2_test.cpp hrt.cpp) +add_gtest(sbus2_test) + +# st24_test +# TODO: move to gtest +add_executable(st24_test st24_test.cpp hrt.cpp ${CMAKE_SOURCE_DIR}/../src/lib/rc/st24.c) +add_gtest(st24_test) -# todo: add st24_test sf0x_test +# todo: add sf0x_test diff --git a/unittests/Makefile b/unittests/Makefile index 1d6db6817c..eb15ebbea4 100644 --- a/unittests/Makefile +++ b/unittests/Makefile @@ -40,11 +40,7 @@ gtest_main.a: gtest-all.o gtest_main.o $(AR) $(ARFLAGS) $@ $^ -all: st24_test sf0x_test - -ST24_FILES=../src/lib/rc/st24.c \ - hrt.cpp \ - st24_test.cpp +all: sf0x_test SF0X_FILES= \ hrt.cpp \ @@ -54,17 +50,13 @@ SF0X_FILES= \ sf0x_test: $(SF0X_FILES) $(CC) -o sf0x_test $(SF0X_FILES) $(CFLAGS) -st24_test: $(ST24_FILES) - $(CC) -o st24_test $(ST24_FILES) $(CFLAGS) - cmake_gtests: mkdir -p build cd build && CC=gcc cmake .. && $(MAKE) check -unittests: clean sf0x_test st24_test cmake_gtests +unittests: clean sf0x_test cmake_gtests ./sf0x_test - ./st24_test .PHONY: clean clean: - rm -rf gtest.a gtest_main.a *.o $(ODIR)/*.o *~ core $(INCDIR)/*~ sample_unittest st24_test sf0x_test build + rm -rf gtest.a gtest_main.a *.o $(ODIR)/*.o *~ core $(INCDIR)/*~ sample_unittest sf0x_test build diff --git a/unittests/mixer_test.cpp b/unittests/mixer_test.cpp index 29595c0671..fd3ece7f87 100644 --- a/unittests/mixer_test.cpp +++ b/unittests/mixer_test.cpp @@ -6,6 +6,6 @@ TEST(MixerTest, Mixer) { - char* args[] = {"empty", "../../ROMFS/px4fmu_common/mixers/IO_pass.mix", "../../ROMFS/px4fmu_common/mixers/FMU_quad_w.mix"}; + char* args[] = {"empty", "../ROMFS/px4fmu_common/mixers/IO_pass.mix", "../ROMFS/px4fmu_common/mixers/FMU_quad_w.mix"}; ASSERT_EQ(test_mixer(3, args), 0) << "IO_pass.mix failed"; } From bd130ec397bea54d28643d6331384fd8a0438f0a Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 5 Jan 2015 18:55:38 -0500 Subject: [PATCH 417/652] move sf0x_test to cmake and remove unittests/Makefile --- Makefile | 2 +- unittests/CMakeLists.txt | 12 ++++---- unittests/Makefile | 62 ---------------------------------------- 3 files changed, 8 insertions(+), 68 deletions(-) delete mode 100644 unittests/Makefile diff --git a/Makefile b/Makefile index 905a60a3b6..64c468d31b 100644 --- a/Makefile +++ b/Makefile @@ -236,7 +236,7 @@ testbuild: # unit tests. .PHONY: tests tests: - $(Q) (cd $(PX4_BASE)/unittests && $(MAKE) unittests) + $(Q) (mkdir -p $(PX4_BASE)/unittests/build && cd $(PX4_BASE)/unittests/build && cmake .. && $(MAKE) unittests) # # Cleanup targets. 'clean' should remove all built products and force diff --git a/unittests/CMakeLists.txt b/unittests/CMakeLists.txt index 1657797e95..02e2a4d902 100644 --- a/unittests/CMakeLists.txt +++ b/unittests/CMakeLists.txt @@ -12,16 +12,16 @@ include_directories(${CMAKE_SOURCE_DIR}/../src/lib) add_definitions(-D__EXPORT=) set(CMAKE_C_FLAGS "-std=c99") -set(CMAKE_CXX_FLAGS "-std=c++11") +set(CMAKE_CXX_FLAGS "-std=c++11 -Wno-write-strings") # check -add_custom_target(check COMMAND ${CMAKE_CTEST_COMMAND} --output-on-failure) +add_custom_target(unittests COMMAND ${CMAKE_CTEST_COMMAND} --output-on-failure) function(add_gtest) foreach(test_name ${ARGN}) target_link_libraries(${test_name} gtest_main) add_test(NAME ${test_name} COMMAND ${test_name} WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}) - add_dependencies(check ${test_name}) + add_dependencies(unittests ${test_name}) endforeach() endfunction() @@ -55,5 +55,7 @@ add_gtest(sbus2_test) add_executable(st24_test st24_test.cpp hrt.cpp ${CMAKE_SOURCE_DIR}/../src/lib/rc/st24.c) add_gtest(st24_test) - -# todo: add sf0x_test +# sf0x_test +# TODO: move to gtest +add_executable(sf0x_test sf0x_test.cpp ${CMAKE_SOURCE_DIR}/../src/drivers/sf0x/sf0x_parser.cpp) +add_gtest(sf0x_test) diff --git a/unittests/Makefile b/unittests/Makefile deleted file mode 100644 index eb15ebbea4..0000000000 --- a/unittests/Makefile +++ /dev/null @@ -1,62 +0,0 @@ - -CC=g++ -CFLAGS=-I. -I../src/modules -I ../src/include -I../src/drivers \ - -I../src -I../src/lib -D__EXPORT="" -Dnullptr="0" -lm - -# 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: sf0x_test - -SF0X_FILES= \ - hrt.cpp \ - sf0x_test.cpp \ - ../src/drivers/sf0x/sf0x_parser.cpp - -sf0x_test: $(SF0X_FILES) - $(CC) -o sf0x_test $(SF0X_FILES) $(CFLAGS) - -cmake_gtests: - mkdir -p build - cd build && CC=gcc cmake .. && $(MAKE) check - -unittests: clean sf0x_test cmake_gtests - ./sf0x_test - -.PHONY: clean -clean: - rm -rf gtest.a gtest_main.a *.o $(ODIR)/*.o *~ core $(INCDIR)/*~ sample_unittest sf0x_test build From d31b0cd5dbdbe13b783176d3e68290a25d14fb62 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 5 Jan 2015 22:12:25 -0500 Subject: [PATCH 418/652] move sbus2 to gtest --- unittests/sbus2_test.cpp | 39 +++++++++++---------------------------- 1 file changed, 11 insertions(+), 28 deletions(-) diff --git a/unittests/sbus2_test.cpp b/unittests/sbus2_test.cpp index ba075f8b3b..ee4f3d1d68 100644 --- a/unittests/sbus2_test.cpp +++ b/unittests/sbus2_test.cpp @@ -1,33 +1,23 @@ - #include -#include #include -#include -#include +#include + +#include "../../src/systemcmds/tests/tests.h" #include #include -#include "../../src/systemcmds/tests/tests.h" +#include +#include -int main(int argc, char *argv[]) { - warnx("SBUS2 test started"); +#include "gtest/gtest.h" - char *filepath = 0; - - if (argc < 2) { - warnx("Using default input file"); - filepath = "testdata/sbus2_r7008SB.txt"; - } else { - filepath = argv[1]; - } - - warnx("loading data from: %s", filepath); +TEST(SBUS2Test, SBUS2) { + char *filepath = "testdata/sbus2_r7008SB.txt"; FILE *fp; - fp = fopen(filepath,"rt"); - if (!fp) - errx(1, "failed opening file"); + ASSERT_TRUE(fp); + warnx("loading data from: %s", filepath); float f; unsigned x; @@ -73,12 +63,5 @@ int main(int argc, char *argv[]) { //sbus_parse(now, frame, &partial_frame_count, rc_values, &num_values, &sbus_failsafe, &sbus_frame_drop, max_channels); } - if (ret == EOF) { - warnx("Test finished, reached end of file"); - ret = 0; - } else { - warnx("Test aborted, errno: %d", ret); - } - - return ret; + ASSERT_EQ(ret, EOF); } From 50e27f60add1e5cd1b61736e6fe82a51060dd5f5 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 6 Jan 2015 00:28:05 -0500 Subject: [PATCH 419/652] travis use gcc 4.8 for unittests --- .travis.yml | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/.travis.yml b/.travis.yml index b201d5130c..f75d034eb4 100644 --- a/.travis.yml +++ b/.travis.yml @@ -4,7 +4,10 @@ language: cpp before_script: - - sudo apt-get update -q + - sudo add-apt-repository --yes ppa:ubuntu-toolchain-r/test + - sudo apt-get update -qq + - if [ "$CXX" = "g++" ]; then sudo apt-get install -qq g++-4.8 gcc-4.8 libstdc++-4.8-dev; fi + - if [ "$CXX" = "g++" ]; then export CXX="g++-4.8" CC="gcc-4.8"; fi # Travis specific tools - sudo apt-get install s3cmd grep zip mailutils # General toolchain dependencies From cd11c4d81c976b76a0db262f43a6c09192cd0e6c Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 6 Jan 2015 10:26:17 +0100 Subject: [PATCH 420/652] make multi_tables script python3 compatible The script still works with python2, I also added a file ending --- .../mixer/{multi_tables => multi_tables.py} | 59 ++++++++++--------- 1 file changed, 31 insertions(+), 28 deletions(-) rename src/modules/systemlib/mixer/{multi_tables => multi_tables.py} (76%) diff --git a/src/modules/systemlib/mixer/multi_tables b/src/modules/systemlib/mixer/multi_tables.py similarity index 76% rename from src/modules/systemlib/mixer/multi_tables rename to src/modules/systemlib/mixer/multi_tables.py index a9edb357b5..21c0affd95 100755 --- a/src/modules/systemlib/mixer/multi_tables +++ b/src/modules/systemlib/mixer/multi_tables.py @@ -36,15 +36,18 @@ # Generate multirotor mixer scale tables compatible with the ArduCopter layout # +# for python2.7 compatibility +from __future__ import print_function + import math -print "/*" -print "* This file is automatically generated by multi_tables - do not edit." -print "*/" -print "" -print "#ifndef _MIXER_MULTI_TABLES" -print "#define _MIXER_MULTI_TABLES" -print "" +print("/*") +print("* This file is automatically generated by multi_tables - do not edit.") +print("*/") +print("") +print("#ifndef _MIXER_MULTI_TABLES") +print("#define _MIXER_MULTI_TABLES") +print("") def rcos(angleInRadians): return math.cos(math.radians(angleInRadians)) @@ -146,7 +149,7 @@ twin_engine = [ ] def variableName(variable): - for variableName, value in list(globals().iteritems()): + for variableName, value in list(globals().items()): if value is variable: return variableName @@ -154,44 +157,44 @@ tables = [quad_x, quad_plus, quad_v, quad_wide, hex_x, hex_plus, hex_cox, octa_x def printEnum(): - print "enum class MultirotorGeometry : MultirotorGeometryUnderlyingType {" + print("enum class MultirotorGeometry : MultirotorGeometryUnderlyingType {") for table in tables: - print "\t{},".format(variableName(table).upper()) + print("\t{},".format(variableName(table).upper())) - print "\n\tMAX_GEOMETRY" - print "}; // enum class MultirotorGeometry\n" + print("\n\tMAX_GEOMETRY") + print("}; // enum class MultirotorGeometry\n") def printScaleTables(): for table in tables: - print "const MultirotorMixer::Rotor _config_{}[] = {{".format(variableName(table)) + print("const MultirotorMixer::Rotor _config_{}[] = {{".format(variableName(table))) for (angle, yawScale) in table: rollScale = rcos(angle + 90) pitchScale = rcos(angle) - print "\t{{ {:9f}, {:9f}, {:9f} }},".format(rollScale, pitchScale, yawScale) - print "};\n" - + print("\t{{ {:9f}, {:9f}, {:9f} }},".format(rollScale, pitchScale, yawScale)) + print("};\n") + def printScaleTablesIndex(): - print "const MultirotorMixer::Rotor *_config_index[] = {" + print("const MultirotorMixer::Rotor *_config_index[] = {") for table in tables: - print "\t&_config_{}[0],".format(variableName(table)) - print "};\n" + print("\t&_config_{}[0],".format(variableName(table))) + print("};\n") def printScaleTablesCounts(): - print "const unsigned _config_rotor_count[] = {" + print("const unsigned _config_rotor_count[] = {") for table in tables: - print "\t{}, /* {} */".format(len(table), variableName(table)) - print "};\n" - - + print("\t{}, /* {} */".format(len(table), variableName(table))) + print("};\n") + + printEnum() -print "namespace {" +print("namespace {") printScaleTables() printScaleTablesIndex() printScaleTablesCounts() -print "} // anonymous namespace\n" -print "#endif /* _MIXER_MULTI_TABLES */" -print "" +print("} // anonymous namespace\n") +print("#endif /* _MIXER_MULTI_TABLES */") +print("") From 0d571458f5880c0fa92d20a4df1f0204277c43a0 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 6 Jan 2015 10:27:40 +0100 Subject: [PATCH 421/652] improve multi_tables makefile --- src/modules/systemlib/mixer/multi_tables.mk | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/src/modules/systemlib/mixer/multi_tables.mk b/src/modules/systemlib/mixer/multi_tables.mk index 6429961cc8..5a1a0fa72e 100644 --- a/src/modules/systemlib/mixer/multi_tables.mk +++ b/src/modules/systemlib/mixer/multi_tables.mk @@ -33,11 +33,12 @@ SELF_DIR := $(dir $(lastword $(MAKEFILE_LIST))) +MULTI_TABLES := $(SELF_DIR)multi_tables.py # Add explicit dependency, as implicit one doesn't work often. $(SELF_DIR)mixer_multirotor.cpp : $(SELF_DIR)mixer_multirotor.generated.h -$(SELF_DIR)mixer_multirotor.generated.h : $(SELF_DIR)multi_tables - $(SELF_DIR)multi_tables > $(SELF_DIR)mixer_multirotor.generated.h - $(SELF_DIR)multi_tables - \ No newline at end of file +$(SELF_DIR)mixer_multirotor.generated.h : $(MULTI_TABLES) + $(Q) $(PYTHON) $(MULTI_TABLES) > $(SELF_DIR)mixer_multirotor.generated.h + $(Q) $(PYTHON) $(MULTI_TABLES) + From a42febc993ae4492e0f43002a5f30f02a0e02fa7 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 6 Jan 2015 10:29:36 +0100 Subject: [PATCH 422/652] remove output of multi_tables script --- src/modules/systemlib/mixer/multi_tables.mk | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/modules/systemlib/mixer/multi_tables.mk b/src/modules/systemlib/mixer/multi_tables.mk index 5a1a0fa72e..c537c83a47 100644 --- a/src/modules/systemlib/mixer/multi_tables.mk +++ b/src/modules/systemlib/mixer/multi_tables.mk @@ -40,5 +40,3 @@ $(SELF_DIR)mixer_multirotor.cpp : $(SELF_DIR)mixer_multirotor.generated.h $(SELF_DIR)mixer_multirotor.generated.h : $(MULTI_TABLES) $(Q) $(PYTHON) $(MULTI_TABLES) > $(SELF_DIR)mixer_multirotor.generated.h - $(Q) $(PYTHON) $(MULTI_TABLES) - From 3606f5370af4d439afc47d1585a76e0b66e34336 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 6 Jan 2015 08:46:58 -0500 Subject: [PATCH 423/652] cmake generate mixer_multirotor.generated.h for unittests --- unittests/CMakeLists.txt | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/unittests/CMakeLists.txt b/unittests/CMakeLists.txt index 05d5fb214e..39fc514374 100644 --- a/unittests/CMakeLists.txt +++ b/unittests/CMakeLists.txt @@ -12,6 +12,7 @@ elseif(COMPILER_SUPPORTS_CXX0X) else() message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.") endif() +set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -std=c99") set(GTEST_DIR gtest) add_subdirectory(${GTEST_DIR}) @@ -40,11 +41,14 @@ add_executable(autodeclination_test autodeclination_test.cpp ${CMAKE_SOURCE_DIR} add_gtest(autodeclination_test) # mixer_test +add_custom_command(OUTPUT ${CMAKE_SOURCE_DIR}/../src/modules/systemlib/mixer/mixer_multirotor.generated.h + COMMAND ${CMAKE_SOURCE_DIR}/../src/modules/systemlib/mixer/multi_tables.py > ${CMAKE_SOURCE_DIR}/../src/modules/systemlib/mixer/mixer_multirotor.generated.h) add_executable(mixer_test mixer_test.cpp hrt.cpp ${CMAKE_SOURCE_DIR}/../src/modules/systemlib/mixer/mixer.cpp ${CMAKE_SOURCE_DIR}/../src/modules/systemlib/mixer/mixer_group.cpp ${CMAKE_SOURCE_DIR}/../src/modules/systemlib/mixer/mixer_load.c ${CMAKE_SOURCE_DIR}/../src/modules/systemlib/mixer/mixer_multirotor.cpp + ${CMAKE_SOURCE_DIR}/../src/modules/systemlib/mixer/mixer_multirotor.generated.h ${CMAKE_SOURCE_DIR}/../src/modules/systemlib/mixer/mixer_simple.cpp ${CMAKE_SOURCE_DIR}/../src/modules/systemlib/pwm_limit/pwm_limit.c ${CMAKE_SOURCE_DIR}/../src/systemcmds/tests/test_mixer.cpp) From 620df8ba0bfdc5f8caf4041e52baa4b83b141dfa Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 6 Jan 2015 08:50:45 -0500 Subject: [PATCH 424/652] cmake unittests add PX_SRC variable to cleanup long paths --- unittests/CMakeLists.txt | 36 +++++++++++++++++++----------------- 1 file changed, 19 insertions(+), 17 deletions(-) diff --git a/unittests/CMakeLists.txt b/unittests/CMakeLists.txt index 39fc514374..c2990c59f2 100644 --- a/unittests/CMakeLists.txt +++ b/unittests/CMakeLists.txt @@ -17,10 +17,12 @@ set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -std=c99") set(GTEST_DIR gtest) add_subdirectory(${GTEST_DIR}) include_directories(${GTEST_DIR}/include) + +set(PX_SRC ${CMAKE_SOURCE_DIR}/../src) include_directories(${CMAKE_SOURCE_DIR}) -include_directories(${CMAKE_SOURCE_DIR}/../src) -include_directories(${CMAKE_SOURCE_DIR}/../src/modules) -include_directories(${CMAKE_SOURCE_DIR}/../src/lib) +include_directories(${PX_SRC}) +include_directories(${PX_SRC}/modules) +include_directories(${PX_SRC}/lib) add_definitions(-D__EXPORT=) @@ -37,25 +39,25 @@ endfunction() # add each test -add_executable(autodeclination_test autodeclination_test.cpp ${CMAKE_SOURCE_DIR}/../src/lib/geo_lookup/geo_mag_declination.c) +add_executable(autodeclination_test autodeclination_test.cpp ${PX_SRC}/lib/geo_lookup/geo_mag_declination.c) add_gtest(autodeclination_test) # mixer_test -add_custom_command(OUTPUT ${CMAKE_SOURCE_DIR}/../src/modules/systemlib/mixer/mixer_multirotor.generated.h - COMMAND ${CMAKE_SOURCE_DIR}/../src/modules/systemlib/mixer/multi_tables.py > ${CMAKE_SOURCE_DIR}/../src/modules/systemlib/mixer/mixer_multirotor.generated.h) +add_custom_command(OUTPUT ${PX_SRC}/modules/systemlib/mixer/mixer_multirotor.generated.h + COMMAND ${PX_SRC}/modules/systemlib/mixer/multi_tables.py > ${PX_SRC}/modules/systemlib/mixer/mixer_multirotor.generated.h) add_executable(mixer_test mixer_test.cpp hrt.cpp - ${CMAKE_SOURCE_DIR}/../src/modules/systemlib/mixer/mixer.cpp - ${CMAKE_SOURCE_DIR}/../src/modules/systemlib/mixer/mixer_group.cpp - ${CMAKE_SOURCE_DIR}/../src/modules/systemlib/mixer/mixer_load.c - ${CMAKE_SOURCE_DIR}/../src/modules/systemlib/mixer/mixer_multirotor.cpp - ${CMAKE_SOURCE_DIR}/../src/modules/systemlib/mixer/mixer_multirotor.generated.h - ${CMAKE_SOURCE_DIR}/../src/modules/systemlib/mixer/mixer_simple.cpp - ${CMAKE_SOURCE_DIR}/../src/modules/systemlib/pwm_limit/pwm_limit.c - ${CMAKE_SOURCE_DIR}/../src/systemcmds/tests/test_mixer.cpp) + ${PX_SRC}/modules/systemlib/mixer/mixer.cpp + ${PX_SRC}/modules/systemlib/mixer/mixer_group.cpp + ${PX_SRC}/modules/systemlib/mixer/mixer_load.c + ${PX_SRC}/modules/systemlib/mixer/mixer_multirotor.cpp + ${PX_SRC}/modules/systemlib/mixer/mixer_multirotor.generated.h + ${PX_SRC}/modules/systemlib/mixer/mixer_simple.cpp + ${PX_SRC}/modules/systemlib/pwm_limit/pwm_limit.c + ${PX_SRC}/systemcmds/tests/test_mixer.cpp) add_gtest(mixer_test) # conversion_test -add_executable(conversion_test conversion_test.cpp ${CMAKE_SOURCE_DIR}/../src/systemcmds/tests/test_conv.cpp) +add_executable(conversion_test conversion_test.cpp ${PX_SRC}/systemcmds/tests/test_conv.cpp) add_gtest(conversion_test) # sbus2_test @@ -65,10 +67,10 @@ add_gtest(sbus2_test) # st24_test # TODO: move to gtest -add_executable(st24_test st24_test.cpp hrt.cpp ${CMAKE_SOURCE_DIR}/../src/lib/rc/st24.c) +add_executable(st24_test st24_test.cpp hrt.cpp ${PX_SRC}/lib/rc/st24.c) add_gtest(st24_test) # sf0x_test # TODO: move to gtest -add_executable(sf0x_test sf0x_test.cpp ${CMAKE_SOURCE_DIR}/../src/drivers/sf0x/sf0x_parser.cpp) +add_executable(sf0x_test sf0x_test.cpp ${PX_SRC}/drivers/sf0x/sf0x_parser.cpp) add_gtest(sf0x_test) From c6722fce0be914e2c678437eee6531e087e5d89a Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 3 Jan 2015 18:24:26 +0100 Subject: [PATCH 425/652] fw att control: cleanup, create base class for ECL Adding a new base class to remove a lot of boilerplate code, no functionality changes --- src/lib/ecl/attitude_fw/ecl_controller.cpp | 128 ++++++++++++++++++ src/lib/ecl/attitude_fw/ecl_controller.h | 119 ++++++++++++++++ .../ecl/attitude_fw/ecl_pitch_controller.cpp | 80 +++++------ .../ecl/attitude_fw/ecl_pitch_controller.h | 67 ++------- .../ecl/attitude_fw/ecl_roll_controller.cpp | 68 ++++------ src/lib/ecl/attitude_fw/ecl_roll_controller.h | 69 +--------- .../ecl/attitude_fw/ecl_yaw_controller.cpp | 84 ++++++------ src/lib/ecl/attitude_fw/ecl_yaw_controller.h | 68 ++-------- src/lib/ecl/module.mk | 3 +- .../fw_att_control/fw_att_control_main.cpp | 47 ++++--- 10 files changed, 403 insertions(+), 330 deletions(-) create mode 100644 src/lib/ecl/attitude_fw/ecl_controller.cpp create mode 100644 src/lib/ecl/attitude_fw/ecl_controller.h diff --git a/src/lib/ecl/attitude_fw/ecl_controller.cpp b/src/lib/ecl/attitude_fw/ecl_controller.cpp new file mode 100644 index 0000000000..46140fbfd9 --- /dev/null +++ b/src/lib/ecl/attitude_fw/ecl_controller.cpp @@ -0,0 +1,128 @@ +/**************************************************************************** + * + * Copyright (c) 2013 Estimation and Control Library (ECL). 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 ECL 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 ecl_controller.cpp + * Definition of base class for other controllers + * + * @author Lorenz Meier + * @author Thomas Gubler + * + * Acknowledgements: + * + * The control design is based on a design + * by Paul Riseborough and Andrew Tridgell, 2013, + * which in turn is based on initial work of + * Jonathan Challinger, 2012. + */ + +#include "ecl_controller.h" + +#include + +ECL_Controller::ECL_Controller(const char *name) : + _last_run(0), + _tc(0.1f), + _k_p(0.0f), + _k_i(0.0f), + _k_ff(0.0f), + _integrator_max(0.0f), + _max_rate(0.0f), + _last_output(0.0f), + _integrator(0.0f), + _rate_error(0.0f), + _rate_setpoint(0.0f), + _bodyrate_setpoint(0.0f), + _perf_name() +{ + /* Init performance counter */ + snprintf(_perf_name, sizeof(_perf_name), "fw att control %s nonfinite input", name); + _nonfinite_input_perf = perf_alloc(PC_COUNT, _perf_name); +} + +ECL_Controller::~ECL_Controller() +{ + perf_free(_nonfinite_input_perf); +} + +void ECL_Controller::reset_integrator() +{ + _integrator = 0.0f; +} + +void ECL_Controller::set_time_constant(float time_constant) +{ + if (time_constant > 0.1f && time_constant < 3.0f) { + _tc = time_constant; + } +} + +void ECL_Controller::set_k_p(float k_p) +{ + _k_p = k_p; +} + +void ECL_Controller::set_k_i(float k_i) +{ + _k_i = k_i; +} + +void ECL_Controller::set_k_ff(float k_ff) +{ + _k_ff = k_ff; +} + +void ECL_Controller::set_integrator_max(float max) +{ + _integrator_max = max; +} + +void ECL_Controller::set_max_rate(float max_rate) +{ + _max_rate = max_rate; +} + +float ECL_Controller::get_rate_error() +{ + return _rate_error; +} + +float ECL_Controller::get_desired_rate() +{ + return _rate_setpoint; +} + +float ECL_Controller::get_desired_bodyrate() +{ + return _bodyrate_setpoint; +} diff --git a/src/lib/ecl/attitude_fw/ecl_controller.h b/src/lib/ecl/attitude_fw/ecl_controller.h new file mode 100644 index 0000000000..9880d1c174 --- /dev/null +++ b/src/lib/ecl/attitude_fw/ecl_controller.h @@ -0,0 +1,119 @@ +/**************************************************************************** + * + * Copyright (c) 2013 Estimation and Control Library (ECL). 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 ECL 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 ecl_controller.h + * Definition of base class for other controllers + * + * @author Lorenz Meier + * @author Thomas Gubler + * + * Acknowledgements: + * + * The control design is based on a design + * by Paul Riseborough and Andrew Tridgell, 2013, + * which in turn is based on initial work of + * Jonathan Challinger, 2012. + */ + +#pragma once + +#include +#include +#include + +struct ECL_ControlData { + float roll; + float pitch; + float yaw; + float roll_rate; + float pitch_rate; + float yaw_rate; + float speed_body_u; + float speed_body_v; + float speed_body_w; + float roll_setpoint; + float pitch_setpoint; + float yaw_setpoint; + float roll_rate_setpoint; + float pitch_rate_setpoint; + float yaw_rate_setpoint; + float airspeed_min; + float airspeed_max; + float airspeed; + float scaler; + bool lock_integrator; +}; + +class __EXPORT ECL_Controller +{ +public: + ECL_Controller(const char *name); + + ~ECL_Controller(); + + virtual float control_attitude(const struct ECL_ControlData &ctl_data) = 0; + virtual float control_bodyrate(const struct ECL_ControlData &ctl_data) = 0; + + /* Setters */ + void set_time_constant(float time_constant); + void set_k_p(float k_p); + void set_k_i(float k_i); + void set_k_ff(float k_ff); + void set_integrator_max(float max); + void set_max_rate(float max_rate); + + /* Getters */ + float get_rate_error(); + float get_desired_rate(); + float get_desired_bodyrate(); + + void reset_integrator(); + +protected: + uint64_t _last_run; + float _tc; + float _k_p; + float _k_i; + float _k_ff; + float _integrator_max; + float _max_rate; + float _last_output; + float _integrator; + float _rate_error; + float _rate_setpoint; + float _bodyrate_setpoint; + perf_counter_t _nonfinite_input_perf; + static const uint8_t _perf_name_max = 40; + char _perf_name[_perf_name_max]; +}; diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp index 926a8db2ac..add884146c 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp @@ -48,33 +48,24 @@ #include ECL_PitchController::ECL_PitchController() : - _last_run(0), - _tc(0.1f), - _k_p(0.0f), - _k_i(0.0f), - _k_ff(0.0f), - _integrator_max(0.0f), - _max_rate_pos(0.0f), + ECL_Controller("pitch"), _max_rate_neg(0.0f), - _roll_ff(0.0f), - _last_output(0.0f), - _integrator(0.0f), - _rate_error(0.0f), - _rate_setpoint(0.0f), - _bodyrate_setpoint(0.0f), - _nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control pitch nonfinite input")) + _roll_ff(0.0f) { } ECL_PitchController::~ECL_PitchController() { - perf_free(_nonfinite_input_perf); } -float ECL_PitchController::control_attitude(float pitch_setpoint, float roll, float pitch, float airspeed) +float ECL_PitchController::control_attitude(const struct ECL_ControlData &ctl_data) { + float roll = ctl_data.roll; /* Do not calculate control signal with bad inputs */ - if (!(isfinite(pitch_setpoint) && isfinite(roll) && isfinite(pitch) && isfinite(airspeed))) { + if (!(isfinite(ctl_data.pitch_setpoint) && + isfinite(roll) && + isfinite(ctl_data.pitch) && + isfinite(ctl_data.airspeed))) { perf_count(_nonfinite_input_perf); warnx("not controlling pitch"); return _rate_setpoint; @@ -102,13 +93,13 @@ float ECL_PitchController::control_attitude(float pitch_setpoint, float roll, fl /* calculate the offset in the rate resulting from rolling */ //xxx needs explanation and conversion to body angular rates or should be removed - float turn_offset = fabsf((CONSTANTS_ONE_G / airspeed) * + float turn_offset = fabsf((CONSTANTS_ONE_G / ctl_data.airspeed) * tanf(roll) * sinf(roll)) * _roll_ff; if (inverted) turn_offset = -turn_offset; /* Calculate the error */ - float pitch_error = pitch_setpoint - pitch; + float pitch_error = ctl_data.pitch_setpoint - ctl_data.pitch; /* Apply P controller: rate setpoint from current error and time constant */ _rate_setpoint = pitch_error / _tc; @@ -117,9 +108,9 @@ float ECL_PitchController::control_attitude(float pitch_setpoint, float roll, fl _rate_setpoint += turn_offset; /* limit the rate */ //XXX: move to body angluar rates - if (_max_rate_pos > 0.01f && _max_rate_neg > 0.01f) { + if (_max_rate > 0.01f && _max_rate_neg > 0.01f) { if (_rate_setpoint > 0.0f) { - _rate_setpoint = (_rate_setpoint > _max_rate_pos) ? _max_rate_pos : _rate_setpoint; + _rate_setpoint = (_rate_setpoint > _max_rate) ? _max_rate : _rate_setpoint; } else { _rate_setpoint = (_rate_setpoint < -_max_rate_neg) ? -_max_rate_neg : _rate_setpoint; } @@ -129,15 +120,17 @@ float ECL_PitchController::control_attitude(float pitch_setpoint, float roll, fl return _rate_setpoint; } -float ECL_PitchController::control_bodyrate(float roll, float pitch, - float pitch_rate, float yaw_rate, - float yaw_rate_setpoint, - float airspeed_min, float airspeed_max, float airspeed, float scaler, bool lock_integrator) +float ECL_PitchController::control_bodyrate(const struct ECL_ControlData &ctl_data) { /* Do not calculate control signal with bad inputs */ - if (!(isfinite(roll) && isfinite(pitch) && isfinite(pitch_rate) && isfinite(yaw_rate) && - isfinite(yaw_rate_setpoint) && isfinite(airspeed_min) && - isfinite(airspeed_max) && isfinite(scaler))) { + if (!(isfinite(ctl_data.roll) && + isfinite(ctl_data.pitch) && + isfinite(ctl_data.pitch_rate) && + isfinite(ctl_data.yaw_rate) && + isfinite(ctl_data.yaw_rate_setpoint) && + isfinite(ctl_data.airspeed_min) && + isfinite(ctl_data.airspeed_max) && + isfinite(ctl_data.scaler))) { perf_count(_nonfinite_input_perf); return math::constrain(_last_output, -1.0f, 1.0f); } @@ -148,28 +141,32 @@ float ECL_PitchController::control_bodyrate(float roll, float pitch, float dt = (float)dt_micros * 1e-6f; /* lock integral for long intervals */ + bool lock_integrator = ctl_data.lock_integrator; if (dt_micros > 500000) lock_integrator = true; /* input conditioning */ + float airspeed = ctl_data.airspeed; if (!isfinite(airspeed)) { /* airspeed is NaN, +- INF or not available, pick center of band */ - airspeed = 0.5f * (airspeed_min + airspeed_max); - } else if (airspeed < airspeed_min) { - airspeed = airspeed_min; + airspeed = 0.5f * (ctl_data.airspeed_min + ctl_data.airspeed_max); + } else if (airspeed < ctl_data.airspeed_min) { + airspeed = ctl_data.airspeed_min; } - /* Transform setpoint to body angular rates */ - _bodyrate_setpoint = cosf(roll) * _rate_setpoint + cosf(pitch) * sinf(roll) * yaw_rate_setpoint; //jacobian + /* Transform setpoint to body angular rates (jacobian) */ + _bodyrate_setpoint = cosf(ctl_data.roll) * _rate_setpoint + + cosf(ctl_data.pitch) * sinf(ctl_data.roll) * ctl_data.yaw_rate_setpoint; - /* Transform estimation to body angular rates */ - float pitch_bodyrate = cosf(roll) * pitch_rate + cosf(pitch) * sinf(roll) * yaw_rate; //jacobian + /* Transform estimation to body angular rates (jacobian) */ + float pitch_bodyrate = cosf(ctl_data.roll) * ctl_data.pitch_rate + + cosf(ctl_data.pitch) * sinf(ctl_data.roll) * ctl_data.yaw_rate; _rate_error = _bodyrate_setpoint - pitch_bodyrate; - if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * airspeed_min) { + if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * ctl_data.airspeed_min) { - float id = _rate_error * dt * scaler; + float id = _rate_error * dt * ctl_data.scaler; /* * anti-windup: do not allow integrator to increase if actuator is at limit @@ -190,15 +187,10 @@ float ECL_PitchController::control_bodyrate(float roll, float pitch, float integrator_constrained = math::constrain(_integrator * _k_i, -_integrator_max, _integrator_max); /* Apply PI rate controller and store non-limited output */ - _last_output = _bodyrate_setpoint * _k_ff * scaler + - _rate_error * _k_p * scaler * scaler + _last_output = _bodyrate_setpoint * _k_ff * ctl_data.scaler + + _rate_error * _k_p * ctl_data.scaler * ctl_data.scaler + integrator_constrained; //scaler is proportional to 1/airspeed // warnx("pitch: _integrator: %.4f, _integrator_max: %.4f, airspeed %.4f, _k_i %.4f, _k_p: %.4f", (double)_integrator, (double)_integrator_max, (double)airspeed, (double)_k_i, (double)_k_p); // warnx("roll: _last_output %.4f", (double)_last_output); return math::constrain(_last_output, -1.0f, 1.0f); } - -void ECL_PitchController::reset_integrator() -{ - _integrator = 0.0f; -} diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.h b/src/lib/ecl/attitude_fw/ecl_pitch_controller.h index 39b9f9d03c..47b5605e95 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.h @@ -51,46 +51,23 @@ #include #include -#include -class __EXPORT ECL_PitchController //XXX: create controller superclass +#include "ecl_controller.h" + +class __EXPORT ECL_PitchController : + public ECL_Controller { public: ECL_PitchController(); ~ECL_PitchController(); - float control_attitude(float pitch_setpoint, float roll, float pitch, float airspeed); - - - float control_bodyrate(float roll, float pitch, - float pitch_rate, float yaw_rate, - float yaw_rate_setpoint, - float airspeed_min = 0.0f, float airspeed_max = 0.0f, float airspeed = (0.0f / 0.0f), float scaler = 1.0f, bool lock_integrator = false); - - void reset_integrator(); - - void set_time_constant(float time_constant) { - _tc = time_constant; - } - void set_k_p(float k_p) { - _k_p = k_p; - } - - void set_k_i(float k_i) { - _k_i = k_i; - } - - void set_k_ff(float k_ff) { - _k_ff = k_ff; - } - - void set_integrator_max(float max) { - _integrator_max = max; - } + float control_attitude(const struct ECL_ControlData &ctl_data); + float control_bodyrate(const struct ECL_ControlData &ctl_data); + /* Additional Setters */ void set_max_rate_pos(float max_rate_pos) { - _max_rate_pos = max_rate_pos; + _max_rate = max_rate_pos; } void set_max_rate_neg(float max_rate_neg) { @@ -101,35 +78,9 @@ public: _roll_ff = roll_ff; } - float get_rate_error() { - return _rate_error; - } - - float get_desired_rate() { - return _rate_setpoint; - } - - float get_desired_bodyrate() { - return _bodyrate_setpoint; - } - -private: - - uint64_t _last_run; - float _tc; - float _k_p; - float _k_i; - float _k_ff; - float _integrator_max; - float _max_rate_pos; +protected: float _max_rate_neg; float _roll_ff; - float _last_output; - float _integrator; - float _rate_error; - float _rate_setpoint; - float _bodyrate_setpoint; - perf_counter_t _nonfinite_input_perf; }; #endif // ECL_PITCH_CONTROLLER_H diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp index 94bd26f03d..0606c87cb9 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp @@ -48,37 +48,24 @@ #include ECL_RollController::ECL_RollController() : - _last_run(0), - _tc(0.1f), - _k_p(0.0f), - _k_i(0.0f), - _k_ff(0.0f), - _integrator_max(0.0f), - _max_rate(0.0f), - _last_output(0.0f), - _integrator(0.0f), - _rate_error(0.0f), - _rate_setpoint(0.0f), - _bodyrate_setpoint(0.0f), - _nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control roll nonfinite input")) + ECL_Controller("roll") { } ECL_RollController::~ECL_RollController() { - perf_free(_nonfinite_input_perf); } -float ECL_RollController::control_attitude(float roll_setpoint, float roll) +float ECL_RollController::control_attitude(const struct ECL_ControlData &ctl_data) { /* Do not calculate control signal with bad inputs */ - if (!(isfinite(roll_setpoint) && isfinite(roll))) { + if (!(isfinite(ctl_data.roll_setpoint) && isfinite(ctl_data.roll))) { perf_count(_nonfinite_input_perf); return _rate_setpoint; } /* Calculate error */ - float roll_error = roll_setpoint - roll; + float roll_error = ctl_data.roll_setpoint - ctl_data.roll; /* Apply P controller */ _rate_setpoint = roll_error / _tc; @@ -92,15 +79,16 @@ float ECL_RollController::control_attitude(float roll_setpoint, float roll) return _rate_setpoint; } -float ECL_RollController::control_bodyrate(float pitch, - float roll_rate, float yaw_rate, - float yaw_rate_setpoint, - float airspeed_min, float airspeed_max, float airspeed, float scaler, bool lock_integrator) +float ECL_RollController::control_bodyrate(const struct ECL_ControlData &ctl_data) { /* Do not calculate control signal with bad inputs */ - if (!(isfinite(pitch) && isfinite(roll_rate) && isfinite(yaw_rate) && isfinite(yaw_rate_setpoint) && - isfinite(airspeed_min) && isfinite(airspeed_max) && - isfinite(scaler))) { + if (!(isfinite(ctl_data.pitch) && + isfinite(ctl_data.roll_rate) && + isfinite(ctl_data.yaw_rate) && + isfinite(ctl_data.yaw_rate_setpoint) && + isfinite(ctl_data.airspeed_min) && + isfinite(ctl_data.airspeed_max) && + isfinite(ctl_data.scaler))) { perf_count(_nonfinite_input_perf); return math::constrain(_last_output, -1.0f, 1.0f); } @@ -111,31 +99,31 @@ float ECL_RollController::control_bodyrate(float pitch, float dt = (float)dt_micros * 1e-6f; /* lock integral for long intervals */ + bool lock_integrator = ctl_data.lock_integrator; if (dt_micros > 500000) lock_integrator = true; /* input conditioning */ -// warnx("airspeed pre %.4f", (double)airspeed); + float airspeed = ctl_data.airspeed; if (!isfinite(airspeed)) { /* airspeed is NaN, +- INF or not available, pick center of band */ - airspeed = 0.5f * (airspeed_min + airspeed_max); - } else if (airspeed < airspeed_min) { - airspeed = airspeed_min; + airspeed = 0.5f * (ctl_data.airspeed_min + ctl_data.airspeed_max); + } else if (airspeed < ctl_data.airspeed_min) { + airspeed = ctl_data.airspeed_min; } + /* Transform setpoint to body angular rates (jacobian) */ + _bodyrate_setpoint = _rate_setpoint - sinf(ctl_data.pitch) * ctl_data.yaw_rate_setpoint; - /* Transform setpoint to body angular rates */ - _bodyrate_setpoint = _rate_setpoint - sinf(pitch) * yaw_rate_setpoint; //jacobian - - /* Transform estimation to body angular rates */ - float roll_bodyrate = roll_rate - sinf(pitch) * yaw_rate; //jacobian + /* Transform estimation to body angular rates (jacobian) */ + float roll_bodyrate = ctl_data.roll_rate - sinf(ctl_data.pitch) * ctl_data.yaw_rate; /* Calculate body angular rate error */ _rate_error = _bodyrate_setpoint - roll_bodyrate; //body angular rate error - if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * airspeed_min) { + if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * ctl_data.airspeed_min) { - float id = _rate_error * dt * scaler; + float id = _rate_error * dt * ctl_data.scaler; /* * anti-windup: do not allow integrator to increase if actuator is at limit @@ -157,15 +145,9 @@ float ECL_RollController::control_bodyrate(float pitch, //warnx("roll: _integrator: %.4f, _integrator_max: %.4f", (double)_integrator, (double)_integrator_max); /* Apply PI rate controller and store non-limited output */ - _last_output = _bodyrate_setpoint * _k_ff * scaler + - _rate_error * _k_p * scaler * scaler + _last_output = _bodyrate_setpoint * _k_ff * ctl_data.scaler + + _rate_error * _k_p * ctl_data.scaler * ctl_data.scaler + integrator_constrained; //scaler is proportional to 1/airspeed return math::constrain(_last_output, -1.0f, 1.0f); } - -void ECL_RollController::reset_integrator() -{ - _integrator = 0.0f; -} - diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.h b/src/lib/ecl/attitude_fw/ecl_roll_controller.h index 0799dbe03b..6d07bab8ab 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.h @@ -51,76 +51,19 @@ #include #include -#include -class __EXPORT ECL_RollController //XXX: create controller superclass +#include "ecl_controller.h" + +class __EXPORT ECL_RollController : + public ECL_Controller { public: ECL_RollController(); ~ECL_RollController(); - float control_attitude(float roll_setpoint, float roll); - - float control_bodyrate(float pitch, - float roll_rate, float yaw_rate, - float yaw_rate_setpoint, - float airspeed_min = 0.0f, float airspeed_max = 0.0f, float airspeed = (0.0f / 0.0f), float scaler = 1.0f, bool lock_integrator = false); - - void reset_integrator(); - - void set_time_constant(float time_constant) { - if (time_constant > 0.1f && time_constant < 3.0f) { - _tc = time_constant; - } - } - - void set_k_p(float k_p) { - _k_p = k_p; - } - - void set_k_i(float k_i) { - _k_i = k_i; - } - - void set_k_ff(float k_ff) { - _k_ff = k_ff; - } - - void set_integrator_max(float max) { - _integrator_max = max; - } - - void set_max_rate(float max_rate) { - _max_rate = max_rate; - } - - float get_rate_error() { - return _rate_error; - } - - float get_desired_rate() { - return _rate_setpoint; - } - - float get_desired_bodyrate() { - return _bodyrate_setpoint; - } - -private: - uint64_t _last_run; - float _tc; - float _k_p; - float _k_i; - float _k_ff; - float _integrator_max; - float _max_rate; - float _last_output; - float _integrator; - float _rate_error; - float _rate_setpoint; - float _bodyrate_setpoint; - perf_counter_t _nonfinite_input_perf; + float control_attitude(const struct ECL_ControlData &ctl_data); + float control_bodyrate(const struct ECL_ControlData &ctl_data); }; #endif // ECL_ROLL_CONTROLLER_H diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp index fe03b80651..82ba2c6c7e 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp @@ -47,45 +47,43 @@ #include ECL_YawController::ECL_YawController() : - _last_run(0), - _k_p(0.0f), - _k_i(0.0f), - _k_ff(0.0f), - _integrator_max(0.0f), - _max_rate(0.0f), - _last_output(0.0f), - _integrator(0.0f), - _rate_error(0.0f), - _rate_setpoint(0.0f), - _bodyrate_setpoint(0.0f), - _coordinated_min_speed(1.0f), - _nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control yaw nonfinite input")) + ECL_Controller("yaw"), + _coordinated_min_speed(1.0f) { } ECL_YawController::~ECL_YawController() { - perf_free(_nonfinite_input_perf); } -float ECL_YawController::control_attitude(float roll, float pitch, - float speed_body_u, float speed_body_v, float speed_body_w, - float roll_rate_setpoint, float pitch_rate_setpoint) +float ECL_YawController::control_attitude(const struct ECL_ControlData &ctl_data) { /* Do not calculate control signal with bad inputs */ - if (!(isfinite(roll) && isfinite(pitch) && isfinite(speed_body_u) && isfinite(speed_body_v) && - isfinite(speed_body_w) && isfinite(roll_rate_setpoint) && - isfinite(pitch_rate_setpoint))) { + if (!(isfinite(ctl_data.roll) && + isfinite(ctl_data.pitch) && + isfinite(ctl_data.speed_body_u) && + isfinite(ctl_data.speed_body_v) && + isfinite(ctl_data.speed_body_w) && + isfinite(ctl_data.roll_rate_setpoint) && + isfinite(ctl_data.pitch_rate_setpoint))) { perf_count(_nonfinite_input_perf); return _rate_setpoint; } + // static int counter = 0; /* Calculate desired yaw rate from coordinated turn constraint / (no side forces) */ _rate_setpoint = 0.0f; - if (sqrtf(speed_body_u * speed_body_u + speed_body_v * speed_body_v + speed_body_w * speed_body_w) > _coordinated_min_speed) { - float denumerator = (speed_body_u * cosf(roll) * cosf(pitch) + speed_body_w * sinf(pitch)); + if (sqrtf(ctl_data.speed_body_u * ctl_data.speed_body_u + ctl_data.speed_body_v * ctl_data.speed_body_v + + ctl_data.speed_body_w * ctl_data.speed_body_w) > _coordinated_min_speed) { + float denumerator = (ctl_data.speed_body_u * cosf(ctl_data.roll) * cosf(ctl_data.pitch) + + ctl_data.speed_body_w * sinf(ctl_data.pitch)); + if(fabsf(denumerator) > FLT_EPSILON) { - _rate_setpoint = (speed_body_w * roll_rate_setpoint + 9.81f * sinf(roll) * cosf(pitch) + speed_body_u * pitch_rate_setpoint * sinf(roll)) / denumerator; + _rate_setpoint = (ctl_data.speed_body_w * ctl_data.roll_rate_setpoint + + 9.81f * sinf(ctl_data.roll) * cosf(ctl_data.pitch) + + ctl_data.speed_body_u * ctl_data.pitch_rate_setpoint * sinf(ctl_data.roll)) / + denumerator; + // warnx("yaw: speed_body_u %.f speed_body_w %1.f roll %.1f pitch %.1f denumerator %.1f _rate_setpoint %.1f", speed_body_u, speed_body_w, denumerator, _rate_setpoint); } @@ -111,46 +109,49 @@ float ECL_YawController::control_attitude(float roll, float pitch, return _rate_setpoint; } -float ECL_YawController::control_bodyrate(float roll, float pitch, - float pitch_rate, float yaw_rate, - float pitch_rate_setpoint, - float airspeed_min, float airspeed_max, float airspeed, float scaler, bool lock_integrator) +float ECL_YawController::control_bodyrate(const struct ECL_ControlData &ctl_data) { /* Do not calculate control signal with bad inputs */ - if (!(isfinite(roll) && isfinite(pitch) && isfinite(pitch_rate) && isfinite(yaw_rate) && - isfinite(pitch_rate_setpoint) && isfinite(airspeed_min) && - isfinite(airspeed_max) && isfinite(scaler))) { + if (!(isfinite(ctl_data.roll) && isfinite(ctl_data.pitch) && isfinite(ctl_data.pitch_rate) && + isfinite(ctl_data.yaw_rate) && isfinite(ctl_data.pitch_rate_setpoint) && + isfinite(ctl_data.airspeed_min) && isfinite(ctl_data.airspeed_max) && + isfinite(ctl_data.scaler))) { perf_count(_nonfinite_input_perf); return math::constrain(_last_output, -1.0f, 1.0f); } + /* get the usual dt estimate */ uint64_t dt_micros = ecl_elapsed_time(&_last_run); _last_run = ecl_absolute_time(); float dt = (float)dt_micros * 1e-6f; /* lock integral for long intervals */ + bool lock_integrator = ctl_data.lock_integrator; if (dt_micros > 500000) lock_integrator = true; /* input conditioning */ + float airspeed = ctl_data.airspeed; if (!isfinite(airspeed)) { /* airspeed is NaN, +- INF or not available, pick center of band */ - airspeed = 0.5f * (airspeed_min + airspeed_max); - } else if (airspeed < airspeed_min) { - airspeed = airspeed_min; + airspeed = 0.5f * (ctl_data.airspeed_min + ctl_data.airspeed_max); + } else if (airspeed < ctl_data.airspeed_min) { + airspeed = ctl_data.airspeed_min; } - /* Transform setpoint to body angular rates */ - _bodyrate_setpoint = -sinf(roll) * pitch_rate_setpoint + cosf(roll)*cosf(pitch) * _rate_setpoint; //jacobian + /* Transform setpoint to body angular rates (jacobian) */ + _bodyrate_setpoint = -sinf(ctl_data.roll) * ctl_data.pitch_rate_setpoint + + cosf(ctl_data.roll)*cosf(ctl_data.pitch) * _rate_setpoint; - /* Transform estimation to body angular rates */ - float yaw_bodyrate = -sinf(roll) * pitch_rate + cosf(roll)*cosf(pitch) * yaw_rate; //jacobian + /* Transform estimation to body angular rates (jacobian) */ + float yaw_bodyrate = -sinf(ctl_data.roll) * ctl_data.pitch_rate + + cosf(ctl_data.roll)*cosf(ctl_data.pitch) * ctl_data.yaw_rate; /* Calculate body angular rate error */ _rate_error = _bodyrate_setpoint - yaw_bodyrate; //body angular rate error - if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * airspeed_min) { + if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * ctl_data.airspeed_min) { float id = _rate_error * dt; @@ -173,14 +174,9 @@ float ECL_YawController::control_bodyrate(float roll, float pitch, float integrator_constrained = math::constrain(_integrator * _k_i, -_integrator_max, _integrator_max); /* Apply PI rate controller and store non-limited output */ - _last_output = (_bodyrate_setpoint * _k_ff + _rate_error * _k_p + integrator_constrained) * scaler * scaler; //scaler is proportional to 1/airspeed + _last_output = (_bodyrate_setpoint * _k_ff + _rate_error * _k_p + integrator_constrained) * ctl_data.scaler * ctl_data.scaler; //scaler is proportional to 1/airspeed //warnx("yaw:_last_output: %.4f, _integrator: %.4f, _integrator_max: %.4f, airspeed %.4f, _k_i %.4f, _k_p: %.4f", (double)_last_output, (double)_integrator, (double)_integrator_max, (double)airspeed, (double)_k_i, (double)_k_p); return math::constrain(_last_output, -1.0f, 1.0f); } - -void ECL_YawController::reset_integrator() -{ - _integrator = 0.0f; -} diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h index a360c14b89..37c03705ef 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h @@ -50,79 +50,27 @@ #include #include -#include -class __EXPORT ECL_YawController //XXX: create controller superclass +#include "ecl_controller.h" + +class __EXPORT ECL_YawController : + public ECL_Controller { public: ECL_YawController(); ~ECL_YawController(); - float control_attitude(float roll, float pitch, - float speed_body_u, float speed_body_v, float speed_body_w, - float roll_rate_setpoint, float pitch_rate_setpoint); - - float control_bodyrate( float roll, float pitch, - float pitch_rate, float yaw_rate, - float pitch_rate_setpoint, - float airspeed_min, float airspeed_max, float airspeed, float scaler, bool lock_integrator); - - void reset_integrator(); - - void set_k_p(float k_p) { - _k_p = k_p; - } - - void set_k_i(float k_i) { - _k_i = k_i; - } - - void set_k_ff(float k_ff) { - _k_ff = k_ff; - } - - void set_integrator_max(float max) { - _integrator_max = max; - } - - void set_max_rate(float max_rate) { - _max_rate = max_rate; - } + float control_attitude(const struct ECL_ControlData &ctl_data); + float control_bodyrate(const struct ECL_ControlData &ctl_data); + /* Additional setters */ void set_coordinated_min_speed(float coordinated_min_speed) { _coordinated_min_speed = coordinated_min_speed; } - - float get_rate_error() { - return _rate_error; - } - - float get_desired_rate() { - return _rate_setpoint; - } - - float get_desired_bodyrate() { - return _bodyrate_setpoint; - } - -private: - uint64_t _last_run; - float _k_p; - float _k_i; - float _k_ff; - float _integrator_max; - float _max_rate; - float _roll_ff; - float _last_output; - float _integrator; - float _rate_error; - float _rate_setpoint; - float _bodyrate_setpoint; +protected: float _coordinated_min_speed; - perf_counter_t _nonfinite_input_perf; - }; #endif // ECL_YAW_CONTROLLER_H diff --git a/src/lib/ecl/module.mk b/src/lib/ecl/module.mk index 93a5b511f9..03974c9503 100644 --- a/src/lib/ecl/module.mk +++ b/src/lib/ecl/module.mk @@ -35,7 +35,8 @@ # Estimation and Control Library # -SRCS = attitude_fw/ecl_pitch_controller.cpp \ +SRCS = attitude_fw/ecl_controller.cpp \ + attitude_fw/ecl_pitch_controller.cpp \ attitude_fw/ecl_roll_controller.cpp \ attitude_fw/ecl_yaw_controller.cpp \ l1/ecl_l1_pos_controller.cpp 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 260b25a480..d2e993ba95 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -933,19 +933,38 @@ FixedwingAttitudeControl::task_main() } } + /* Prepare data for attitude controllers */ + struct ECL_ControlData control_input = {}; + control_input.roll = _att.roll; + control_input.pitch = _att.pitch; + control_input.yaw = _att.yaw; + control_input.roll_rate = _att.rollspeed; + control_input.pitch_rate = _att.pitchspeed; + control_input.yaw_rate = _att.yawspeed; + control_input.speed_body_u = speed_body_u; + control_input.speed_body_v = speed_body_v; + control_input.speed_body_w = speed_body_w; + control_input.roll_setpoint = roll_sp; + control_input.pitch_setpoint = pitch_sp; + control_input.airspeed_min = _parameters.airspeed_min; + control_input.airspeed_max = _parameters.airspeed_max; + control_input.airspeed = airspeed; + control_input.scaler = airspeed_scaling; + control_input.lock_integrator = lock_integrator; + /* Run attitude controllers */ if (isfinite(roll_sp) && isfinite(pitch_sp)) { - _roll_ctrl.control_attitude(roll_sp, _att.roll); - _pitch_ctrl.control_attitude(pitch_sp, _att.roll, _att.pitch, airspeed); - _yaw_ctrl.control_attitude(_att.roll, _att.pitch, - speed_body_u, speed_body_v, speed_body_w, - _roll_ctrl.get_desired_rate(), _pitch_ctrl.get_desired_rate()); //runs last, because is depending on output of roll and pitch attitude + _roll_ctrl.control_attitude(control_input); + _pitch_ctrl.control_attitude(control_input); + _yaw_ctrl.control_attitude(control_input); //runs last, because is depending on output of roll and pitch attitude + + /* Update input data for rate controllers */ + control_input.roll_rate_setpoint = _roll_ctrl.get_desired_rate(); + control_input.pitch_rate_setpoint = _pitch_ctrl.get_desired_rate(); + control_input.yaw_rate_setpoint = _yaw_ctrl.get_desired_rate(); /* Run attitude RATE controllers which need the desired attitudes from above, add trim */ - float roll_u = _roll_ctrl.control_bodyrate(_att.pitch, - _att.rollspeed, _att.yawspeed, - _yaw_ctrl.get_desired_rate(), - _parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator); + float roll_u = _roll_ctrl.control_bodyrate(control_input); _actuators.control[0] = (isfinite(roll_u)) ? roll_u + _parameters.trim_roll : _parameters.trim_roll; if (!isfinite(roll_u)) { _roll_ctrl.reset_integrator(); @@ -956,10 +975,7 @@ FixedwingAttitudeControl::task_main() } } - float pitch_u = _pitch_ctrl.control_bodyrate(_att.roll, _att.pitch, - _att.pitchspeed, _att.yawspeed, - _yaw_ctrl.get_desired_rate(), - _parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator); + float pitch_u = _pitch_ctrl.control_bodyrate(control_input); _actuators.control[1] = (isfinite(pitch_u)) ? pitch_u + _parameters.trim_pitch : _parameters.trim_pitch; if (!isfinite(pitch_u)) { _pitch_ctrl.reset_integrator(); @@ -980,10 +996,7 @@ FixedwingAttitudeControl::task_main() } } - float yaw_u = _yaw_ctrl.control_bodyrate(_att.roll, _att.pitch, - _att.pitchspeed, _att.yawspeed, - _pitch_ctrl.get_desired_rate(), - _parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator); + float yaw_u = _yaw_ctrl.control_bodyrate(control_input); _actuators.control[2] = (isfinite(yaw_u)) ? yaw_u + _parameters.trim_yaw : _parameters.trim_yaw; /* add in manual rudder control */ From 78cde98ea89dfab02e27c4e36536dc1e827dfce5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 6 Jan 2015 18:22:44 +0100 Subject: [PATCH 426/652] GPS driver: Add missing wall clock setup for MTK GPS modules --- src/drivers/gps/mtk.cpp | 22 +++++++++++++++++++--- 1 file changed, 19 insertions(+), 3 deletions(-) diff --git a/src/drivers/gps/mtk.cpp b/src/drivers/gps/mtk.cpp index c0c47073bc..c112f65a8b 100644 --- a/src/drivers/gps/mtk.cpp +++ b/src/drivers/gps/mtk.cpp @@ -264,7 +264,7 @@ MTK::handle_message(gps_mtk_packet_t &packet) _gps_position->satellites_used = packet.satellites; /* convert time and date information to unix timestamp */ - struct tm timeinfo; //TODO: test this conversion + struct tm timeinfo; uint32_t timeinfo_conversion_temp; timeinfo.tm_mday = packet.date * 1e-4; @@ -280,8 +280,24 @@ MTK::handle_message(gps_mtk_packet_t &packet) timeinfo_conversion_temp -= timeinfo.tm_sec * 1e3; time_t epoch = mktime(&timeinfo); - _gps_position->time_utc_usec = epoch * 1e6; //TODO: test this - _gps_position->time_utc_usec += timeinfo_conversion_temp * 1e3; + if (epoch > GPS_EPOCH_SECS) { + // FMUv2+ boards have a hardware RTC, but GPS helps us to configure it + // and control its drift. Since we rely on the HRT for our monotonic + // clock, updating it from time to time is safe. + + timespec ts; + ts.tv_sec = epoch; + ts.tv_nsec = timeinfo_conversion_temp * 1000000ULL; + if (clock_settime(CLOCK_REALTIME, &ts)) { + warn("failed setting clock"); + } + + _gps_position->time_utc_usec = static_cast(epoch) * 1000000ULL; + _gps_position->time_utc_usec += timeinfo_conversion_temp * 1000ULL; + } else { + _gps_position->time_utc_usec = 0; + } + _gps_position->timestamp_position = _gps_position->timestamp_time = hrt_absolute_time(); // Position and velocity update always at the same time From f4c294bf5d023c31500ed0cfca14fc4544bcb35a Mon Sep 17 00:00:00 2001 From: tumbili Date: Mon, 5 Jan 2015 20:50:51 +0100 Subject: [PATCH 427/652] added startup script and mixer file for FireFly6 --- ROMFS/px4fmu_common/init.d/13002_firefly6 | 14 ++++++++++++++ ROMFS/px4fmu_common/mixers/FMU_firefly6.mix | 15 +++++++++++++++ 2 files changed, 29 insertions(+) create mode 100644 ROMFS/px4fmu_common/init.d/13002_firefly6 create mode 100644 ROMFS/px4fmu_common/mixers/FMU_firefly6.mix diff --git a/ROMFS/px4fmu_common/init.d/13002_firefly6 b/ROMFS/px4fmu_common/init.d/13002_firefly6 new file mode 100644 index 0000000000..e371a9a1e5 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/13002_firefly6 @@ -0,0 +1,14 @@ +#!nsh +# +# Generic configuration file for BirdsEyeView Aerobotics FireFly6 +# +# Roman Bapst +# + +sh /etc/init.d/rc.vtol_defaults + +set MIXER FMU_FireFly6 + +set PWM_OUT 123456 +param set VT_MOT_COUNT 6 +param set VT_IDLE_PWM_MC 1080 diff --git a/ROMFS/px4fmu_common/mixers/FMU_firefly6.mix b/ROMFS/px4fmu_common/mixers/FMU_firefly6.mix new file mode 100644 index 0000000000..598f9f797d --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/FMU_firefly6.mix @@ -0,0 +1,15 @@ +# FireFly6 mixer for PX4FMU +# +#=========================== +R: 6c 10000 10000 10000 0 + +# mixer for the elevons +M: 2 +O: 10000 10000 0 -10000 10000 +S: 1 0 7500 7500 0 -10000 10000 +S: 1 1 8000 8000 0 -10000 10000 + +M: 2 +O: 10000 10000 0 -10000 10000 +S: 1 0 7500 7500 0 -10000 10000 +S: 1 1 -8000 -8000 0 -10000 10000 From ef9b1b134e5e7b37d275599ef8f3824b3cf48d70 Mon Sep 17 00:00:00 2001 From: tumbili Date: Mon, 5 Jan 2015 20:52:18 +0100 Subject: [PATCH 428/652] get MAV_TYPE by mixer file for FireFly6 --- ROMFS/px4fmu_common/init.d/rcS | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 26b729aad1..10c5e09475 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -580,6 +580,10 @@ then then set MAV_TYPE 19 fi + if [ $MIXER == FMU_firefly6 ] + then + set MAV_TYPE 21 + fi fi # Still no MAV_TYPE found From 648cb78268b6ce636d8b810c07564e76fedc6089 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 6 Jan 2015 12:54:23 +0100 Subject: [PATCH 429/652] FMU driver: Show up as secondary interface to allow mixer loads. --- src/drivers/px4fmu/fmu.cpp | 20 +++++++++++--------- 1 file changed, 11 insertions(+), 9 deletions(-) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 3d3e1b0ebb..4366720407 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -128,7 +128,7 @@ private: orb_advert_t _outputs_pub; actuator_armed_s _armed; unsigned _num_outputs; - bool _primary_pwm_device; + int _class_instance; volatile bool _task_should_exit; bool _servo_armed; @@ -141,6 +141,7 @@ private: int _control_subs[NUM_ACTUATOR_CONTROL_GROUPS]; actuator_controls_s _controls[NUM_ACTUATOR_CONTROL_GROUPS]; orb_id_t _control_topics[NUM_ACTUATOR_CONTROL_GROUPS]; + orb_id_t _actuator_output_topic; pollfd _poll_fds[NUM_ACTUATOR_CONTROL_GROUPS]; unsigned _poll_fds_num; @@ -247,7 +248,7 @@ PX4FMU::PX4FMU() : _outputs_pub(-1), _armed{}, _num_outputs(0), - _primary_pwm_device(false), + _class_instance(0), _task_should_exit(false), _servo_armed(false), _pwm_on(false), @@ -255,6 +256,7 @@ PX4FMU::PX4FMU() : _groups_required(0), _groups_subscribed(0), _control_subs{-1}, + _actuator_output_topic(nullptr), _poll_fds_num(0), _pwm_limit{}, _failsafe_pwm{0}, @@ -300,8 +302,7 @@ PX4FMU::~PX4FMU() } /* clean up the alternate device node */ - if (_primary_pwm_device) - unregister_driver(PWM_OUTPUT_DEVICE_PATH); + unregister_class_devname(PWM_OUTPUT_DEVICE_PATH, _class_instance); g_fmu = nullptr; } @@ -320,13 +321,14 @@ PX4FMU::init() return ret; /* 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); + _class_instance = register_class_devname(PWM_OUTPUT_DEVICE_PATH); - if (ret == OK) { + if (_class_instance == CLASS_DEVICE_PRIMARY) { log("default PWM output device"); - _primary_pwm_device = true; } + _actuator_output_topic = ORB_ID_DOUBLE(actuator_outputs_, _class_instance); + /* reset GPIOs */ gpio_reset(); @@ -677,10 +679,10 @@ PX4FMU::task_main() /* publish mixed control outputs */ if (_outputs_pub < 0) { - _outputs_pub = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), &outputs); + _outputs_pub = orb_advertise(_actuator_output_topic, &outputs); } else { - orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), _outputs_pub, &outputs); + orb_publish(_actuator_output_topic, _outputs_pub, &outputs); } } } From ff932820e341d4eae7e0764719fcbf5555c5da14 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Tue, 6 Jan 2015 16:29:10 +0100 Subject: [PATCH 430/652] use two mixers, one for rotors via px4io and one for elevons via fmu outputs --- .../mixers/{FMU_firefly6.mix => FMU_firefly6_elevons.mix} | 7 +------ ROMFS/px4fmu_common/mixers/FMU_firefly6_rotors.mix | 4 ++++ 2 files changed, 5 insertions(+), 6 deletions(-) rename ROMFS/px4fmu_common/mixers/{FMU_firefly6.mix => FMU_firefly6_elevons.mix} (70%) create mode 100644 ROMFS/px4fmu_common/mixers/FMU_firefly6_rotors.mix diff --git a/ROMFS/px4fmu_common/mixers/FMU_firefly6.mix b/ROMFS/px4fmu_common/mixers/FMU_firefly6_elevons.mix similarity index 70% rename from ROMFS/px4fmu_common/mixers/FMU_firefly6.mix rename to ROMFS/px4fmu_common/mixers/FMU_firefly6_elevons.mix index 598f9f797d..9ed6eeed91 100644 --- a/ROMFS/px4fmu_common/mixers/FMU_firefly6.mix +++ b/ROMFS/px4fmu_common/mixers/FMU_firefly6_elevons.mix @@ -1,9 +1,4 @@ -# FireFly6 mixer for PX4FMU -# -#=========================== -R: 6c 10000 10000 10000 0 - -# mixer for the elevons +# mixer for the FireFly6 elevons M: 2 O: 10000 10000 0 -10000 10000 S: 1 0 7500 7500 0 -10000 10000 diff --git a/ROMFS/px4fmu_common/mixers/FMU_firefly6_rotors.mix b/ROMFS/px4fmu_common/mixers/FMU_firefly6_rotors.mix new file mode 100644 index 0000000000..3e1d9d7a34 --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/FMU_firefly6_rotors.mix @@ -0,0 +1,4 @@ +# FireFly6 mixer for PX4FMU +# +#=========================== +R: 6c 10000 10000 10000 0 From 7ab58a8bc136da2e8a9166bd3fee7ed801c2adb0 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Tue, 6 Jan 2015 16:30:47 +0100 Subject: [PATCH 431/652] allow to specify secondary mixer which drives fmu PWM outputs --- ROMFS/px4fmu_common/init.d/rc.interface | 14 ++++++++++++++ ROMFS/px4fmu_common/init.d/rcS | 3 ++- 2 files changed, 16 insertions(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.interface b/ROMFS/px4fmu_common/init.d/rc.interface index bab71be930..87c9e30452 100644 --- a/ROMFS/px4fmu_common/init.d/rc.interface +++ b/ROMFS/px4fmu_common/init.d/rc.interface @@ -80,3 +80,17 @@ then pwm failsafe -d $OUTPUT_DEV $FAILSAFE fi fi + +# check if should load secondary mixer +if [ $MIXER_SEC != none ] +then + if fmu mode_pwm + then + echo "[i] FMU mode_pwm started to handle secondary mixer" + # load the specified mixer + mixer load /dev/pwm_output1 /etc/mixers/$MIXER_SEC.mix + else + echo "[i] ERROR: FMU mode_pwm start failed" + tone_alarm $TUNE_ERR + fi +fi diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 10c5e09475..924f6ebc08 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -116,6 +116,7 @@ then set HIL no set VEHICLE_TYPE none set MIXER none + set MIXER_SEC none set OUTPUT_MODE none set PWM_OUT none set PWM_RATE none @@ -580,7 +581,7 @@ then then set MAV_TYPE 19 fi - if [ $MIXER == FMU_firefly6 ] + if [ $MIXER == FMU_firefly6_rotors ] then set MAV_TYPE 21 fi From 5c5bba5d5f8c330715e13a0c40d116b9c3a2c937 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Tue, 6 Jan 2015 16:31:40 +0100 Subject: [PATCH 432/652] specify secondary elevon mixer --- ROMFS/px4fmu_common/init.d/13002_firefly6 | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/13002_firefly6 b/ROMFS/px4fmu_common/init.d/13002_firefly6 index e371a9a1e5..d5a104b4cf 100644 --- a/ROMFS/px4fmu_common/init.d/13002_firefly6 +++ b/ROMFS/px4fmu_common/init.d/13002_firefly6 @@ -7,8 +7,9 @@ sh /etc/init.d/rc.vtol_defaults -set MIXER FMU_FireFly6 +set MIXER FMU_firefly6_rotors +set MIXER_SEC FMU_firefly6_elevons -set PWM_OUT 123456 +set PWM_OUT 12345678 param set VT_MOT_COUNT 6 param set VT_IDLE_PWM_MC 1080 From 74b63d08e1f2dd34b2c18be9d46d0837a9be5b9f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 6 Jan 2015 19:53:52 +0100 Subject: [PATCH 433/652] Mixer load: Be less chatty --- ROMFS/px4fmu_common/init.d/rc.interface | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.interface b/ROMFS/px4fmu_common/init.d/rc.interface index 87c9e30452..98f41b31ec 100644 --- a/ROMFS/px4fmu_common/init.d/rc.interface +++ b/ROMFS/px4fmu_common/init.d/rc.interface @@ -9,7 +9,7 @@ then # Load mixer # - #Use the mixer file from the SD-card if it exists + # Use the mixer file from the SD-card if it exists if [ -f /fs/microsd/etc/mixers/$MIXER.mix ] then set MIXER_FILE /fs/microsd/etc/mixers/$MIXER.mix @@ -86,11 +86,8 @@ if [ $MIXER_SEC != none ] then if fmu mode_pwm then - echo "[i] FMU mode_pwm started to handle secondary mixer" - # load the specified mixer mixer load /dev/pwm_output1 /etc/mixers/$MIXER_SEC.mix else - echo "[i] ERROR: FMU mode_pwm start failed" tone_alarm $TUNE_ERR fi fi From 08062e6546f02cb2e819f690d35b485425788cc4 Mon Sep 17 00:00:00 2001 From: Anton Matosov Date: Wed, 7 Jan 2015 00:25:06 +0200 Subject: [PATCH 434/652] Changed naming of the mixers to get rid of umbiguity as outputs are actually going to be driven by io, not fmu Implemented automatic lookup for the .aux.mix file if it exists --- .../init.d/1000_rc_fw_easystar.hil | 2 +- .../px4fmu_common/init.d/10015_tbs_discovery | 2 +- ROMFS/px4fmu_common/init.d/10016_3dr_iris | 2 +- .../init.d/10017_steadidrone_qu4d | 2 +- .../px4fmu_common/init.d/10018_tbs_endurance | 2 +- ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil | 2 +- ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil | 2 +- .../init.d/1004_rc_fw_Rascal110.hil | 2 +- .../init.d/1005_rc_fw_Malolo1.hil | 2 +- ROMFS/px4fmu_common/init.d/11001_hexa_cox | 2 +- ROMFS/px4fmu_common/init.d/12001_octo_cox | 2 +- .../init.d/13001_caipirinha_vtol | 2 +- ROMFS/px4fmu_common/init.d/13002_firefly6 | 3 +- ROMFS/px4fmu_common/init.d/2101_hk_bixler | 2 +- .../px4fmu_common/init.d/2103_skyhunter_1800 | 2 +- ROMFS/px4fmu_common/init.d/3030_io_camflyer | 2 +- ROMFS/px4fmu_common/init.d/3032_skywalker_x5 | 2 +- ROMFS/px4fmu_common/init.d/3034_fx79 | 2 +- .../px4fmu_common/init.d/3100_tbs_caipirinha | 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 +- ROMFS/px4fmu_common/init.d/rc.interface | 58 ++++++++++++++----- ROMFS/px4fmu_common/init.d/rcS | 20 +++---- .../mixers/{FMU_AERT.mix => AERT.main.mix} | 0 .../mixers/{FMU_AET.mix => AET.main.mix} | 0 .../mixers/{FMU_CCPM.mix => CCPM.main.mix} | 0 .../mixers/{FMU_FX79.mix => FX79.main.mix} | 0 .../mixers/{FMU_Q.mix => Q.main.mix} | 0 .../mixers/{FMU_RET.mix => RET.main.mix} | 0 .../mixers/{Viper.mix => Viper.main.mix} | 0 .../mixers/{FMU_X5.mix => X5.main.mix} | 0 ...inha_vtol.mix => caipirinha_vtol.main.mix} | 0 .../mixers/{FMU_delta.mix => delta.main.mix} | 0 .../{easystar.mix => easystar.main.mix} | 0 ..._firefly6_elevons.mix => firefly6.aux.mix} | 0 ..._firefly6_rotors.mix => firefly6.main.mix} | 0 .../{FMU_hexa_+.mix => hexa_+.main.mix} | 0 .../{FMU_hexa_cox.mix => hexa_cox.main.mix} | 0 .../{FMU_hexa_x.mix => hexa_x.main.mix} | 0 .../{FMU_octo_+.mix => octo_+.main.mix} | 0 .../{FMU_octo_cox.mix => octo_cox.main.mix} | 0 .../{FMU_octo_x.mix => octo_x.main.mix} | 0 .../mixers/{phantom.mix => phantom.main.mix} | 0 .../{FMU_quad_+.mix => quad_+.main.mix} | 0 .../{FMU_quad_v.mix => quad_v.main.mix} | 0 .../{FMU_quad_w.mix => quad_w.main.mix} | 0 .../{FMU_quad_x.mix => quad_x.main.mix} | 0 .../{skywalker.mix => skywalker.main.mix} | 0 .../{wingwing.mix => wingwing.main.mix} | 0 src/systemcmds/tests/test_mixer.cpp | 2 +- unittests/conversion_test.cpp | 2 +- unittests/mixer_test.cpp | 2 +- 56 files changed, 81 insertions(+), 54 deletions(-) rename ROMFS/px4fmu_common/mixers/{FMU_AERT.mix => AERT.main.mix} (100%) rename ROMFS/px4fmu_common/mixers/{FMU_AET.mix => AET.main.mix} (100%) rename ROMFS/px4fmu_common/mixers/{FMU_CCPM.mix => CCPM.main.mix} (100%) rename ROMFS/px4fmu_common/mixers/{FMU_FX79.mix => FX79.main.mix} (100%) rename ROMFS/px4fmu_common/mixers/{FMU_Q.mix => Q.main.mix} (100%) rename ROMFS/px4fmu_common/mixers/{FMU_RET.mix => RET.main.mix} (100%) rename ROMFS/px4fmu_common/mixers/{Viper.mix => Viper.main.mix} (100%) rename ROMFS/px4fmu_common/mixers/{FMU_X5.mix => X5.main.mix} (100%) rename ROMFS/px4fmu_common/mixers/{FMU_caipirinha_vtol.mix => caipirinha_vtol.main.mix} (100%) rename ROMFS/px4fmu_common/mixers/{FMU_delta.mix => delta.main.mix} (100%) rename ROMFS/px4fmu_common/mixers/{easystar.mix => easystar.main.mix} (100%) rename ROMFS/px4fmu_common/mixers/{FMU_firefly6_elevons.mix => firefly6.aux.mix} (100%) rename ROMFS/px4fmu_common/mixers/{FMU_firefly6_rotors.mix => firefly6.main.mix} (100%) rename ROMFS/px4fmu_common/mixers/{FMU_hexa_+.mix => hexa_+.main.mix} (100%) rename ROMFS/px4fmu_common/mixers/{FMU_hexa_cox.mix => hexa_cox.main.mix} (100%) rename ROMFS/px4fmu_common/mixers/{FMU_hexa_x.mix => hexa_x.main.mix} (100%) rename ROMFS/px4fmu_common/mixers/{FMU_octo_+.mix => octo_+.main.mix} (100%) rename ROMFS/px4fmu_common/mixers/{FMU_octo_cox.mix => octo_cox.main.mix} (100%) rename ROMFS/px4fmu_common/mixers/{FMU_octo_x.mix => octo_x.main.mix} (100%) rename ROMFS/px4fmu_common/mixers/{phantom.mix => phantom.main.mix} (100%) rename ROMFS/px4fmu_common/mixers/{FMU_quad_+.mix => quad_+.main.mix} (100%) rename ROMFS/px4fmu_common/mixers/{FMU_quad_v.mix => quad_v.main.mix} (100%) rename ROMFS/px4fmu_common/mixers/{FMU_quad_w.mix => quad_w.main.mix} (100%) rename ROMFS/px4fmu_common/mixers/{FMU_quad_x.mix => quad_x.main.mix} (100%) rename ROMFS/px4fmu_common/mixers/{skywalker.mix => skywalker.main.mix} (100%) rename ROMFS/px4fmu_common/mixers/{wingwing.mix => wingwing.main.mix} (100%) diff --git a/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil b/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil index 4d6d350b8a..40b9ed8df5 100644 --- a/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil +++ b/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil @@ -7,4 +7,4 @@ sh /etc/init.d/rc.fw_defaults set HIL yes -set MIXER FMU_AERT +set MIXER AERT diff --git a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery index c1b366de83..2c7c0d68e2 100644 --- a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery +++ b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery @@ -24,7 +24,7 @@ then param set MC_YAWRATE_D 0.0 fi -set MIXER FMU_quad_w +set MIXER quad_w 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 3879737a8e..caa8e28fc5 100644 --- a/ROMFS/px4fmu_common/init.d/10016_3dr_iris +++ b/ROMFS/px4fmu_common/init.d/10016_3dr_iris @@ -27,7 +27,7 @@ then param set BAT_C_SCALING 0.0124 fi -set MIXER FMU_quad_w +set MIXER quad_w set PWM_OUT 1234 diff --git a/ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d b/ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d index 57f77754c8..5e41d6957d 100644 --- a/ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d +++ b/ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d @@ -26,7 +26,7 @@ then param set BAT_N_CELLS 4 fi -set MIXER FMU_quad_w +set MIXER quad_w set PWM_MIN 1210 set PWM_MAX 2100 diff --git a/ROMFS/px4fmu_common/init.d/10018_tbs_endurance b/ROMFS/px4fmu_common/init.d/10018_tbs_endurance index 668026c616..eeb9b48b29 100644 --- a/ROMFS/px4fmu_common/init.d/10018_tbs_endurance +++ b/ROMFS/px4fmu_common/init.d/10018_tbs_endurance @@ -25,7 +25,7 @@ then param set MC_YAWRATE_D 0.0 fi -set MIXER FMU_quad_w +set MIXER quad_w set PWM_OUTPUTS 1234 set PWM_MIN 1200 diff --git a/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil b/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil index 1c4f6803b6..03b6b30d27 100644 --- a/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil +++ b/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil @@ -7,6 +7,6 @@ sh /etc/init.d/rc.mc_defaults -set MIXER FMU_quad_x +set MIXER quad_x set HIL yes diff --git a/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil b/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil index 0cbdd75bed..2b418dfc50 100644 --- a/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil +++ b/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil @@ -7,6 +7,6 @@ sh /etc/init.d/rc.mc_defaults -set MIXER FMU_quad_+ +set MIXER quad_+ set HIL yes \ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil b/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil index fb440d2fc7..0909208478 100644 --- a/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil +++ b/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil @@ -9,4 +9,4 @@ sh /etc/init.d/rc.fw_defaults set HIL yes -set MIXER FMU_AERT +set MIXER AERT 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 f208b692a0..15e5cf21d8 100644 --- a/ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil +++ b/ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil @@ -35,4 +35,4 @@ fi set HIL yes # Set the AERT mixer for HIL (even if the malolo is a flying wing) -set MIXER FMU_AERT +set MIXER AERT diff --git a/ROMFS/px4fmu_common/init.d/11001_hexa_cox b/ROMFS/px4fmu_common/init.d/11001_hexa_cox index 50f717e3df..0bb8cb52ee 100644 --- a/ROMFS/px4fmu_common/init.d/11001_hexa_cox +++ b/ROMFS/px4fmu_common/init.d/11001_hexa_cox @@ -7,7 +7,7 @@ sh /etc/init.d/rc.mc_defaults -set MIXER FMU_hexa_cox +set MIXER hexa_cox # Need to set all 8 channels 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 e0a8381851..16e86fd5fa 100644 --- a/ROMFS/px4fmu_common/init.d/12001_octo_cox +++ b/ROMFS/px4fmu_common/init.d/12001_octo_cox @@ -7,6 +7,6 @@ sh /etc/init.d/rc.mc_defaults -set MIXER FMU_octo_cox +set MIXER octo_cox set PWM_OUT 12345678 diff --git a/ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol b/ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol index 7e9a6d3dc8..87b98a02bc 100644 --- a/ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol +++ b/ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol @@ -7,7 +7,7 @@ sh /etc/init.d/rc.vtol_defaults -set MIXER FMU_caipirinha_vtol +set MIXER caipirinha_vtol set PWM_OUT 12 set PWM_MAX 2000 diff --git a/ROMFS/px4fmu_common/init.d/13002_firefly6 b/ROMFS/px4fmu_common/init.d/13002_firefly6 index d5a104b4cf..551a19928e 100644 --- a/ROMFS/px4fmu_common/init.d/13002_firefly6 +++ b/ROMFS/px4fmu_common/init.d/13002_firefly6 @@ -7,8 +7,7 @@ sh /etc/init.d/rc.vtol_defaults -set MIXER FMU_firefly6_rotors -set MIXER_SEC FMU_firefly6_elevons +set MIXER firefly6 set PWM_OUT 12345678 param set VT_MOT_COUNT 6 diff --git a/ROMFS/px4fmu_common/init.d/2101_hk_bixler b/ROMFS/px4fmu_common/init.d/2101_hk_bixler index dcc5db824a..0e6d2481c0 100644 --- a/ROMFS/px4fmu_common/init.d/2101_hk_bixler +++ b/ROMFS/px4fmu_common/init.d/2101_hk_bixler @@ -2,4 +2,4 @@ sh /etc/init.d/rc.fw_defaults -set MIXER FMU_AERT \ No newline at end of file +set MIXER AERT \ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d/2103_skyhunter_1800 b/ROMFS/px4fmu_common/init.d/2103_skyhunter_1800 index 9bc0262d83..2433ab4f40 100644 --- a/ROMFS/px4fmu_common/init.d/2103_skyhunter_1800 +++ b/ROMFS/px4fmu_common/init.d/2103_skyhunter_1800 @@ -2,4 +2,4 @@ sh /etc/init.d/rc.fw_defaults -set MIXER FMU_AET +set MIXER AET diff --git a/ROMFS/px4fmu_common/init.d/3030_io_camflyer b/ROMFS/px4fmu_common/init.d/3030_io_camflyer index fe0269557f..1886783249 100644 --- a/ROMFS/px4fmu_common/init.d/3030_io_camflyer +++ b/ROMFS/px4fmu_common/init.d/3030_io_camflyer @@ -2,7 +2,7 @@ sh /etc/init.d/rc.fw_defaults -set MIXER FMU_Q +set MIXER Q # Provide ESC a constant 1000 us pulse while disarmed set PWM_OUT 4 set PWM_DISARMED 1000 diff --git a/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 b/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 index 94363bf6ab..3d464a4ae9 100644 --- a/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 +++ b/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 @@ -28,4 +28,4 @@ then param set FW_R_RMAX 0 fi -set MIXER FMU_X5 +set MIXER X5 diff --git a/ROMFS/px4fmu_common/init.d/3034_fx79 b/ROMFS/px4fmu_common/init.d/3034_fx79 index 4a76ba6ebd..d46147ede5 100644 --- a/ROMFS/px4fmu_common/init.d/3034_fx79 +++ b/ROMFS/px4fmu_common/init.d/3034_fx79 @@ -7,4 +7,4 @@ sh /etc/init.d/rc.fw_defaults -set MIXER FMU_FX79 +set MIXER FX79 diff --git a/ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha b/ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha index 9eafac1c59..9e1c1c170a 100644 --- a/ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha +++ b/ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha @@ -31,4 +31,4 @@ then param set FW_R_RMAX 0 fi -set MIXER FMU_Q +set MIXER Q diff --git a/ROMFS/px4fmu_common/init.d/4001_quad_x b/ROMFS/px4fmu_common/init.d/4001_quad_x index 4677f9fc3a..e0538160f0 100644 --- a/ROMFS/px4fmu_common/init.d/4001_quad_x +++ b/ROMFS/px4fmu_common/init.d/4001_quad_x @@ -7,6 +7,6 @@ sh /etc/init.d/rc.mc_defaults -set MIXER FMU_quad_x +set MIXER quad_x set PWM_OUT 1234 diff --git a/ROMFS/px4fmu_common/init.d/5001_quad_+ b/ROMFS/px4fmu_common/init.d/5001_quad_+ index c789113919..e51f9cf89d 100644 --- a/ROMFS/px4fmu_common/init.d/5001_quad_+ +++ b/ROMFS/px4fmu_common/init.d/5001_quad_+ @@ -7,6 +7,6 @@ sh /etc/init.d/rc.mc_defaults -set MIXER FMU_quad_+ +set MIXER quad_+ 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 0df25b11a1..7a6dda648a 100644 --- a/ROMFS/px4fmu_common/init.d/6001_hexa_x +++ b/ROMFS/px4fmu_common/init.d/6001_hexa_x @@ -7,7 +7,7 @@ sh /etc/init.d/rc.mc_defaults -set MIXER FMU_hexa_x +set MIXER hexa_x # Need to set all 8 channels set PWM_OUT 12345678 diff --git a/ROMFS/px4fmu_common/init.d/7001_hexa_+ b/ROMFS/px4fmu_common/init.d/7001_hexa_+ index 16c772ee1e..dd9589d614 100644 --- a/ROMFS/px4fmu_common/init.d/7001_hexa_+ +++ b/ROMFS/px4fmu_common/init.d/7001_hexa_+ @@ -7,7 +7,7 @@ sh /etc/init.d/rc.mc_defaults -set MIXER FMU_hexa_+ +set MIXER hexa_+ # Need to set all 8 channels 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 bae36737f9..7cbb3ddfcd 100644 --- a/ROMFS/px4fmu_common/init.d/8001_octo_x +++ b/ROMFS/px4fmu_common/init.d/8001_octo_x @@ -7,6 +7,6 @@ sh /etc/init.d/rc.mc_defaults -set MIXER FMU_octo_x +set MIXER octo_x set PWM_OUT 12345678 diff --git a/ROMFS/px4fmu_common/init.d/9001_octo_+ b/ROMFS/px4fmu_common/init.d/9001_octo_+ index ca5439f68f..5d608d593c 100644 --- a/ROMFS/px4fmu_common/init.d/9001_octo_+ +++ b/ROMFS/px4fmu_common/init.d/9001_octo_+ @@ -7,6 +7,6 @@ sh /etc/init.d/rc.mc_defaults -set MIXER FMU_octo_+ +set MIXER octo_+ set PWM_OUT 12345678 diff --git a/ROMFS/px4fmu_common/init.d/rc.interface b/ROMFS/px4fmu_common/init.d/rc.interface index 98f41b31ec..d4880ea420 100644 --- a/ROMFS/px4fmu_common/init.d/rc.interface +++ b/ROMFS/px4fmu_common/init.d/rc.interface @@ -3,19 +3,30 @@ # Script to configure control interface # +set SDCARD_MIXERS_PATH /fs/microsd/etc/mixers + if [ $MIXER != none -a $MIXER != skip ] then # - # Load mixer + # Load main mixer # + + if [ $MIXER_AUX == none ] + then + MIXER_AUX = $MIXER.aux + fi # Use the mixer file from the SD-card if it exists - if [ -f /fs/microsd/etc/mixers/$MIXER.mix ] + if [ -f $SDCARD_MIXERS_PATH/$MIXER.main.mix ] then - set MIXER_FILE /fs/microsd/etc/mixers/$MIXER.mix - else - set MIXER_FILE /etc/mixers/$MIXER.mix - fi + set MIXER_FILE $SDCARD_MIXERS_PATH/$MIXER.main.mix + # Try out the old convention, for backward compatibility + elif [ -f $SDCARD_MIXERS_PATH/$MIXER.mix ] + then + set MIXER_FILE $SDCARD_MIXERS_PATH/$MIXER.mix + else + set MIXER_FILE /etc/mixers/$MIXER.main.mix + fi if [ $OUTPUT_MODE == mkblctrl ] then @@ -80,14 +91,31 @@ then pwm failsafe -d $OUTPUT_DEV $FAILSAFE fi fi - + # check if should load secondary mixer -if [ $MIXER_SEC != none ] +if [ $MIXER_AUX != none ] then - if fmu mode_pwm - then - mixer load /dev/pwm_output1 /etc/mixers/$MIXER_SEC.mix - else - tone_alarm $TUNE_ERR - fi -fi + # + # Load aux mixer + # + + set MIXER_AUX_FILE none + + if [ -f $SDCARD_MIXERS_PATH/$MIXER_AUX.mix ] + then + set MIXER_AUX_FILE $SDCARD_MIXERS_PATH/$MIXER_AUX.mix + elif [ -f /etc/mixers/$MIXER_AUX.mix ] + then + set MIXER_AUX_FILE /etc/mixers/$MIXER_AUX.mix + fi + + if [ $MIXER_AUX_FILE != none -a $FMU_MODE == pwm ] + then + if fmu mode_pwm + then + mixer load /dev/pwm_output1 $MIXER_AUX_FILE + else + tone_alarm $TUNE_ERR + fi + fi +fi \ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 924f6ebc08..31d6a8a2d1 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -116,7 +116,7 @@ then set HIL no set VEHICLE_TYPE none set MIXER none - set MIXER_SEC none + set MIXER_AUX none set OUTPUT_MODE none set PWM_OUT none set PWM_RATE none @@ -482,7 +482,7 @@ then if [ $MIXER == none ] then # Set default mixer for fixed wing if not defined - set MIXER FMU_AERT + set MIXER AERT fi if [ $MAV_TYPE == none ] @@ -518,27 +518,27 @@ then if [ $MAV_TYPE == none ] then # Use mixer to detect vehicle type - if [ $MIXER == FMU_quad_x -o $MIXER == FMU_quad_+ ] + if [ $MIXER == quad_x -o $MIXER == quad_+ ] then set MAV_TYPE 2 fi - if [ $MIXER == FMU_quad_w ] + if [ $MIXER == quad_w ] then set MAV_TYPE 2 fi - if [ $MIXER == FMU_hexa_x -o $MIXER == FMU_hexa_+ ] + if [ $MIXER == hexa_x -o $MIXER == hexa_+ ] then set MAV_TYPE 13 fi - if [ $MIXER == FMU_hexa_cox ] + if [ $MIXER == hexa_cox ] then set MAV_TYPE 13 fi - if [ $MIXER == FMU_octo_x -o $MIXER == FMU_octo_+ ] + if [ $MIXER == octo_x -o $MIXER == octo_+ ] then set MAV_TYPE 14 fi - if [ $MIXER == FMU_octo_cox ] + if [ $MIXER == octo_cox ] then set MAV_TYPE 14 fi @@ -577,11 +577,11 @@ then if [ $MAV_TYPE == none ] then # Use mixer to detect vehicle type - if [ $MIXER == FMU_caipirinha_vtol ] + if [ $MIXER == caipirinha_vtol ] then set MAV_TYPE 19 fi - if [ $MIXER == FMU_firefly6_rotors ] + if [ $MIXER == firefly6_rotors ] then set MAV_TYPE 21 fi diff --git a/ROMFS/px4fmu_common/mixers/FMU_AERT.mix b/ROMFS/px4fmu_common/mixers/AERT.main.mix similarity index 100% rename from ROMFS/px4fmu_common/mixers/FMU_AERT.mix rename to ROMFS/px4fmu_common/mixers/AERT.main.mix diff --git a/ROMFS/px4fmu_common/mixers/FMU_AET.mix b/ROMFS/px4fmu_common/mixers/AET.main.mix similarity index 100% rename from ROMFS/px4fmu_common/mixers/FMU_AET.mix rename to ROMFS/px4fmu_common/mixers/AET.main.mix diff --git a/ROMFS/px4fmu_common/mixers/FMU_CCPM.mix b/ROMFS/px4fmu_common/mixers/CCPM.main.mix similarity index 100% rename from ROMFS/px4fmu_common/mixers/FMU_CCPM.mix rename to ROMFS/px4fmu_common/mixers/CCPM.main.mix diff --git a/ROMFS/px4fmu_common/mixers/FMU_FX79.mix b/ROMFS/px4fmu_common/mixers/FX79.main.mix similarity index 100% rename from ROMFS/px4fmu_common/mixers/FMU_FX79.mix rename to ROMFS/px4fmu_common/mixers/FX79.main.mix diff --git a/ROMFS/px4fmu_common/mixers/FMU_Q.mix b/ROMFS/px4fmu_common/mixers/Q.main.mix similarity index 100% rename from ROMFS/px4fmu_common/mixers/FMU_Q.mix rename to ROMFS/px4fmu_common/mixers/Q.main.mix diff --git a/ROMFS/px4fmu_common/mixers/FMU_RET.mix b/ROMFS/px4fmu_common/mixers/RET.main.mix similarity index 100% rename from ROMFS/px4fmu_common/mixers/FMU_RET.mix rename to ROMFS/px4fmu_common/mixers/RET.main.mix diff --git a/ROMFS/px4fmu_common/mixers/Viper.mix b/ROMFS/px4fmu_common/mixers/Viper.main.mix similarity index 100% rename from ROMFS/px4fmu_common/mixers/Viper.mix rename to ROMFS/px4fmu_common/mixers/Viper.main.mix diff --git a/ROMFS/px4fmu_common/mixers/FMU_X5.mix b/ROMFS/px4fmu_common/mixers/X5.main.mix similarity index 100% rename from ROMFS/px4fmu_common/mixers/FMU_X5.mix rename to ROMFS/px4fmu_common/mixers/X5.main.mix diff --git a/ROMFS/px4fmu_common/mixers/FMU_caipirinha_vtol.mix b/ROMFS/px4fmu_common/mixers/caipirinha_vtol.main.mix similarity index 100% rename from ROMFS/px4fmu_common/mixers/FMU_caipirinha_vtol.mix rename to ROMFS/px4fmu_common/mixers/caipirinha_vtol.main.mix diff --git a/ROMFS/px4fmu_common/mixers/FMU_delta.mix b/ROMFS/px4fmu_common/mixers/delta.main.mix similarity index 100% rename from ROMFS/px4fmu_common/mixers/FMU_delta.mix rename to ROMFS/px4fmu_common/mixers/delta.main.mix diff --git a/ROMFS/px4fmu_common/mixers/easystar.mix b/ROMFS/px4fmu_common/mixers/easystar.main.mix similarity index 100% rename from ROMFS/px4fmu_common/mixers/easystar.mix rename to ROMFS/px4fmu_common/mixers/easystar.main.mix diff --git a/ROMFS/px4fmu_common/mixers/FMU_firefly6_elevons.mix b/ROMFS/px4fmu_common/mixers/firefly6.aux.mix similarity index 100% rename from ROMFS/px4fmu_common/mixers/FMU_firefly6_elevons.mix rename to ROMFS/px4fmu_common/mixers/firefly6.aux.mix diff --git a/ROMFS/px4fmu_common/mixers/FMU_firefly6_rotors.mix b/ROMFS/px4fmu_common/mixers/firefly6.main.mix similarity index 100% rename from ROMFS/px4fmu_common/mixers/FMU_firefly6_rotors.mix rename to ROMFS/px4fmu_common/mixers/firefly6.main.mix diff --git a/ROMFS/px4fmu_common/mixers/FMU_hexa_+.mix b/ROMFS/px4fmu_common/mixers/hexa_+.main.mix similarity index 100% rename from ROMFS/px4fmu_common/mixers/FMU_hexa_+.mix rename to ROMFS/px4fmu_common/mixers/hexa_+.main.mix diff --git a/ROMFS/px4fmu_common/mixers/FMU_hexa_cox.mix b/ROMFS/px4fmu_common/mixers/hexa_cox.main.mix similarity index 100% rename from ROMFS/px4fmu_common/mixers/FMU_hexa_cox.mix rename to ROMFS/px4fmu_common/mixers/hexa_cox.main.mix diff --git a/ROMFS/px4fmu_common/mixers/FMU_hexa_x.mix b/ROMFS/px4fmu_common/mixers/hexa_x.main.mix similarity index 100% rename from ROMFS/px4fmu_common/mixers/FMU_hexa_x.mix rename to ROMFS/px4fmu_common/mixers/hexa_x.main.mix diff --git a/ROMFS/px4fmu_common/mixers/FMU_octo_+.mix b/ROMFS/px4fmu_common/mixers/octo_+.main.mix similarity index 100% rename from ROMFS/px4fmu_common/mixers/FMU_octo_+.mix rename to ROMFS/px4fmu_common/mixers/octo_+.main.mix diff --git a/ROMFS/px4fmu_common/mixers/FMU_octo_cox.mix b/ROMFS/px4fmu_common/mixers/octo_cox.main.mix similarity index 100% rename from ROMFS/px4fmu_common/mixers/FMU_octo_cox.mix rename to ROMFS/px4fmu_common/mixers/octo_cox.main.mix diff --git a/ROMFS/px4fmu_common/mixers/FMU_octo_x.mix b/ROMFS/px4fmu_common/mixers/octo_x.main.mix similarity index 100% rename from ROMFS/px4fmu_common/mixers/FMU_octo_x.mix rename to ROMFS/px4fmu_common/mixers/octo_x.main.mix diff --git a/ROMFS/px4fmu_common/mixers/phantom.mix b/ROMFS/px4fmu_common/mixers/phantom.main.mix similarity index 100% rename from ROMFS/px4fmu_common/mixers/phantom.mix rename to ROMFS/px4fmu_common/mixers/phantom.main.mix diff --git a/ROMFS/px4fmu_common/mixers/FMU_quad_+.mix b/ROMFS/px4fmu_common/mixers/quad_+.main.mix similarity index 100% rename from ROMFS/px4fmu_common/mixers/FMU_quad_+.mix rename to ROMFS/px4fmu_common/mixers/quad_+.main.mix diff --git a/ROMFS/px4fmu_common/mixers/FMU_quad_v.mix b/ROMFS/px4fmu_common/mixers/quad_v.main.mix similarity index 100% rename from ROMFS/px4fmu_common/mixers/FMU_quad_v.mix rename to ROMFS/px4fmu_common/mixers/quad_v.main.mix diff --git a/ROMFS/px4fmu_common/mixers/FMU_quad_w.mix b/ROMFS/px4fmu_common/mixers/quad_w.main.mix similarity index 100% rename from ROMFS/px4fmu_common/mixers/FMU_quad_w.mix rename to ROMFS/px4fmu_common/mixers/quad_w.main.mix diff --git a/ROMFS/px4fmu_common/mixers/FMU_quad_x.mix b/ROMFS/px4fmu_common/mixers/quad_x.main.mix similarity index 100% rename from ROMFS/px4fmu_common/mixers/FMU_quad_x.mix rename to ROMFS/px4fmu_common/mixers/quad_x.main.mix diff --git a/ROMFS/px4fmu_common/mixers/skywalker.mix b/ROMFS/px4fmu_common/mixers/skywalker.main.mix similarity index 100% rename from ROMFS/px4fmu_common/mixers/skywalker.mix rename to ROMFS/px4fmu_common/mixers/skywalker.main.mix diff --git a/ROMFS/px4fmu_common/mixers/wingwing.mix b/ROMFS/px4fmu_common/mixers/wingwing.main.mix similarity index 100% rename from ROMFS/px4fmu_common/mixers/wingwing.mix rename to ROMFS/px4fmu_common/mixers/wingwing.main.mix diff --git a/src/systemcmds/tests/test_mixer.cpp b/src/systemcmds/tests/test_mixer.cpp index 8ab8fa2d6e..2896a8e405 100644 --- a/src/systemcmds/tests/test_mixer.cpp +++ b/src/systemcmds/tests/test_mixer.cpp @@ -343,7 +343,7 @@ int test_mixer(int argc, char *argv[]) if (argc > 2) filename = argv[2]; else - filename = "/etc/mixers/FMU_quad_w.mix"; + filename = "/etc/mixers/quad_w.main.mix"; load_mixer_file(filename, &buf[0], sizeof(buf)); loaded = strlen(buf); diff --git a/unittests/conversion_test.cpp b/unittests/conversion_test.cpp index 99e1c7721d..12d2213e2c 100644 --- a/unittests/conversion_test.cpp +++ b/unittests/conversion_test.cpp @@ -4,6 +4,6 @@ #include "gtest/gtest.h" -TEST(ConversionTest, FMU_quad_w) { +TEST(ConversionTest, quad_w_main) { ASSERT_EQ(test_conv(0, NULL), 0) << "Conversion test failed"; } diff --git a/unittests/mixer_test.cpp b/unittests/mixer_test.cpp index fd3ece7f87..1271dab5e0 100644 --- a/unittests/mixer_test.cpp +++ b/unittests/mixer_test.cpp @@ -6,6 +6,6 @@ TEST(MixerTest, Mixer) { - char* args[] = {"empty", "../ROMFS/px4fmu_common/mixers/IO_pass.mix", "../ROMFS/px4fmu_common/mixers/FMU_quad_w.mix"}; + char* args[] = {"empty", "../ROMFS/px4fmu_common/mixers/IO_pass.mix", "../ROMFS/px4fmu_common/mixers/quad_w.main.mix"}; ASSERT_EQ(test_mixer(3, args), 0) << "IO_pass.mix failed"; } From 7cb1d092780dbf9128b74a148d21012370006e30 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Wed, 7 Jan 2015 17:59:23 -1000 Subject: [PATCH 435/652] Updated submodule and config for SDIO fix --- NuttX | 2 +- nuttx-configs/px4fmu-v2/nsh/defconfig | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/NuttX b/NuttX index 255dc96065..dbcccb2455 160000 --- a/NuttX +++ b/NuttX @@ -1 +1 @@ -Subproject commit 255dc96065ce1fdd7f0f56ca5afb6b6112abfc70 +Subproject commit dbcccb2455d759b789d549d25e1fbf489b2d3c83 diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index f9ab949e17..ac3dbe5daf 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -389,7 +389,7 @@ CONFIG_ARCH_BOARD="" # CONFIG_NSH_MMCSDMINOR=0 CONFIG_NSH_MMCSDSLOTNO=0 - +CONFIG_MMCSD_HAVE_SDIOWAIT_WRCOMPLETE=y # # Board-Specific Options # From a4db73dad894e7b6f6920fcd1184127f9c997201 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 7 Jan 2015 23:15:12 -0500 Subject: [PATCH 436/652] move st24_test to gtest --- unittests/CMakeLists.txt | 2 -- unittests/st24_test.cpp | 31 +++++-------------------------- 2 files changed, 5 insertions(+), 28 deletions(-) diff --git a/unittests/CMakeLists.txt b/unittests/CMakeLists.txt index c2990c59f2..247ddbb3d2 100644 --- a/unittests/CMakeLists.txt +++ b/unittests/CMakeLists.txt @@ -61,12 +61,10 @@ add_executable(conversion_test conversion_test.cpp ${PX_SRC}/systemcmds/tests/te add_gtest(conversion_test) # sbus2_test -# TODO: move to gtest add_executable(sbus2_test sbus2_test.cpp hrt.cpp) add_gtest(sbus2_test) # st24_test -# TODO: move to gtest add_executable(st24_test st24_test.cpp hrt.cpp ${PX_SRC}/lib/rc/st24.c) add_gtest(st24_test) diff --git a/unittests/st24_test.cpp b/unittests/st24_test.cpp index 0c56df1732..89c7ffb1c3 100644 --- a/unittests/st24_test.cpp +++ b/unittests/st24_test.cpp @@ -1,4 +1,3 @@ - #include #include #include @@ -7,30 +6,17 @@ #include #include "../../src/systemcmds/tests/tests.h" -int main(int argc, char *argv[]) -{ - warnx("ST24 test started"); +#include "gtest/gtest.h" - char* defaultfile = "testdata/st24_data.txt"; - - char* filepath = 0; - - if (argc < 2) { - warnx("Too few arguments. Using default file: %s", defaultfile); - filepath = defaultfile; - } else { - filepath = argv[1]; - } +TEST(ST24Test, ST24) { + const char* filepath = "testdata/st24_data.txt"; warnx("loading data from: %s", filepath); FILE *fp; fp = fopen(filepath, "rt"); - - if (!fp) { - errx(1, "failed opening file"); - } + //ASSERT_TRUE(fp); float f; unsigned x; @@ -73,12 +59,5 @@ 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; + ASSERT_EQ(EOF, ret); } From cc2d0f00d6826d050c1f9e852970dd409946ee0d Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 7 Jan 2015 23:17:31 -0500 Subject: [PATCH 437/652] move sf0x_test to gtest --- unittests/CMakeLists.txt | 1 - unittests/sf0x_test.cpp | 20 +++++++------------- 2 files changed, 7 insertions(+), 14 deletions(-) diff --git a/unittests/CMakeLists.txt b/unittests/CMakeLists.txt index 247ddbb3d2..666570a71e 100644 --- a/unittests/CMakeLists.txt +++ b/unittests/CMakeLists.txt @@ -69,6 +69,5 @@ add_executable(st24_test st24_test.cpp hrt.cpp ${PX_SRC}/lib/rc/st24.c) add_gtest(st24_test) # sf0x_test -# TODO: move to gtest add_executable(sf0x_test sf0x_test.cpp ${PX_SRC}/drivers/sf0x/sf0x_parser.cpp) add_gtest(sf0x_test) diff --git a/unittests/sf0x_test.cpp b/unittests/sf0x_test.cpp index 1af128bbe4..ffaaaf04aa 100644 --- a/unittests/sf0x_test.cpp +++ b/unittests/sf0x_test.cpp @@ -1,19 +1,15 @@ - -#include -#include -#include #include -#include +#include +#include +#include + #include - #include +#include -int main(int argc, char *argv[]) -{ - warnx("SF0X test started"); - - int ret = 0; +#include "gtest/gtest.h" +TEST(SF0XTest, SF0X) { const char LINE_MAX = 20; char _linebuf[LINE_MAX]; _linebuf[0] = '\0'; @@ -60,6 +56,4 @@ int main(int argc, char *argv[]) } warnx("test finished"); - - return ret; } From 25054d389a0d860664e2c3a67c180f8518876cbe Mon Sep 17 00:00:00 2001 From: Anton Matosov Date: Thu, 8 Jan 2015 12:55:28 +0200 Subject: [PATCH 438/652] Broke down the elif into else and if as nsh doesn't support elif Fix invalid set of the variable --- ROMFS/px4fmu_common/init.d/rc.interface | 80 +++++++++++++------------ 1 file changed, 43 insertions(+), 37 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.interface b/ROMFS/px4fmu_common/init.d/rc.interface index d4880ea420..1a2d8c0163 100644 --- a/ROMFS/px4fmu_common/init.d/rc.interface +++ b/ROMFS/px4fmu_common/init.d/rc.interface @@ -4,29 +4,32 @@ # set SDCARD_MIXERS_PATH /fs/microsd/etc/mixers - + if [ $MIXER != none -a $MIXER != skip ] then # # Load main mixer # - - if [ $MIXER_AUX == none ] - then - MIXER_AUX = $MIXER.aux - fi + + if [ $MIXER_AUX == none ] + then + set MIXER_AUX $MIXER.aux + fi # Use the mixer file from the SD-card if it exists if [ -f $SDCARD_MIXERS_PATH/$MIXER.main.mix ] then set MIXER_FILE $SDCARD_MIXERS_PATH/$MIXER.main.mix # Try out the old convention, for backward compatibility - elif [ -f $SDCARD_MIXERS_PATH/$MIXER.mix ] - then - set MIXER_FILE $SDCARD_MIXERS_PATH/$MIXER.mix - else - set MIXER_FILE /etc/mixers/$MIXER.main.mix - fi + else + + if [ -f $SDCARD_MIXERS_PATH/$MIXER.mix ] + then + set MIXER_FILE $SDCARD_MIXERS_PATH/$MIXER.mix + else + set MIXER_FILE /etc/mixers/$MIXER.main.mix + fi + fi if [ $OUTPUT_MODE == mkblctrl ] then @@ -34,7 +37,7 @@ then else set OUTPUT_DEV /dev/pwm_output fi - + if [ $OUTPUT_MODE == uavcan_esc ] then set OUTPUT_DEV /dev/uavcan/esc @@ -91,31 +94,34 @@ then pwm failsafe -d $OUTPUT_DEV $FAILSAFE fi fi - + # check if should load secondary mixer if [ $MIXER_AUX != none ] then - # - # Load aux mixer - # - - set MIXER_AUX_FILE none - - if [ -f $SDCARD_MIXERS_PATH/$MIXER_AUX.mix ] - then - set MIXER_AUX_FILE $SDCARD_MIXERS_PATH/$MIXER_AUX.mix - elif [ -f /etc/mixers/$MIXER_AUX.mix ] - then - set MIXER_AUX_FILE /etc/mixers/$MIXER_AUX.mix - fi - - if [ $MIXER_AUX_FILE != none -a $FMU_MODE == pwm ] - then - if fmu mode_pwm - then - mixer load /dev/pwm_output1 $MIXER_AUX_FILE - else - tone_alarm $TUNE_ERR - fi - fi + # + # Load aux mixer + # + + set MIXER_AUX_FILE none + + if [ -f $SDCARD_MIXERS_PATH/$MIXER_AUX.mix ] + then + set MIXER_AUX_FILE $SDCARD_MIXERS_PATH/$MIXER_AUX.mix + else + + if [ -f /etc/mixers/$MIXER_AUX.mix ] + then + set MIXER_AUX_FILE /etc/mixers/$MIXER_AUX.mix + fi + fi + + if [ $MIXER_AUX_FILE != none -a $FMU_MODE == pwm ] + then + if fmu mode_pwm + then + mixer load /dev/pwm_output1 $MIXER_AUX_FILE + else + tone_alarm $TUNE_ERR + fi + fi fi \ No newline at end of file From 1df7939072f87a5f3f3cc05ff57f545db7c144ad Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Thu, 8 Jan 2015 10:55:01 +0100 Subject: [PATCH 439/652] Remove bottle_drop as it should be optional. --- ROMFS/px4fmu_common/init.d/rc.fw_apps | 2 -- 1 file changed, 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_apps b/ROMFS/px4fmu_common/init.d/rc.fw_apps index c97b3477f8..9aca3fc5f4 100644 --- a/ROMFS/px4fmu_common/init.d/rc.fw_apps +++ b/ROMFS/px4fmu_common/init.d/rc.fw_apps @@ -13,5 +13,3 @@ ekf_att_pos_estimator start # fw_att_control start fw_pos_control_l1 start - -bottle_drop start From e405fafe882c95345ccf88ea5f372f353aa4c21f Mon Sep 17 00:00:00 2001 From: Anton Matosov Date: Thu, 8 Jan 2015 13:35:02 +0200 Subject: [PATCH 440/652] Applied code review comments (newline at end of file, whitespaces in empty lines) --- ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil | 2 +- ROMFS/px4fmu_common/init.d/2101_hk_bixler | 2 +- ROMFS/px4fmu_common/init.d/rc.interface | 26 +++++++++---------- 3 files changed, 15 insertions(+), 15 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil b/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil index 2b418dfc50..7e651216d0 100644 --- a/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil +++ b/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil @@ -9,4 +9,4 @@ sh /etc/init.d/rc.mc_defaults set MIXER quad_+ -set HIL yes \ No newline at end of file +set HIL yes diff --git a/ROMFS/px4fmu_common/init.d/2101_hk_bixler b/ROMFS/px4fmu_common/init.d/2101_hk_bixler index 0e6d2481c0..05ee57ffa0 100644 --- a/ROMFS/px4fmu_common/init.d/2101_hk_bixler +++ b/ROMFS/px4fmu_common/init.d/2101_hk_bixler @@ -2,4 +2,4 @@ sh /etc/init.d/rc.fw_defaults -set MIXER AERT \ No newline at end of file +set MIXER AERT diff --git a/ROMFS/px4fmu_common/init.d/rc.interface b/ROMFS/px4fmu_common/init.d/rc.interface index 1a2d8c0163..4015a911b2 100644 --- a/ROMFS/px4fmu_common/init.d/rc.interface +++ b/ROMFS/px4fmu_common/init.d/rc.interface @@ -4,25 +4,25 @@ # set SDCARD_MIXERS_PATH /fs/microsd/etc/mixers - + if [ $MIXER != none -a $MIXER != skip ] then # # Load main mixer # - + if [ $MIXER_AUX == none ] then set MIXER_AUX $MIXER.aux fi - + # Use the mixer file from the SD-card if it exists if [ -f $SDCARD_MIXERS_PATH/$MIXER.main.mix ] then set MIXER_FILE $SDCARD_MIXERS_PATH/$MIXER.main.mix # Try out the old convention, for backward compatibility else - + if [ -f $SDCARD_MIXERS_PATH/$MIXER.mix ] then set MIXER_FILE $SDCARD_MIXERS_PATH/$MIXER.mix @@ -30,19 +30,19 @@ then set MIXER_FILE /etc/mixers/$MIXER.main.mix fi fi - + if [ $OUTPUT_MODE == mkblctrl ] then set OUTPUT_DEV /dev/mkblctrl else set OUTPUT_DEV /dev/pwm_output fi - + if [ $OUTPUT_MODE == uavcan_esc ] then set OUTPUT_DEV /dev/uavcan/esc fi - + if mixer load $OUTPUT_DEV $MIXER_FILE then echo "[i] Mixer: $MIXER_FILE" @@ -71,7 +71,7 @@ then then pwm rate -c $PWM_OUT -r $PWM_RATE fi - + # # Set disarmed, min and max PWM values # @@ -101,20 +101,20 @@ then # # Load aux mixer # - + set MIXER_AUX_FILE none - + if [ -f $SDCARD_MIXERS_PATH/$MIXER_AUX.mix ] then set MIXER_AUX_FILE $SDCARD_MIXERS_PATH/$MIXER_AUX.mix else - + if [ -f /etc/mixers/$MIXER_AUX.mix ] then set MIXER_AUX_FILE /etc/mixers/$MIXER_AUX.mix fi fi - + if [ $MIXER_AUX_FILE != none -a $FMU_MODE == pwm ] then if fmu mode_pwm @@ -124,4 +124,4 @@ then tone_alarm $TUNE_ERR fi fi -fi \ No newline at end of file +fi From 7d528330d570fa4900b1a24c2a1f851c3a7dcba0 Mon Sep 17 00:00:00 2001 From: Anton Matosov Date: Tue, 6 Jan 2015 00:57:54 +0200 Subject: [PATCH 441/652] Implemented SK450 DeadCat frame support Implemented the way to specify motor output scale which is required for SK450 DeadCat as it has asymetrical arms (front arms are longer than back ones) --- .../px4fmu_common/init.d/10019_sk450_deadcat | 32 +++++++++++++++++++ ROMFS/px4fmu_common/init.d/rc.autostart | 5 +++ .../px4fmu_common/mixers/FMU_quad_deadcat.mix | 25 +++++++++++++++ src/modules/systemlib/mixer/mixer.h | 3 +- .../systemlib/mixer/mixer_multirotor.cpp | 9 +++++- src/modules/systemlib/mixer/multi_tables.py | 21 +++++++++--- 6 files changed, 89 insertions(+), 6 deletions(-) create mode 100644 ROMFS/px4fmu_common/init.d/10019_sk450_deadcat create mode 100644 ROMFS/px4fmu_common/mixers/FMU_quad_deadcat.mix diff --git a/ROMFS/px4fmu_common/init.d/10019_sk450_deadcat b/ROMFS/px4fmu_common/init.d/10019_sk450_deadcat new file mode 100644 index 0000000000..96463d5ef5 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/10019_sk450_deadcat @@ -0,0 +1,32 @@ +#!nsh +# +# HobbyKing SK450 DeadCat modification +# +# Anton Matosov +# + +sh /etc/init.d/rc.mc_defaults + +if [ $AUTOCNF == yes ] +then + param set MC_ROLL_P 6.0 + param set MC_ROLLRATE_P 0.04 + param set MC_ROLLRATE_I 0.1 + param set MC_ROLLRATE_D 0.0015 + + param set MC_PITCH_P 6.0 + param set MC_PITCHRATE_P 0.08 + param set MC_PITCHRATE_I 0.2 + param set MC_PITCHRATE_D 0.0015 + + param set MC_YAW_P 2.8 + param set MC_YAWRATE_P 0.1 + param set MC_YAWRATE_I 0.07 + param set MC_YAWRATE_D 0.0 +fi + +set VEHICLE_TYPE mc +set MIXER FMU_quad_deadcat + +set PWM_OUTPUTS 1234 +set PWM_MIN 1075 diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart b/ROMFS/px4fmu_common/init.d/rc.autostart index 20f2be0d9e..b83687fbd4 100644 --- a/ROMFS/px4fmu_common/init.d/rc.autostart +++ b/ROMFS/px4fmu_common/init.d/rc.autostart @@ -226,6 +226,11 @@ then sh /etc/init.d/10018_tbs_endurance fi +if param compare SYS_AUTOSTART 10019 +then + sh /etc/init.d/10019_sk450_deadcat +fi + # # Hexa Coaxial # diff --git a/ROMFS/px4fmu_common/mixers/FMU_quad_deadcat.mix b/ROMFS/px4fmu_common/mixers/FMU_quad_deadcat.mix new file mode 100644 index 0000000000..a8c5b716d6 --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/FMU_quad_deadcat.mix @@ -0,0 +1,25 @@ +Multirotor mixer for PX4FMU +=========================== + +This file defines a single mixer for a quadrotor in SK450 DeadCat configuration. All controls are mixed 100%. + +R: 4dc 10000 10000 10000 0 + +Gimbal / payload mixer for last four channels +----------------------------------------------------- + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 4 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 5 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 6 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 7 10000 10000 0 -10000 10000 diff --git a/src/modules/systemlib/mixer/mixer.h b/src/modules/systemlib/mixer/mixer.h index 864ce21a58..67ef521b4e 100644 --- a/src/modules/systemlib/mixer/mixer.h +++ b/src/modules/systemlib/mixer/mixer.h @@ -441,7 +441,6 @@ private: SimpleMixer operator=(const SimpleMixer&); }; - /** * Supported multirotor geometries. * @@ -460,12 +459,14 @@ class __EXPORT MultirotorMixer : public Mixer { public: /** + * Precalculated rotor mix. */ struct Rotor { float roll_scale; /**< scales roll for this rotor */ float pitch_scale; /**< scales pitch for this rotor */ float yaw_scale; /**< scales yaw for this rotor */ + float out_scale; /**< scales total out for this rotor */ }; /** diff --git a/src/modules/systemlib/mixer/mixer_multirotor.cpp b/src/modules/systemlib/mixer/mixer_multirotor.cpp index 5cfbe47f07..2ab5b5e8e4 100644 --- a/src/modules/systemlib/mixer/mixer_multirotor.cpp +++ b/src/modules/systemlib/mixer/mixer_multirotor.cpp @@ -75,7 +75,8 @@ float constrain(float val, float min, float max) { return (val < min) ? min : ((val > max) ? max : val); } -} + +} // anonymous namespace MultirotorMixer::MultirotorMixer(ControlCallback control_cb, uintptr_t cb_handle, @@ -89,6 +90,7 @@ MultirotorMixer::MultirotorMixer(ControlCallback control_cb, _pitch_scale(pitch_scale), _yaw_scale(yaw_scale), _idle_speed(-1.0f + idle_speed * 2.0f), /* shift to output range here to avoid runtime calculation */ + _limits_pub(), _rotor_count(_config_rotor_count[(MultirotorGeometryUnderlyingType)geometry]), _rotors(_config_index[(MultirotorGeometryUnderlyingType)geometry]) { @@ -152,6 +154,9 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl } else if (!strcmp(geomname, "4w")) { geometry = MultirotorGeometry::QUAD_WIDE; + } else if (!strcmp(geomname, "4dc")) { + geometry = MultirotorGeometry::QUAD_DEADCAT; + } else if (!strcmp(geomname, "6+")) { geometry = MultirotorGeometry::HEX_PLUS; @@ -212,6 +217,8 @@ MultirotorMixer::mix(float *outputs, unsigned space) pitch * _rotors[i].pitch_scale + thrust; + out *= _rotors[i].out_scale; + /* limit yaw if it causes outputs clipping */ if (out >= 0.0f && out < -yaw * _rotors[i].yaw_scale) { yaw = -out / _rotors[i].yaw_scale; diff --git a/src/modules/systemlib/mixer/multi_tables.py b/src/modules/systemlib/mixer/multi_tables.py index 21c0affd95..ba59e0536d 100755 --- a/src/modules/systemlib/mixer/multi_tables.py +++ b/src/modules/systemlib/mixer/multi_tables.py @@ -69,6 +69,13 @@ quad_plus = [ [ 180, CW], ] +quad_deadcat = [ + [ 63, CCW, 1.0], + [-135, CCW, 0.964], + [ -63, CW, 1.0], + [ 135, CW, 0.964], +] + quad_v = [ [ 18.8, 0.4242], [ -18.8, 1.0], @@ -148,13 +155,18 @@ twin_engine = [ [-90, 0.0], ] + +tables = [quad_x, quad_plus, quad_v, quad_wide, quad_deadcat, hex_x, hex_plus, hex_cox, octa_x, octa_plus, octa_cox, twin_engine] + def variableName(variable): for variableName, value in list(globals().items()): if value is variable: return variableName -tables = [quad_x, quad_plus, quad_v, quad_wide, hex_x, hex_plus, hex_cox, octa_x, octa_plus, octa_cox, twin_engine] - +def unpackScales(scalesList): + if len(scalesList) == 2: + scalesList += [1.0] #Add thrust scale + return scalesList def printEnum(): print("enum class MultirotorGeometry : MultirotorGeometryUnderlyingType {") @@ -167,10 +179,11 @@ def printEnum(): def printScaleTables(): for table in tables: print("const MultirotorMixer::Rotor _config_{}[] = {{".format(variableName(table))) - for (angle, yawScale) in table: + for row in table: + angle, yawScale, thrustScale = unpackScales(row) rollScale = rcos(angle + 90) pitchScale = rcos(angle) - print("\t{{ {:9f}, {:9f}, {:9f} }},".format(rollScale, pitchScale, yawScale)) + print("\t{{ {:9f}, {:9f}, {:9f}, {:9f} }},".format(rollScale, pitchScale, yawScale, thrustScale)) print("};\n") def printScaleTablesIndex(): From 2776a705f46434e4cbbaa88f28e04078ad5ff558 Mon Sep 17 00:00:00 2001 From: Anton Matosov Date: Tue, 6 Jan 2015 19:30:45 +0200 Subject: [PATCH 442/652] Adopted SK540 config to the latest changes in the init scripts Lowered the pwm min --- ROMFS/px4fmu_common/init.d/10019_sk450_deadcat | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/10019_sk450_deadcat b/ROMFS/px4fmu_common/init.d/10019_sk450_deadcat index 96463d5ef5..d42721adaa 100644 --- a/ROMFS/px4fmu_common/init.d/10019_sk450_deadcat +++ b/ROMFS/px4fmu_common/init.d/10019_sk450_deadcat @@ -25,8 +25,7 @@ then param set MC_YAWRATE_D 0.0 fi -set VEHICLE_TYPE mc set MIXER FMU_quad_deadcat -set PWM_OUTPUTS 1234 -set PWM_MIN 1075 +set PWM_OUT 1234 +set PWM_MIN 1050 From 8624e2a77582b400206042918352418b4892790f Mon Sep 17 00:00:00 2001 From: Anton Matosov Date: Wed, 7 Jan 2015 00:42:35 +0200 Subject: [PATCH 443/652] Fixed naming of the mixer Added mixer check to set MAV_TYPE correctly --- ROMFS/px4fmu_common/init.d/10019_sk450_deadcat | 2 +- ROMFS/px4fmu_common/init.d/rcS | 2 +- .../mixers/{FMU_quad_deadcat.mix => sk450_deadcat.mix} | 0 3 files changed, 2 insertions(+), 2 deletions(-) rename ROMFS/px4fmu_common/mixers/{FMU_quad_deadcat.mix => sk450_deadcat.mix} (100%) diff --git a/ROMFS/px4fmu_common/init.d/10019_sk450_deadcat b/ROMFS/px4fmu_common/init.d/10019_sk450_deadcat index d42721adaa..e68f57f252 100644 --- a/ROMFS/px4fmu_common/init.d/10019_sk450_deadcat +++ b/ROMFS/px4fmu_common/init.d/10019_sk450_deadcat @@ -25,7 +25,7 @@ then param set MC_YAWRATE_D 0.0 fi -set MIXER FMU_quad_deadcat +set MIXER sk450_deadcat set PWM_OUT 1234 set PWM_MIN 1050 diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 31d6a8a2d1..2d9f56a40b 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -522,7 +522,7 @@ then then set MAV_TYPE 2 fi - if [ $MIXER == quad_w ] + if [ $MIXER == quad_w -o $MIXER == sk450_deadcat ] then set MAV_TYPE 2 fi diff --git a/ROMFS/px4fmu_common/mixers/FMU_quad_deadcat.mix b/ROMFS/px4fmu_common/mixers/sk450_deadcat.mix similarity index 100% rename from ROMFS/px4fmu_common/mixers/FMU_quad_deadcat.mix rename to ROMFS/px4fmu_common/mixers/sk450_deadcat.mix From ba89c9eae26ba990317ff0b66d42e114b8c7dffa Mon Sep 17 00:00:00 2001 From: Anton Matosov Date: Wed, 7 Jan 2015 00:39:22 +0200 Subject: [PATCH 444/652] Adopted sk450_deadcat mixer file name --- .../mixers/{sk450_deadcat.mix => sk450_deadcat.main.mix} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename ROMFS/px4fmu_common/mixers/{sk450_deadcat.mix => sk450_deadcat.main.mix} (100%) diff --git a/ROMFS/px4fmu_common/mixers/sk450_deadcat.mix b/ROMFS/px4fmu_common/mixers/sk450_deadcat.main.mix similarity index 100% rename from ROMFS/px4fmu_common/mixers/sk450_deadcat.mix rename to ROMFS/px4fmu_common/mixers/sk450_deadcat.main.mix From 2aa7b3038a64269ec5417597a445f4a6992ae60d Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Thu, 8 Jan 2015 13:18:50 +0100 Subject: [PATCH 445/652] px_uploader: Added progress bar for erasing and uploading firmware --- Tools/px_uploader.py | 36 ++++++++++++++++++++++++++++++++++-- 1 file changed, 34 insertions(+), 2 deletions(-) diff --git a/Tools/px_uploader.py b/Tools/px_uploader.py index 3a4540ac06..e65e7342b0 100755 --- a/Tools/px_uploader.py +++ b/Tools/px_uploader.py @@ -261,6 +261,16 @@ class uploader(object): self.__getSync() return value + def __drawProgressBar(self, progress, maxVal): + if maxVal < progress: + progress = maxVal + + percent = (float(progress) / float(maxVal)) * 100.0 + + sys.stdout.write("\rprogress:[%-20s] %.2f%%" % ('='*int(percent/5.0), percent)) + sys.stdout.flush() + + # send the CHIP_ERASE command and wait for the bootloader to become ready def __erase(self): self.__send(uploader.CHIP_ERASE @@ -268,8 +278,19 @@ class uploader(object): # erase is very slow, give it 20s deadline = time.time() + 20 while time.time() < deadline: + + #Draw progress bar (erase usually takes about 9 seconds to complete) + estimatedTimeRemaining = deadline-time.time()-11.0 + if estimatedTimeRemaining > 0: + self.__drawProgressBar(10.0-estimatedTimeRemaining, 10.0) + else: + self.__drawProgressBar(10.0, 10.0) + sys.stdout.write(" (timeout: %d seconds) " % int(time.time()-deadline) ) + try: self.__getSync() + self.__drawProgressBar(10.0, 10.0) + sys.stdout.write("\nerase complete!\n") return except RuntimeError: # we timed out, that's OK @@ -329,9 +350,18 @@ class uploader(object): def __program(self, fw): code = fw.image groups = self.__split_len(code, uploader.PROG_MULTI_MAX) + + uploadProgress = 0 for bytes in groups: self.__program_multi(bytes) + #Print upload progress (throttled, so it does not delay upload progress) + uploadProgress += 1 + if uploadProgress % 256 == 0: + self.__drawProgressBar(uploadProgress, len(groups)) + self.__drawProgressBar(100, 100) + print("\nprogram complete!") + # verify code def __verify_v2(self, fw): self.__send(uploader.CHIP_VERIFY @@ -478,7 +508,7 @@ while True: for port in portlist: #print("Trying %s" % port) - + # create an uploader attached to the port try: if "linux" in _platform: @@ -511,8 +541,10 @@ while True: print("attempting reboot on %s..." % port) print("if the board does not respond, unplug and re-plug the USB connector.") up.send_reboot() + # wait for the reboot, without we might run into Serial I/O Error 5 time.sleep(0.5) + # always close the port up.close() continue @@ -524,7 +556,7 @@ while True: except RuntimeError as ex: # print the error - print("ERROR: %s" % ex.args) + print("\nERROR: %s" % ex.args) finally: # always close the port From 1eda1f816bdd4877fee79abb4f0edb13bbc02a91 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Thu, 8 Jan 2015 16:30:47 +0100 Subject: [PATCH 446/652] px_uploader: Minor code cleanup --- Tools/px_uploader.py | 51 ++++++++++++++++++++++---------------------- 1 file changed, 26 insertions(+), 25 deletions(-) diff --git a/Tools/px_uploader.py b/Tools/px_uploader.py index e65e7342b0..2720806199 100755 --- a/Tools/px_uploader.py +++ b/Tools/px_uploader.py @@ -1,7 +1,7 @@ #!/usr/bin/env python ############################################################################ # -# Copyright (C) 2012, 2013 PX4 Development Team. All rights reserved. +# Copyright (C) 2012-2015 PX4 Development Team. All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions @@ -227,16 +227,21 @@ class uploader(object): + uploader.EOC) self.__getSync() -# def __trySync(self): -# c = self.__recv() -# if (c != self.INSYNC): -# #print("unexpected 0x%x instead of INSYNC" % ord(c)) -# return False; -# c = self.__recv() -# if (c != self.OK): -# #print("unexpected 0x%x instead of OK" % ord(c)) -# return False -# return True + def __trySync(self): + try: + self.port.flush() + if (self.__recv() != self.INSYNC): + #print("unexpected 0x%x instead of INSYNC" % ord(c)) + return False; + + if (self.__recv() != self.OK): + #print("unexpected 0x%x instead of OK" % ord(c)) + return False + return True + + except RuntimeError: + #timeout, no response yet + return False # send the GET_DEVICE command and wait for an info parameter def __getInfo(self, param): @@ -275,26 +280,23 @@ class uploader(object): def __erase(self): self.__send(uploader.CHIP_ERASE + uploader.EOC) + # erase is very slow, give it 20s - deadline = time.time() + 20 + deadline = time.time() + 20.0 while time.time() < deadline: #Draw progress bar (erase usually takes about 9 seconds to complete) - estimatedTimeRemaining = deadline-time.time()-11.0 + estimatedTimeRemaining = deadline-time.time() if estimatedTimeRemaining > 0: - self.__drawProgressBar(10.0-estimatedTimeRemaining, 10.0) + self.__drawProgressBar(20.0-estimatedTimeRemaining, 9.0) else: self.__drawProgressBar(10.0, 10.0) sys.stdout.write(" (timeout: %d seconds) " % int(time.time()-deadline) ) - try: - self.__getSync() - self.__drawProgressBar(10.0, 10.0) - sys.stdout.write("\nerase complete!\n") - return - except RuntimeError: - # we timed out, that's OK - continue + if self.__trySync(): + self.__drawProgressBar(10.0, 10.0) + sys.stdout.write("\nerase complete!\n") + return; raise RuntimeError("timed out waiting for erase") @@ -464,8 +466,7 @@ class uploader(object): self.__send(uploader.MAVLINK_REBOOT_ID0) except: return - - + # Detect python version if sys.version_info[0] < 3: @@ -508,7 +509,7 @@ while True: for port in portlist: #print("Trying %s" % port) - + # create an uploader attached to the port try: if "linux" in _platform: From 450d8ee45289d6a820808c54bc6fa8fd30faa57b Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 8 Jan 2015 15:42:25 +0100 Subject: [PATCH 447/652] remove/unset startup script variables --- ROMFS/px4fmu_common/init.d/rc.interface | 1 + ROMFS/px4fmu_common/init.d/rcS | 10 ++++++++-- 2 files changed, 9 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.interface b/ROMFS/px4fmu_common/init.d/rc.interface index 4015a911b2..7fde482df7 100644 --- a/ROMFS/px4fmu_common/init.d/rc.interface +++ b/ROMFS/px4fmu_common/init.d/rc.interface @@ -125,3 +125,4 @@ then fi fi fi +unset OUTPUT_DEV diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 31d6a8a2d1..84c7e096d7 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -12,7 +12,6 @@ set MODE autostart set FRC /fs/microsd/etc/rc.txt set FCONFIG /fs/microsd/etc/config.txt -set FEXTRAS /fs/microsd/etc/extras.txt set TUNE_ERR ML< Date: Fri, 9 Jan 2015 07:09:58 +0100 Subject: [PATCH 448/652] Temporarily disable HoTT support --- 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 fa5dff93c2..577140f054 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -32,9 +32,9 @@ MODULES += drivers/ll40ls # MODULES += drivers/trone MODULES += drivers/gps MODULES += drivers/hil -MODULES += drivers/hott -MODULES += drivers/hott/hott_telemetry -MODULES += drivers/hott/hott_sensors +# MODULES += drivers/hott +# MODULES += drivers/hott/hott_telemetry +# MODULES += drivers/hott/hott_sensors # MODULES += drivers/blinkm MODULES += drivers/airspeed MODULES += drivers/ets_airspeed From d1e060650069ad722629ee663bf011d2571e3a4d Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 9 Jan 2015 07:56:20 +0100 Subject: [PATCH 449/652] unset MODE and AUTOCNF --- ROMFS/px4fmu_common/init.d/rcS | 2 ++ 1 file changed, 2 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 84c7e096d7..5bc5b4cf5d 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -164,6 +164,7 @@ then else sh /etc/init.d/rc.autostart fi + unset MODE # # Override parameters from user configuration file @@ -185,6 +186,7 @@ then param set SYS_AUTOCONFIG 0 param save fi + unset AUTOCNF set IO_PRESENT no From eab9ddb21273234fce7c610f664a35d2dae64a55 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 9 Jan 2015 09:08:51 +0100 Subject: [PATCH 450/652] ROMFS: Set unneeded string during startup --- ROMFS/px4fmu_common/init.d/rcS | 1 + 1 file changed, 1 insertion(+) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 36a5124415..2c9387ff06 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -230,6 +230,7 @@ then tone_alarm $TUNE_ERR fi fi + unset IO_FILE if [ $IO_PRESENT == no ] then From 69a7e310774f1df820303346143e5128856723fb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 9 Jan 2015 09:09:11 +0100 Subject: [PATCH 451/652] Att EKF: Adjust stack size to larger requirement --- .../attitude_estimator_ekf/attitude_estimator_ekf_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 702894d600..e61204e6c6 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -133,7 +133,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, - 7200, + 7700, attitude_estimator_ekf_thread_main, (argv) ? (char * const *)&argv[2] : (char * const *)NULL); exit(0); From bb1e082781b7839c5642e35dcecc1de74286858d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 9 Jan 2015 09:09:24 +0100 Subject: [PATCH 452/652] Commander: be less chatty --- src/modules/commander/commander.cpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 086f291f68..ee20f57a1a 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -739,9 +739,6 @@ int commander_thread_main(int argc, char *argv[]) param_t _param_ef_time_thres = param_find("COM_EF_TIME"); param_t _param_autostart_id = param_find("SYS_AUTOSTART"); - /* welcome user */ - warnx("starting"); - const char *main_states_str[MAIN_STATE_MAX]; main_states_str[MAIN_STATE_MANUAL] = "MANUAL"; main_states_str[MAIN_STATE_ALTCTL] = "ALTCTL"; From d351772a4695238672611070ab2ecb4628aff993 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 9 Jan 2015 09:09:40 +0100 Subject: [PATCH 453/652] mavlink app: Be less chatty on startup --- src/modules/mavlink/mavlink_main.cpp | 27 +++---------------------- src/modules/mavlink/mavlink_mission.cpp | 4 +--- 2 files changed, 4 insertions(+), 27 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index dc031404de..9e4ab00df2 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1286,30 +1286,11 @@ Mavlink::task_main(int argc, char *argv[]) } if (Mavlink::instance_exists(_device_name, this)) { - warnx("mavlink instance for %s already running", _device_name); + warnx("%s already running", _device_name); return ERROR; } - /* inform about mode */ - switch (_mode) { - case MAVLINK_MODE_NORMAL: - warnx("mode: NORMAL"); - break; - - case MAVLINK_MODE_CUSTOM: - warnx("mode: CUSTOM"); - break; - - case MAVLINK_MODE_ONBOARD: - warnx("mode: ONBOARD"); - break; - - default: - warnx("ERROR: Unknown mode"); - break; - } - - warnx("data rate: %d Bytes/s, port: %s, baud: %d", _datarate, _device_name, _baudrate); + warnx("mode: %u, data rate: %d B/s on %s @ %dB", _mode, _datarate, _device_name, _baudrate); /* flush stdout in case MAVLink is about to take it over */ fflush(stdout); @@ -1337,7 +1318,7 @@ Mavlink::task_main(int argc, char *argv[]) * marker ring buffer approach. */ if (OK != message_buffer_init(2 * sizeof(mavlink_message_t) + 1)) { - errx(1, "can't allocate message buffer, exiting"); + errx(1, "msg buf:"); } /* initialize message buffer mutex */ @@ -1571,8 +1552,6 @@ Mavlink::task_main(int argc, char *argv[]) _subscriptions = nullptr; - warnx("waiting for UART receive thread"); - /* wait for threads to complete */ pthread_join(_receive_thread, NULL); diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index 859d380fe5..442d36dfba 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -120,13 +120,11 @@ MavlinkMissionManager::init_offboard_mission() _count = mission_state.count; _current_seq = mission_state.current_seq; - warnx("offboard mission init: dataman_id=%d, count=%u, current_seq=%d", _dataman_id, _count, _current_seq); - } else { _dataman_id = 0; _count = 0; _current_seq = 0; - warnx("offboard mission init: ERROR, reading mission state failed"); + warnx("offboard mission init: ERROR"); } } From 54a22aed94c9fb23cd5bfcb116b557bfcb4bec9d Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 9 Jan 2015 17:23:32 +1100 Subject: [PATCH 454/652] hmc5883: fixed handling of 3 bus options use a table of possible bus options. This prevents us starting two drivers on the same bus --- src/drivers/hmc5883/hmc5883.cpp | 309 +++++++++++++------------------- src/drivers/hmc5883/hmc5883.h | 1 + 2 files changed, 123 insertions(+), 187 deletions(-) diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index fe70bd37fe..2a10b00632 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -80,9 +80,6 @@ * HMC5883 internal constants and data structures. */ -#define HMC5883L_DEVICE_PATH_INT "/dev/hmc5883_int" -#define HMC5883L_DEVICE_PATH_EXT "/dev/hmc5883_ext" - /* Max measurement rate is 160Hz, however with 160 it will be set to 166 Hz, therefore workaround using 150 */ #define HMC5883_CONVERSION_INTERVAL (1000000 / 150) /* microseconds */ @@ -114,9 +111,10 @@ #define STATUS_REG_DATA_READY (1 << 0) /* page 16: set if all axes have valid measurements */ enum HMC5883_BUS { - HMC5883_BUS_ALL, - HMC5883_BUS_INTERNAL, - HMC5883_BUS_EXTERNAL + HMC5883_BUS_ALL = 0, + HMC5883_BUS_I2C_INTERNAL, + HMC5883_BUS_I2C_EXTERNAL, + HMC5883_BUS_SPI }; /* oddly, ERROR is not defined for c++ */ @@ -1297,17 +1295,70 @@ namespace hmc5883 #endif const int ERROR = -1; -HMC5883 *g_dev_int = nullptr; -HMC5883 *g_dev_ext = nullptr; +/* + list of supported bus configurations + */ +struct hmc5883_bus_option { + enum HMC5883_BUS busid; + const char *devpath; + HMC5883_constructor interface_constructor; + uint8_t busnum; + HMC5883 *dev; +} bus_options[] = { + { HMC5883_BUS_I2C_INTERNAL, "/dev/hmc5883_int", &HMC5883_I2C_interface, PX4_I2C_BUS_EXPANSION, NULL }, +#ifdef PX4_I2C_BUS_ONBOARD + { HMC5883_BUS_I2C_EXTERNAL, "/dev/hmc5883_ext", &HMC5883_I2C_interface, PX4_I2C_BUS_ONBOARD, NULL }, +#endif +#ifdef PX4_SPIDEV_HMC + { HMC5883_BUS_SPI, "/dev/hmc5883_spi", &HMC5883_SPI_interface, PX4_SPI_BUS_SENSORS, NULL }, +#endif +}; +#define NUM_BUS_OPTIONS (sizeof(bus_options)/sizeof(bus_options[0])) -void start(int bus, enum Rotation rotation); -void test(int bus); -void reset(int bus); -int info(int bus); -int calibrate(int bus); -const char* get_path(int bus); +void start(enum HMC5883_BUS busid, enum Rotation rotation); +bool start_bus(struct hmc5883_bus_option &bus, enum Rotation rotation); +struct hmc5883_bus_option &find_bus(enum HMC5883_BUS busid); +void test(enum HMC5883_BUS busid); +void reset(enum HMC5883_BUS busid); +int info(enum HMC5883_BUS busid); +int calibrate(enum HMC5883_BUS busid); void usage(); +/** + * start driver for a specific bus option + */ +bool +start_bus(struct hmc5883_bus_option &bus, enum Rotation rotation) +{ + if (bus.dev != nullptr) + errx(1,"bus option already started"); + + device::Device *interface = bus.interface_constructor(bus.busnum); + if (interface->init() != OK) { + delete interface; + warnx("no device on bus %u", (unsigned)bus.busid); + return false; + } + bus.dev = new HMC5883(interface, bus.devpath, rotation); + if (bus.dev != nullptr && OK != bus.dev->init()) { + delete bus.dev; + bus.dev = NULL; + return false; + } + + int fd = open(bus.devpath, O_RDONLY); + if (fd < 0) + return false; + + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { + close(fd); + errx(1,"Failed to setup poll rate"); + } + + return true; +} + + /** * Start the driver. * @@ -1315,151 +1366,58 @@ void usage(); * is either successfully up and running or failed to start. */ void -start(int external_bus, enum Rotation rotation) +start(enum HMC5883_BUS busid, enum Rotation rotation) { - int fd; + uint8_t i; + bool started = false; - /* create the driver, attempt expansion bus first */ - if (g_dev_ext != nullptr) { - warnx("already started external"); - } else if (external_bus == HMC5883_BUS_ALL || external_bus == HMC5883_BUS_EXTERNAL) { - - device::Device *interface = nullptr; - - /* create the driver, only attempt I2C for the external bus */ - if (interface == nullptr && (HMC5883_I2C_interface != nullptr)) { - interface = HMC5883_I2C_interface(PX4_I2C_BUS_EXPANSION); - - if (interface->init() != OK) { - delete interface; - interface = nullptr; - warnx("no device on I2C bus #%u", PX4_I2C_BUS_EXPANSION); - } + for (i=0; iinit() != OK) { - delete interface; - interface = nullptr; - warnx("no device on I2C bus #%u", PX4_I2C_BUS_ONBOARD); - } - } -#endif - - /* interface will be null if init failed */ - if (interface != nullptr) { - - g_dev_ext = new HMC5883(interface, HMC5883L_DEVICE_PATH_EXT, rotation); - if (g_dev_ext != nullptr && OK != g_dev_ext->init()) { - delete g_dev_ext; - g_dev_ext = nullptr; - } - - } - } - - - /* if this failed, attempt onboard sensor */ - if (g_dev_int != nullptr) { - warnx("already started internal"); - } else if (external_bus == HMC5883_BUS_ALL || external_bus == HMC5883_BUS_INTERNAL) { - - device::Device *interface = nullptr; - - /* create the driver, try SPI first, fall back to I2C if unsuccessful */ -#ifdef PX4_SPIDEV_HMC - if (HMC5883_SPI_interface != nullptr) { - interface = HMC5883_SPI_interface(PX4_SPI_BUS_SENSORS); - } -#endif - -#ifdef PX4_I2C_BUS_ONBOARD - /* this device is already connected as external if present above */ - if (interface == nullptr && (HMC5883_I2C_interface != nullptr)) { - interface = HMC5883_I2C_interface(PX4_I2C_BUS_ONBOARD); - } -#endif - if (interface == nullptr) { - warnx("no internal bus scanned"); - goto fail; - } - - if (interface->init() != OK) { - delete interface; - warnx("no device on internal bus"); - } else { - - g_dev_int = new HMC5883(interface, HMC5883L_DEVICE_PATH_INT, rotation); - if (g_dev_int != nullptr && OK != g_dev_int->init()) { - - /* tear down the failing onboard instance */ - delete g_dev_int; - g_dev_int = nullptr; - - if (external_bus == HMC5883_BUS_INTERNAL) { - goto fail; - } - } - if (g_dev_int == nullptr && external_bus == HMC5883_BUS_INTERNAL) { - goto fail; - } + if (busid != HMC5883_BUS_ALL && bus_options[i].busid != busid) { + // not the one that is asked for + continue; } + started |= start_bus(bus_options[i], rotation); } - if (g_dev_int == nullptr && g_dev_ext == nullptr) - goto fail; - - /* set the poll rate to default, starts automatic data collection */ - if (g_dev_int != nullptr) { - fd = open(HMC5883L_DEVICE_PATH_INT, O_RDONLY); - if (fd < 0) - goto fail; - - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) - goto fail; - close(fd); - } - - if (g_dev_ext != nullptr) { - fd = open(HMC5883L_DEVICE_PATH_EXT, O_RDONLY); - if (fd < 0) - goto fail; - - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) - goto fail; - close(fd); - } + if (!started) + errx(1, "driver start failed"); + // one or more drivers started OK exit(0); - -fail: - if (g_dev_int != nullptr && (external_bus == HMC5883_BUS_ALL || external_bus == HMC5883_BUS_INTERNAL)) { - delete g_dev_int; - g_dev_int = nullptr; - } - if (g_dev_ext != nullptr && (external_bus == HMC5883_BUS_ALL || external_bus == HMC5883_BUS_EXTERNAL)) { - delete g_dev_ext; - g_dev_ext = nullptr; - } - - errx(1, "driver start failed"); } +/** + * find a bus structure for a busid + */ +struct hmc5883_bus_option &find_bus(enum HMC5883_BUS busid) +{ + for (uint8_t i=0; iprint_info(); - ret = 0; - } - - return ret; -} - -const char* -get_path(int bus) -{ - return ((bus == HMC5883_BUS_INTERNAL) ? HMC5883L_DEVICE_PATH_INT : HMC5883L_DEVICE_PATH_EXT); + warnx("running on bus: %u (%s)\n", (unsigned)bus.busid, bus.devpath); + bus.dev->print_info(); + exit(0); } void @@ -1650,22 +1595,25 @@ int hmc5883_main(int argc, char *argv[]) { int ch; - int bus = HMC5883_BUS_ALL; + enum HMC5883_BUS busid = HMC5883_BUS_ALL; enum Rotation rotation = ROTATION_NONE; bool calibrate = false; - while ((ch = getopt(argc, argv, "XIR:C")) != EOF) { + while ((ch = getopt(argc, argv, "XISR:C")) != EOF) { switch (ch) { case 'R': rotation = (enum Rotation)atoi(optarg); break; #if (PX4_I2C_BUS_ONBOARD || PX4_SPIDEV_HMC) case 'I': - bus = HMC5883_BUS_INTERNAL; + busid = HMC5883_BUS_I2C_INTERNAL; break; #endif case 'X': - bus = HMC5883_BUS_EXTERNAL; + busid = HMC5883_BUS_I2C_EXTERNAL; + break; + case 'S': + busid = HMC5883_BUS_SPI; break; case 'C': calibrate = true; @@ -1682,9 +1630,9 @@ hmc5883_main(int argc, char *argv[]) * Start/load the driver. */ if (!strcmp(verb, "start")) { - hmc5883::start(bus, rotation); + hmc5883::start(busid, rotation); if (calibrate) { - if (hmc5883::calibrate(bus) == 0) { + if (hmc5883::calibrate(busid) == 0) { errx(0, "calibration successful"); } else { @@ -1697,38 +1645,25 @@ hmc5883_main(int argc, char *argv[]) * Test the driver/device. */ if (!strcmp(verb, "test")) - hmc5883::test(bus); + hmc5883::test(busid); /* * Reset the driver. */ if (!strcmp(verb, "reset")) - hmc5883::reset(bus); + hmc5883::reset(busid); /* * Print driver information. */ - if (!strcmp(verb, "info") || !strcmp(verb, "status")) { - if (bus == HMC5883_BUS_ALL) { - int ret = 0; - if (hmc5883::info(HMC5883_BUS_INTERNAL)) { - ret = 1; - } - - if (hmc5883::info(HMC5883_BUS_EXTERNAL)) { - ret = 1; - } - exit(ret); - } else { - exit(hmc5883::info(bus)); - } - } + if (!strcmp(verb, "info") || !strcmp(verb, "status")) + hmc5883::info(busid); /* * Autocalibrate the scaling */ if (!strcmp(verb, "calibrate")) { - if (hmc5883::calibrate(bus) == 0) { + if (hmc5883::calibrate(busid) == 0) { errx(0, "calibration successful"); } else { diff --git a/src/drivers/hmc5883/hmc5883.h b/src/drivers/hmc5883/hmc5883.h index 0eb7736295..e91e91fc09 100644 --- a/src/drivers/hmc5883/hmc5883.h +++ b/src/drivers/hmc5883/hmc5883.h @@ -50,3 +50,4 @@ /* interface factories */ extern device::Device *HMC5883_SPI_interface(int bus) weak_function; extern device::Device *HMC5883_I2C_interface(int bus) weak_function; +typedef device::Device* (*HMC5883_constructor)(int); From 8ed6612c1a8838acd9f0e9a386800c4136b98223 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Fri, 9 Jan 2015 12:44:31 +0100 Subject: [PATCH 455/652] Fix PWM OUT command and update fligth tested params. --- .../px4fmu_common/init.d/10018_tbs_endurance | 22 ++++++++++++------- 1 file changed, 14 insertions(+), 8 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/10018_tbs_endurance b/ROMFS/px4fmu_common/init.d/10018_tbs_endurance index eeb9b48b29..e83a864d8d 100644 --- a/ROMFS/px4fmu_common/init.d/10018_tbs_endurance +++ b/ROMFS/px4fmu_common/init.d/10018_tbs_endurance @@ -2,7 +2,7 @@ # # Team Blacksheep Discovery Long Range Quadcopter # -# Setup: 15 x 6.5" Props, 6S 4000mAh TBS LiPo, TBS 30A ESCs, TBS 400kV Motors +# Setup: 15 x 5" Props, 6S 4000mAh TBS LiPo, TBS 30A ESCs, TBS 400kV Motors # # Simon Wilks # @@ -11,21 +11,27 @@ sh /etc/init.d/rc.mc_defaults if [ $AUTOCNF == yes ] then + param set BAT_N_CELLS 6 + param set BAT_V_EMPTY 3.5 + param set MC_ROLL_P 7.0 - param set MC_ROLLRATE_P 0.07 + param set MC_ROLLRATE_P 0.08 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_PITCHRATE_P 0.13 + param set MC_PITCHRATE_I 0.02 + param set MC_PITCHRATE_D 0.005 param set MC_YAW_P 2.8 - param set MC_YAWRATE_P 0.4 + param set MC_YAWRATE_P 0.2 param set MC_YAWRATE_I 0.1 param set MC_YAWRATE_D 0.0 + + param set MPC_XY_FF 0.2 + param set MPC_XY_VEL_MAX 2 fi set MIXER quad_w -set PWM_OUTPUTS 1234 -set PWM_MIN 1200 +set PWM_OUT 1234 +set PWM_MIN 1080 From 26ac1ab03eca9dcf70782e952cb4ee8d597ea6d6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 9 Jan 2015 22:18:19 +0100 Subject: [PATCH 456/652] README.md: Add link to unit testing doku --- README.md | 3 +++ 1 file changed, 3 insertions(+) diff --git a/README.md b/README.md index 1a559d3c3c..06210b6a10 100644 --- a/README.md +++ b/README.md @@ -26,6 +26,9 @@ http://px4.io/dev/contributing Developer guide: http://px4.io/dev/ +Testing guide: +http://px4.io/dev/unit_tests + This repository contains code supporting these boards: * PX4FMUv1.x * PX4FMUv2.x From aa3d2c41f1a3f8177cf7a5d36ecf468209d54a4a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 9 Jan 2015 22:56:58 +0100 Subject: [PATCH 457/652] README.md: Further improvements --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 06210b6a10..0c00e0ce27 100644 --- a/README.md +++ b/README.md @@ -6,7 +6,7 @@ * Official Website: http://px4.io * License: BSD 3-clause (see LICENSE.md) -* Supported airframes: +* Supported airframes (more experimental are supported): * [Multicopters](http://px4.io/platforms/multicopters/start) * [Fixed wing](http://px4.io/platforms/planes/start) * [VTOL](http://px4.io/platforms/vtol/start) From c51b73c1967199aafc8d31262172be51fe987fae Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 10 Jan 2015 01:18:51 +0300 Subject: [PATCH 458/652] UAVCAN update - fixes https://github.com/UAVCAN/uavcan/issues/8 --- src/lib/uavcan | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/uavcan b/src/lib/uavcan index 1efd244275..c4c45b995f 160000 --- a/src/lib/uavcan +++ b/src/lib/uavcan @@ -1 +1 @@ -Subproject commit 1efd24427539fa332a15151143466ec760fa5fff +Subproject commit c4c45b995f5c8192c7a36c4293c201711ceac74d From 1507d479e099549a81c99fd9f702b4a278fa763d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 10 Jan 2015 18:18:28 +0100 Subject: [PATCH 459/652] commander: Refresh safety tests to avoid false positives in unit tests. --- .../commander/commander_tests/state_machine_helper_test.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/commander/commander_tests/state_machine_helper_test.cpp b/src/modules/commander/commander_tests/state_machine_helper_test.cpp index 874090e93e..3cfa8b4c6c 100644 --- a/src/modules/commander/commander_tests/state_machine_helper_test.cpp +++ b/src/modules/commander/commander_tests/state_machine_helper_test.cpp @@ -280,6 +280,8 @@ bool StateMachineHelperTest::armingStateTransitionTest(void) status.arming_state = test->current_state.arming_state; status.condition_system_sensors_initialized = test->condition_system_sensors_initialized; status.hil_state = test->hil_state; + // The power status of the test unit is not relevant for the unit test + status.circuit_breaker_engaged_power_check = true; safety.safety_switch_available = test->safety_switch_available; safety.safety_off = test->safety_off; armed.armed = test->current_state.armed; From a3bce71b97b6e958737d11414cce2609e5d4848d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 7 Jan 2015 17:14:46 +0100 Subject: [PATCH 460/652] Performance counters: Estimate RMS for elapsed counters. Allow to use a perf counter across processes, deal with overruns and other resulting inconsistencies from cross-process use. --- src/modules/systemlib/perf_counter.c | 86 ++++++++++++++++++++-------- src/modules/systemlib/perf_counter.h | 12 +++- 2 files changed, 74 insertions(+), 24 deletions(-) diff --git a/src/modules/systemlib/perf_counter.c b/src/modules/systemlib/perf_counter.c index f9e90652d7..350c74d8ae 100644 --- a/src/modules/systemlib/perf_counter.c +++ b/src/modules/systemlib/perf_counter.c @@ -39,6 +39,7 @@ #include #include +#include #include #include #include @@ -67,10 +68,13 @@ struct perf_ctr_count { struct perf_ctr_elapsed { struct perf_ctr_header hdr; uint64_t event_count; + uint64_t event_overruns; uint64_t time_start; uint64_t time_total; uint64_t time_least; uint64_t time_most; + float mean; + float M2; }; /** @@ -126,6 +130,28 @@ perf_alloc(enum perf_counter_type type, const char *name) return ctr; } +perf_counter_t +perf_alloc_once(enum perf_counter_type type, const char *name) +{ + perf_counter_t handle = (perf_counter_t)sq_peek(&perf_counters); + + while (handle != NULL) { + if (!strcmp(handle->name, name)) { + if (type == handle->type) { + /* they are the same counter */ + return handle; + } else { + /* same name but different type, assuming this is an error and not intended */ + return NULL; + } + } + handle = (perf_counter_t)sq_next(&handle->link); + } + + /* if the execution reaches here, no existing counter of that name was found */ + return perf_alloc(type, name); +} + void perf_free(perf_counter_t handle) { @@ -213,18 +239,30 @@ perf_end(perf_counter_t handle) struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle; if (pce->time_start != 0) { - hrt_abstime elapsed = hrt_absolute_time() - pce->time_start; + int64_t elapsed = hrt_absolute_time() - pce->time_start; - pce->event_count++; - pce->time_total += elapsed; + if (elapsed < 0) { + pce->event_overruns++; + } else { - if ((pce->time_least > elapsed) || (pce->time_least == 0)) - pce->time_least = elapsed; + pce->event_count++; + pce->time_total += elapsed; - if (pce->time_most < elapsed) - pce->time_most = elapsed; + if ((pce->time_least > (uint64_t)elapsed) || (pce->time_least == 0)) + pce->time_least = elapsed; - pce->time_start = 0; + if (pce->time_most < (uint64_t)elapsed) + pce->time_most = elapsed; + + // maintain mean and variance of the elapsed time in seconds + // Knuth/Welford recursive mean and variance of update intervals (via Wikipedia) + float dt = elapsed / 1e6f; + float delta_intvl = dt - pce->mean; + pce->mean += delta_intvl / pce->event_count; + pce->M2 += delta_intvl * (dt - pce->mean); + + pce->time_start = 0; + } } } break; @@ -310,14 +348,17 @@ perf_print_counter_fd(int fd, perf_counter_t handle) case PC_ELAPSED: { struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle; + float rms = sqrtf(pce->M2 / (pce->event_count-1)); - dprintf(fd, "%s: %llu events, %lluus elapsed, %lluus avg, min %lluus max %lluus\n", - handle->name, - pce->event_count, - pce->time_total, - pce->time_total / pce->event_count, - pce->time_least, - pce->time_most); + dprintf(fd, "%s: %llu events, %llu overruns, %lluus elapsed, %lluus avg, min %lluus max %lluus %5.3fus rms\n", + handle->name, + pce->event_count, + pce->event_overruns, + pce->time_total, + pce->time_total / pce->event_count, + pce->time_least, + pce->time_most, + (double)(1e6f * rms)); break; } @@ -325,14 +366,13 @@ perf_print_counter_fd(int fd, perf_counter_t handle) 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 %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, - (double)(1000 * pci->mean), - (double)(1000 * rms)); + dprintf(fd, "%s: %llu events, %lluus avg, min %lluus max %lluus %5.3fus rms\n", + handle->name, + pci->event_count, + (pci->time_last - pci->time_first) / pci->event_count, + pci->time_least, + pci->time_most, + (double)(1e6f * rms)); break; } diff --git a/src/modules/systemlib/perf_counter.h b/src/modules/systemlib/perf_counter.h index d06606a5d0..92f064d048 100644 --- a/src/modules/systemlib/perf_counter.h +++ b/src/modules/systemlib/perf_counter.h @@ -56,7 +56,7 @@ typedef struct perf_ctr_header *perf_counter_t; __BEGIN_DECLS /** - * Create a new counter. + * Create a new local counter. * * @param type The type of the new counter. * @param name The counter name. @@ -65,6 +65,16 @@ __BEGIN_DECLS */ __EXPORT extern perf_counter_t perf_alloc(enum perf_counter_type type, const char *name); +/** + * Get the reference to an existing counter or create a new one if it does not exist. + * + * @param type The type of the counter. + * @param name The counter name. + * @return Handle for the counter, or NULL if a counter + * could not be allocated. + */ +__EXPORT extern perf_counter_t perf_alloc_once(enum perf_counter_type type, const char *name); + /** * Free a counter. * From 2bff39d562f1d7c0ffa5e8875d355eb3271c70fe Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 7 Jan 2015 17:15:19 +0100 Subject: [PATCH 461/652] MPU6K driver: Start performance counters for system latency, as its commonly the main sensor --- src/drivers/mpu6000/mpu6000.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 6cac28a7db..168b34ea91 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -251,6 +251,8 @@ private: perf_counter_t _bad_registers; perf_counter_t _good_transfers; perf_counter_t _reset_retries; + perf_counter_t _system_latency_perf; + perf_counter_t _controller_latency_perf; uint8_t _register_wait; uint64_t _reset_wait; @@ -491,6 +493,8 @@ MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev _bad_registers(perf_alloc(PC_COUNT, "mpu6000_bad_registers")), _good_transfers(perf_alloc(PC_COUNT, "mpu6000_good_transfers")), _reset_retries(perf_alloc(PC_COUNT, "mpu6000_reset_retries")), + _system_latency_perf(perf_alloc_once(PC_ELAPSED, "sys_latency")), + _controller_latency_perf(perf_alloc_once(PC_ELAPSED, "ctrl_latency")), _register_wait(0), _reset_wait(0), _accel_filter_x(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), @@ -1731,6 +1735,9 @@ MPU6000::measure() _gyro->parent_poll_notify(); if (!(_pub_blocked)) { + /* log the time of this report */ + perf_begin(_controller_latency_perf); + perf_begin(_system_latency_perf); /* publish it */ orb_publish(_accel_orb_id, _accel_topic, &arb); } From 76821607130eab7916b70d789a189b2a52a115da Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 7 Jan 2015 17:15:42 +0100 Subject: [PATCH 462/652] mc attitude controller: Log the controller latency --- src/modules/mc_att_control/mc_att_control_main.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) 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 82d2ff23a5..cfb237711a 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -141,6 +141,7 @@ private: struct vehicle_status_s _vehicle_status; /**< vehicle status */ perf_counter_t _loop_perf; /**< loop performance counter */ + perf_counter_t _controller_latency_perf; math::Vector<3> _rates_prev; /**< angular rates on previous step */ math::Vector<3> _rates_sp; /**< angular rates setpoint */ @@ -289,7 +290,8 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _actuators_0_circuit_breaker_enabled(false), /* performance counters */ - _loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control")) + _loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control")), + _controller_latency_perf(perf_alloc_once(PC_ELAPSED, "ctrl_latency")) { memset(&_v_att, 0, sizeof(_v_att)); @@ -890,6 +892,7 @@ MulticopterAttitudeControl::task_main() if (!_actuators_0_circuit_breaker_enabled) { if (_actuators_0_pub > 0) { orb_publish(_actuators_id, _actuators_0_pub, &_actuators); + perf_end(_controller_latency_perf); } else if (_actuators_id) { _actuators_0_pub = orb_advertise(_actuators_id, &_actuators); From 4712c75dea935b5709c944b31ae670dd6879f2e9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 7 Jan 2015 17:16:15 +0100 Subject: [PATCH 463/652] IO driver: Log the total system latency up to the IO transfer --- src/drivers/px4io/px4io.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 58390ba4c3..ed9487cf95 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -263,6 +263,7 @@ private: perf_counter_t _perf_update; /// Date: Wed, 7 Jan 2015 18:19:23 +0100 Subject: [PATCH 464/652] Performance counters: Add option to set otherwise estimated time interval --- src/modules/systemlib/perf_counter.c | 40 ++++++++++++++++++++++++++++ src/modules/systemlib/perf_counter.h | 12 +++++++++ 2 files changed, 52 insertions(+) diff --git a/src/modules/systemlib/perf_counter.c b/src/modules/systemlib/perf_counter.c index 350c74d8ae..4d62db0ee9 100644 --- a/src/modules/systemlib/perf_counter.c +++ b/src/modules/systemlib/perf_counter.c @@ -272,6 +272,46 @@ perf_end(perf_counter_t handle) } } +void +perf_set(perf_counter_t handle, int64_t elapsed) +{ + if (handle == NULL) + return; + + switch (handle->type) { + case PC_ELAPSED: { + struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle; + + if (elapsed < 0) { + pce->event_overruns++; + } else { + + pce->event_count++; + pce->time_total += elapsed; + + if ((pce->time_least > (uint64_t)elapsed) || (pce->time_least == 0)) + pce->time_least = elapsed; + + if (pce->time_most < (uint64_t)elapsed) + pce->time_most = elapsed; + + // maintain mean and variance of the elapsed time in seconds + // Knuth/Welford recursive mean and variance of update intervals (via Wikipedia) + float dt = elapsed / 1e6f; + float delta_intvl = dt - pce->mean; + pce->mean += delta_intvl / pce->event_count; + pce->M2 += delta_intvl * (dt - pce->mean); + + pce->time_start = 0; + } + } + break; + + default: + break; + } +} + void perf_cancel(perf_counter_t handle) { diff --git a/src/modules/systemlib/perf_counter.h b/src/modules/systemlib/perf_counter.h index 92f064d048..0c1243de34 100644 --- a/src/modules/systemlib/perf_counter.h +++ b/src/modules/systemlib/perf_counter.h @@ -111,6 +111,18 @@ __EXPORT extern void perf_begin(perf_counter_t handle); */ __EXPORT extern void perf_end(perf_counter_t handle); +/** + * Register a measurement + * + * This call applies to counters that operate over ranges of time; PC_ELAPSED etc. + * If a call is made without a corresponding perf_begin call. It sets the + * value provided as argument as a new measurement. + * + * @param handle The handle returned from perf_alloc. + * @param elapsed The time elapsed. Negative values lead to incrementing the overrun counter. + */ +__EXPORT extern void perf_set(perf_counter_t handle, int64_t elapsed); + /** * Cancel a performance event. * From 1bee73af2237d72a64adbf0a4bedd8b20581b4bd Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 7 Jan 2015 18:19:47 +0100 Subject: [PATCH 465/652] Latency measurements: Estimate latency based on sensor timestamp through full system --- src/drivers/px4io/px4io.cpp | 7 +++++-- src/modules/fw_att_control/fw_att_control_main.cpp | 2 ++ src/modules/uORB/topics/actuator_controls.h | 1 + 3 files changed, 8 insertions(+), 2 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index ed9487cf95..72b2dc772a 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -263,7 +263,8 @@ private: perf_counter_t _perf_update; /// 0) { diff --git a/src/modules/uORB/topics/actuator_controls.h b/src/modules/uORB/topics/actuator_controls.h index 4d1f9a6380..668f8f1646 100644 --- a/src/modules/uORB/topics/actuator_controls.h +++ b/src/modules/uORB/topics/actuator_controls.h @@ -62,6 +62,7 @@ struct actuator_controls_s { uint64_t timestamp; + uint64_t timestamp_sample; /**< the timestamp the data this control response is based on was sampled */ float control[NUM_ACTUATOR_CONTROLS]; }; From 05367f8a006ae6e36fec0911c97490c31033551b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 7 Jan 2015 23:05:33 +0100 Subject: [PATCH 466/652] Handle slight increase of frame size in example --- src/examples/fixedwing_control/module.mk | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/examples/fixedwing_control/module.mk b/src/examples/fixedwing_control/module.mk index f6c882ead4..da96054d3b 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 = -Wframe-larger-than=1200 +EXTRACFLAGS = -Wframe-larger-than=1300 From 7d56ae4ed65f67cfe159a436eb58da59e9e0b0f6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 8 Jan 2015 12:59:20 +0100 Subject: [PATCH 467/652] mc attitude control: Log sensor time stamp in actuator output --- src/modules/mc_att_control/mc_att_control_main.cpp | 1 + 1 file changed, 1 insertion(+) 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 cfb237711a..a094ed2c60 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -888,6 +888,7 @@ MulticopterAttitudeControl::task_main() _actuators.control[2] = (isfinite(_att_control(2))) ? _att_control(2) : 0.0f; _actuators.control[3] = (isfinite(_thrust_sp)) ? _thrust_sp : 0.0f; _actuators.timestamp = hrt_absolute_time(); + _actuators.timestamp_sample = _v_att.timestamp; if (!_actuators_0_circuit_breaker_enabled) { if (_actuators_0_pub > 0) { From 6203c73ccc1426b4280fb23284a2f52e4425ef4d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 9 Jan 2015 09:08:21 +0100 Subject: [PATCH 468/652] Perf counter fixes --- src/drivers/px4io/px4io.cpp | 16 +++------------- src/modules/systemlib/perf_counter.c | 5 ++++- 2 files changed, 7 insertions(+), 14 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 72b2dc772a..96ebedd83c 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -260,10 +260,8 @@ private: int _mavlink_fd; ///< mavlink file descriptor. - perf_counter_t _perf_update; /// + void perf_set(perf_counter_t handle, int64_t elapsed) { - if (handle == NULL) + if (handle == NULL) { return; + } switch (handle->type) { case PC_ELAPSED: { From ae6198b0bad801535c879d3269920e12781cea92 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 11 Jan 2015 11:41:29 +0100 Subject: [PATCH 469/652] sdlog2: Made sdlog writer performance available in log fiiles, reduced telemetry messages --- src/modules/sdlog2/sdlog2.c | 23 ++++++++++++----------- 1 file changed, 12 insertions(+), 11 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index abc69a4b50..d35b702391 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -199,6 +199,8 @@ static bool space_warning_sent = false; static pthread_t logwriter_pthread = 0; static pthread_attr_t logwriter_attr; +static perf_counter_t perf_write; + /** * Log buffer writing thread. Open and close file here. */ @@ -451,10 +453,10 @@ int open_log_file() int fd = open(log_file_path, O_CREAT | O_WRONLY | O_DSYNC); if (fd < 0) { - mavlink_and_console_log_critical(mavlink_fd, "[sdlog2] failed opening log: %s", log_file_name); + mavlink_and_console_log_critical(mavlink_fd, "[sdlog2] failed opening: %s", log_file_name); } else { - mavlink_and_console_log_info(mavlink_fd, "[sdlog2] log file: %s", log_file_name); + mavlink_and_console_log_info(mavlink_fd, "[sdlog2] starting: %s", log_file_name); } return fd; @@ -515,8 +517,6 @@ 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) { @@ -620,16 +620,11 @@ static void *logwriter_thread(void *arg) fsync(log_fd); close(log_fd); - /* free performance counter */ - perf_free(perf_write); - return NULL; } void sdlog2_start_log() { - mavlink_and_console_log_info(mavlink_fd, "[sdlog2] start logging"); - /* create log dir if needed */ if (create_log_dir() != 0) { mavlink_and_console_log_critical(mavlink_fd, "[sdlog2] error creating log dir"); @@ -655,6 +650,9 @@ void sdlog2_start_log() logwriter_should_exit = false; + /* allocate write performance counter */ + perf_write = perf_alloc(PC_ELAPSED, "sd write"); + /* start log buffer emptying thread */ if (0 != pthread_create(&logwriter_pthread, &logwriter_attr, logwriter_thread, &lb)) { errx(1, "error creating logwriter thread"); @@ -674,8 +672,6 @@ void sdlog2_start_log() void sdlog2_stop_log() { - mavlink_and_console_log_info(mavlink_fd, "[sdlog2] stop logging"); - logging_enabled = false; /* wake up write thread one last time */ @@ -701,6 +697,11 @@ void sdlog2_stop_log() perf_print_all(perf_fd); close(perf_fd); + /* free log writer performance counter */ + perf_free(perf_write); + + mavlink_and_console_log_info(mavlink_fd, "[sdlog2] logging stopped"); + sdlog2_status(); } From 831e2a349cacb4ea19be572fed98b49f9f8cea36 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 11 Jan 2015 12:55:57 +0100 Subject: [PATCH 470/652] README.md: Make board names consistent --- README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index 0c00e0ce27..e152f29ca3 100644 --- a/README.md +++ b/README.md @@ -30,8 +30,8 @@ Testing guide: http://px4.io/dev/unit_tests This repository contains code supporting these boards: - * PX4FMUv1.x - * PX4FMUv2.x + * FMUv1.x + * FMUv2.x * AeroCore (v1 and v2) ## NuttShell (NSH) ## From 171ca079ba39f2c7c5003419aec20a055391c964 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 11 Jan 2015 13:26:45 +0100 Subject: [PATCH 471/652] README: Wording --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index e152f29ca3..33b8cdec79 100644 --- a/README.md +++ b/README.md @@ -16,7 +16,7 @@ ### Users ### -Please refer to the [user documentation](https://pixhawk.org/users/start) for flying drones with PX4. +Please refer to the [user documentation](https://pixhawk.org/users/start) for flying drones with the PX4 flight stack. ### Developers ### From 0954ee74714373ee99740cbadb665874b95faa19 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 11 Jan 2015 14:55:53 +0100 Subject: [PATCH 472/652] Improving the contribution guide file --- CONTRIBUTING.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md index 3bf02fbff3..fef02e6228 100644 --- a/CONTRIBUTING.md +++ b/CONTRIBUTING.md @@ -16,7 +16,7 @@ git checkout -b mydescriptivebranchname ### Edit and build the code -The [developer guide](http://pixhawk.org/dev/start) explains how to set up the development environment on Mac OS, Linux or Windows. Please take note of our [coding style](http://pixhawk.org/dev/code_style) when editing files. +The [developer guide](http://px4.io/dev/start) explains how to set up the development environment on Mac OS, Linux or Windows. Please take note of our [coding style](http://px4.io/dev/code_style) when editing files. ### Commit your changes @@ -41,4 +41,4 @@ Since we care about safety, we will regularly ask you for test results. Best is Push changes to your repo and send a [pull request](https://github.com/PX4/Firmware/compare/). -Make sure to provide some testing feedback and if possible the link to a flight log file. +Make sure to provide some testing feedback and if possible the link to a flight log file. Upload flight log files to [Log Muncher](http://dash.oznet.ch) and link the resulting report. From ddf5be50e1058cbb18771393d3c70aed9d8655a6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 11 Jan 2015 16:10:47 +0100 Subject: [PATCH 473/652] README: Add link to contribution.md --- README.md | 1 + 1 file changed, 1 insertion(+) diff --git a/README.md b/README.md index 33b8cdec79..4ab1cbd2d8 100644 --- a/README.md +++ b/README.md @@ -21,6 +21,7 @@ Please refer to the [user documentation](https://pixhawk.org/users/start) for fl ### Developers ### Contributing guide: +https://github.com/PX4/Firmware/blob/master/CONTRIBUTING.md http://px4.io/dev/contributing Developer guide: From 9e42d5bed2c73ee49dd7692499e0de6c6ff76475 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 11 Jan 2015 16:13:17 +0100 Subject: [PATCH 474/652] README: Update contribution link style --- README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index 4ab1cbd2d8..80f5a585fe 100644 --- a/README.md +++ b/README.md @@ -21,8 +21,8 @@ Please refer to the [user documentation](https://pixhawk.org/users/start) for fl ### Developers ### Contributing guide: -https://github.com/PX4/Firmware/blob/master/CONTRIBUTING.md -http://px4.io/dev/contributing + * [CONTRIBUTING.md](https://github.com/PX4/Firmware/blob/master/CONTRIBUTING.md) + * [PX4 Contribution Guide](http://px4.io/dev/contributing) Developer guide: http://px4.io/dev/ From 11469ad9b5e54167604f4b46dd0b826ba4f1889b Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Mon, 12 Jan 2015 12:25:30 +0100 Subject: [PATCH 475/652] px_uploader: Small fix to properly display timeouts --- Tools/px_uploader.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/Tools/px_uploader.py b/Tools/px_uploader.py index 2720806199..c46f6bede1 100755 --- a/Tools/px_uploader.py +++ b/Tools/px_uploader.py @@ -287,11 +287,12 @@ class uploader(object): #Draw progress bar (erase usually takes about 9 seconds to complete) estimatedTimeRemaining = deadline-time.time() - if estimatedTimeRemaining > 0: + if estimatedTimeRemaining >= 9.0: self.__drawProgressBar(20.0-estimatedTimeRemaining, 9.0) else: self.__drawProgressBar(10.0, 10.0) - sys.stdout.write(" (timeout: %d seconds) " % int(time.time()-deadline) ) + sys.stdout.write(" (timeout: %d seconds) " % int(deadline-time.time()) ) + sys.stdout.flush() if self.__trySync(): self.__drawProgressBar(10.0, 10.0) From 78b603bb3532e927e617fe1946a1090b509e72ad Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 12 Jan 2015 17:20:58 +0100 Subject: [PATCH 476/652] ROMFS: Fix up VTOL autostarts --- .../px4fmu_common/init.d/13001_caipirinha_vtol | 1 - ROMFS/px4fmu_common/init.d/rc.autostart | 18 +++++++++++++----- 2 files changed, 13 insertions(+), 6 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol b/ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol index 87b98a02bc..5c00041492 100644 --- a/ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol +++ b/ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol @@ -1,4 +1,3 @@ -#!nsh # # Generic configuration file for caipirinha VTOL version # diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart b/ROMFS/px4fmu_common/init.d/rc.autostart index b83687fbd4..0d6e0564af 100644 --- a/ROMFS/px4fmu_common/init.d/rc.autostart +++ b/ROMFS/px4fmu_common/init.d/rc.autostart @@ -252,9 +252,17 @@ fi # 13000 is historically reserved for the quadshot # -# VTOL caipririnha +# VTOL Caipririnha (Tailsitter) # - if param compare SYS_AUTOSTART 13001 - then - sh /etc/init.d/13001_caipirinha_vtol - fi +if param compare SYS_AUTOSTART 13001 +then + sh /etc/init.d/13001_caipirinha_vtol +fi + +# +# VTOL BirdsEyeView FireFly x6 (Tilt-Rotor) +# +if param compare SYS_AUTOSTART 13002 +then + sh /etc/init.d/13002_firefly6 +fi From 3c98c7119ebe956d1dc9c10379e9158567d16642 Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Mon, 22 Dec 2014 13:47:20 -0700 Subject: [PATCH 477/652] use non-uniform 1st order IIR lowpass filters for baro_gps_offset estimation --- src/drivers/ms5611/ms5611.cpp | 2 +- .../mathlib/math/filter/LowPassFilter2p.cpp | 5 +- .../ekf_att_pos_estimator_main.cpp | 59 ++++++++++++++++--- 3 files changed, 57 insertions(+), 9 deletions(-) diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp index 0a793cbc97..6eda6933d9 100644 --- a/src/drivers/ms5611/ms5611.cpp +++ b/src/drivers/ms5611/ms5611.cpp @@ -90,7 +90,7 @@ static const int ERROR = -1; /* internal conversion time: 9.17 ms, so should not be read at rates higher than 100 Hz */ #define MS5611_CONVERSION_INTERVAL 10000 /* microseconds */ -#define MS5611_MEASUREMENT_RATIO 3 /* pressure measurements per temperature measurement */ +#define MS5611_MEASUREMENT_RATIO 100 /* pressure measurements per temperature measurement */ #define MS5611_BARO_DEVICE_PATH_EXT "/dev/ms5611_ext" #define MS5611_BARO_DEVICE_PATH_INT "/dev/ms5611_int" diff --git a/src/lib/mathlib/math/filter/LowPassFilter2p.cpp b/src/lib/mathlib/math/filter/LowPassFilter2p.cpp index 6f640c9f9a..c709a28348 100644 --- a/src/lib/mathlib/math/filter/LowPassFilter2p.cpp +++ b/src/lib/mathlib/math/filter/LowPassFilter2p.cpp @@ -66,6 +66,7 @@ float LowPassFilter2p::apply(float sample) // no filtering return sample; } + // do the filtering float delay_element_0 = sample - _delay_element_1 * _a1 - _delay_element_2 * _a2; if (isnan(delay_element_0) || isinf(delay_element_0)) { @@ -82,7 +83,9 @@ float LowPassFilter2p::apply(float sample) } float LowPassFilter2p::reset(float sample) { - _delay_element_1 = _delay_element_2 = sample; + float dval = sample / (_b0 + _b1 + _b2); + _delay_element_1 = dval; + _delay_element_2 = dval; return apply(sample); } diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index c41777968e..1770a75e53 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -82,12 +82,11 @@ #include #include #include +#include #include #include "estimator_23states.h" - - /** * estimator app start / stop handling function * @@ -101,7 +100,7 @@ __EXPORT uint64_t getMicros(); static uint64_t IMUmsec = 0; static uint64_t IMUusec = 0; -static const uint64_t FILTER_INIT_DELAY = 1 * 1000 * 1000; +static const uint64_t FILTER_INIT_DELAY = 1 * 1000 * 1000; // units: microseconds uint32_t millis() { @@ -222,8 +221,10 @@ private: float _baro_ref; /**< barometer reference altitude */ float _baro_ref_offset; /**< offset between initial baro reference and GPS init baro altitude */ float _baro_gps_offset; /**< offset between baro altitude (at GPS init time) and GPS altitude */ + hrt_abstime _last_debug_print = 0; perf_counter_t _loop_perf; ///< loop performance counter + perf_counter_t _loop_intvl; ///< loop rate counter perf_counter_t _perf_gyro; ///gpsLon = math::radians(_gps.lon / (double)1e7) - M_PI; _ekf->gpsHgt = _gps.alt / 1e3f; + // update LPF + _gps_alt_filt += (1 - expf(-gps_elapsed * rc)) * (_gps_last - _gps_alt_filt); + _gps_last = _ekf->gpsHgt; + + //warnx("gps alt: %6.1f, interval: %6.3f", (double)_ekf->gpsHgt, (double)gps_elapsed); + // if (_gps.s_variance_m_s > 0.25f && _gps.s_variance_m_s < 100.0f * 100.0f) { // _ekf->vneSigma = sqrtf(_gps.s_variance_m_s); // } else { @@ -1070,7 +1085,6 @@ FixedwingEstimator::task_main() // warnx("vel: %8.4f pos: %8.4f", _gps.s_variance_m_s, _gps.p_variance_m); newDataGps = true; - last_gps = _gps.timestamp_position; } @@ -1082,15 +1096,16 @@ FixedwingEstimator::task_main() if (baro_updated) { - hrt_abstime baro_last = _baro.timestamp; - orb_copy(ORB_ID(sensor_baro0), _baro_sub, &_baro); float baro_elapsed = (_baro.timestamp - baro_last) / 1e6f; + baro_last = _baro.timestamp; _ekf->updateDtHgtFilt(math::constrain(baro_elapsed, 0.001f, 0.1f)); _ekf->baroHgt = _baro.altitude; + _baro_alt_filt += (1 - expf(-baro_elapsed * rc)) * (_baro_last - _baro_alt_filt); + _baro_last = _baro.altitude; if (!_baro_init) { _baro_ref = _baro.altitude; @@ -1180,6 +1195,24 @@ FixedwingEstimator::task_main() float initVelNED[3]; + // maintain filtered baro and gps altitudes to calculate weather offset + // baro sample rate is ~70Hz and measurement bandwidth is high + // gps sample rate is 5Hz and altitude is assumed accurate when averaged over 30 seconds + // maintain heavily filtered values for both baro and gps altitude + // Assume the filtered output should be identical for both sensors + _baro_gps_offset = _baro_alt_filt - _gps_alt_filt; + if (hrt_elapsed_time(&_last_debug_print) >= 5e6) { + _last_debug_print = hrt_absolute_time(); + perf_print_counter(_perf_baro); + perf_reset(_perf_baro); + warnx("gpsoff: %5.1f, baro_alt_filt: %6.1f, gps_alt_filt: %6.1f, gpos.alt: %5.1f, lpos.z: %6.1f", + (double)_baro_gps_offset, + (double)_baro_alt_filt, + (double)_gps_alt_filt, + (double)_global_pos.alt, + (double)_local_pos.z); + } + /* Initialize the filter first */ if (!_gps_initialized && _gps.fix_type > 2 && _gps.eph < _parameters.pos_stddev_threshold && _gps.epv < _parameters.pos_stddev_threshold) { @@ -1195,7 +1228,13 @@ FixedwingEstimator::task_main() // Set up height correctly orb_copy(ORB_ID(sensor_baro0), _baro_sub, &_baro); _baro_ref_offset = _ekf->states[9]; // this should become zero in the local frame - _baro_gps_offset = _baro.altitude - gps_alt; + + // init filtered gps and baro altitudes + _gps_alt_filt = gps_alt; + _gps_last = gps_alt; + _baro_alt_filt = _baro.altitude; + _baro_last = _baro.altitude; + _ekf->baroHgt = _baro.altitude; _ekf->hgtMea = 1.0f * (_ekf->baroHgt - (_baro_ref)); @@ -1502,6 +1541,12 @@ FixedwingEstimator::task_main() _global_pos.timestamp = _local_pos.timestamp; + // FIXME: usurp terrain alt field for baro_gps_offset + _global_pos.terrain_alt = _baro_gps_offset; + _global_pos.terrain_alt_valid = true; + _global_pos.eph = _baro_alt_filt; + _global_pos.epv = _gps_alt_filt; + /* lazily publish the global position only once available */ if (_global_pos_pub > 0) { /* publish the global position */ From ea57dec24b14d94ec82f04b67bc31775b1f4a9cf Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Mon, 12 Jan 2015 17:15:38 -0700 Subject: [PATCH 478/652] revert to original ms5611 temperature sample rate --- src/drivers/ms5611/ms5611.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp index 6eda6933d9..0a793cbc97 100644 --- a/src/drivers/ms5611/ms5611.cpp +++ b/src/drivers/ms5611/ms5611.cpp @@ -90,7 +90,7 @@ static const int ERROR = -1; /* internal conversion time: 9.17 ms, so should not be read at rates higher than 100 Hz */ #define MS5611_CONVERSION_INTERVAL 10000 /* microseconds */ -#define MS5611_MEASUREMENT_RATIO 100 /* pressure measurements per temperature measurement */ +#define MS5611_MEASUREMENT_RATIO 3 /* pressure measurements per temperature measurement */ #define MS5611_BARO_DEVICE_PATH_EXT "/dev/ms5611_ext" #define MS5611_BARO_DEVICE_PATH_INT "/dev/ms5611_int" From dba0a5a90fd9fb87ba5a0083b0a009e8ac74b0ea Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Mon, 12 Jan 2015 17:23:30 -0700 Subject: [PATCH 479/652] comment out debug warnx --- .../ekf_att_pos_estimator_main.cpp | 22 +++++++++---------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 1770a75e53..8acf09977e 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -1201,17 +1201,17 @@ FixedwingEstimator::task_main() // maintain heavily filtered values for both baro and gps altitude // Assume the filtered output should be identical for both sensors _baro_gps_offset = _baro_alt_filt - _gps_alt_filt; - if (hrt_elapsed_time(&_last_debug_print) >= 5e6) { - _last_debug_print = hrt_absolute_time(); - perf_print_counter(_perf_baro); - perf_reset(_perf_baro); - warnx("gpsoff: %5.1f, baro_alt_filt: %6.1f, gps_alt_filt: %6.1f, gpos.alt: %5.1f, lpos.z: %6.1f", - (double)_baro_gps_offset, - (double)_baro_alt_filt, - (double)_gps_alt_filt, - (double)_global_pos.alt, - (double)_local_pos.z); - } +// if (hrt_elapsed_time(&_last_debug_print) >= 5e6) { +// _last_debug_print = hrt_absolute_time(); +// perf_print_counter(_perf_baro); +// perf_reset(_perf_baro); +// warnx("gpsoff: %5.1f, baro_alt_filt: %6.1f, gps_alt_filt: %6.1f, gpos.alt: %5.1f, lpos.z: %6.1f", +// (double)_baro_gps_offset, +// (double)_baro_alt_filt, +// (double)_gps_alt_filt, +// (double)_global_pos.alt, +// (double)_local_pos.z); +// } /* Initialize the filter first */ if (!_gps_initialized && _gps.fix_type > 2 && _gps.eph < _parameters.pos_stddev_threshold && _gps.epv < _parameters.pos_stddev_threshold) { From b795705640c6a14ba359e381c935351f9377aea9 Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Tue, 13 Jan 2015 08:53:11 -0700 Subject: [PATCH 480/652] improve efficiency of non-uniform rate LPFs --- .../ekf_att_pos_estimator_main.cpp | 11 +++-------- 1 file changed, 3 insertions(+), 8 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 8acf09977e..4ee2b70e65 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -786,8 +786,7 @@ FixedwingEstimator::task_main() // init lowpass filters for baro and gps altitude float _gps_alt_filt = 0, _baro_alt_filt = 0; - float _gps_last = 0, _baro_last = 0; - float rc = (1.0f/10); // actually 1/RC time constant of 1st order LPF + float rc = 10.0f; // RC time constant of 1st order LPF in seconds hrt_abstime baro_last = 0; _task_running = true; @@ -1065,8 +1064,7 @@ FixedwingEstimator::task_main() _ekf->gpsHgt = _gps.alt / 1e3f; // update LPF - _gps_alt_filt += (1 - expf(-gps_elapsed * rc)) * (_gps_last - _gps_alt_filt); - _gps_last = _ekf->gpsHgt; + _gps_alt_filt += (gps_elapsed / (rc + gps_elapsed)) * (_ekf->gpsHgt - _gps_alt_filt); //warnx("gps alt: %6.1f, interval: %6.3f", (double)_ekf->gpsHgt, (double)gps_elapsed); @@ -1104,8 +1102,7 @@ FixedwingEstimator::task_main() _ekf->updateDtHgtFilt(math::constrain(baro_elapsed, 0.001f, 0.1f)); _ekf->baroHgt = _baro.altitude; - _baro_alt_filt += (1 - expf(-baro_elapsed * rc)) * (_baro_last - _baro_alt_filt); - _baro_last = _baro.altitude; + _baro_alt_filt += (baro_elapsed/(rc + baro_elapsed)) * (_baro.altitude - _baro_alt_filt); if (!_baro_init) { _baro_ref = _baro.altitude; @@ -1231,9 +1228,7 @@ FixedwingEstimator::task_main() // init filtered gps and baro altitudes _gps_alt_filt = gps_alt; - _gps_last = gps_alt; _baro_alt_filt = _baro.altitude; - _baro_last = _baro.altitude; _ekf->baroHgt = _baro.altitude; _ekf->hgtMea = 1.0f * (_ekf->baroHgt - (_baro_ref)); From ddf65bae049fe4e4b053dc0b53fd6fe840a92aea Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Thu, 15 Jan 2015 11:09:31 +0100 Subject: [PATCH 481/652] Commander: Play tune on disarm and when safety switch is toggled --- src/modules/commander/commander.cpp | 20 ++++++++++++++++++++ 1 file changed, 20 insertions(+) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index ee20f57a1a..f511f38762 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1243,6 +1243,7 @@ int commander_thread_main(int argc, char *argv[]) orb_check(safety_sub, &updated); if (updated) { + bool previous_safety_off = safety.safety_off; orb_copy(ORB_ID(safety), safety_sub, &safety); /* disarm if safety is now on and still armed */ @@ -1256,6 +1257,19 @@ int commander_thread_main(int argc, char *argv[]) arming_state_changed = true; } } + + //Notify the user if the status of the safety switch changes + if(safety.safety_switch_available && previous_safety_off != safety.safety_off) { + + if(safety.safety_off) { + set_tune(TONE_NOTIFY_POSITIVE_TUNE); + } + else { + tune_neutral(true); + } + + status_changed = true; + } } /* update vtol vehicle status*/ @@ -1949,6 +1963,12 @@ int commander_thread_main(int argc, char *argv[]) /* reset arm_tune_played when disarmed */ if (!armed.armed || (safety.safety_switch_available && !safety.safety_off)) { + + //Notify the user that it is safe to approach the vehicle + if(arm_tune_played) { + tune_neutral(true); + } + arm_tune_played = false; } From d0af62783d1b18da6a15cb2da63e7ea88f5c398a Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Tue, 6 Jan 2015 11:39:22 +0100 Subject: [PATCH 482/652] uORB: Added vehicle_landed uORB topic --- src/modules/uORB/objects_common.cpp | 3 +++ .../uORB/topics/vehicle_land_detected.h | 23 +++++++++++++++++++ 2 files changed, 26 insertions(+) create mode 100644 src/modules/uORB/topics/vehicle_land_detected.h diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 293412455e..78fdf4de75 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -83,6 +83,9 @@ ORB_DEFINE(sensor_combined, struct sensor_combined_s); #include "topics/vehicle_gps_position.h" ORB_DEFINE(vehicle_gps_position, struct vehicle_gps_position_s); +#include "topics/vehicle_land_detected.h" +ORB_DEFINE(vehicle_land_detected, struct vehicle_land_detected_s); + #include "topics/satellite_info.h" ORB_DEFINE(satellite_info, struct satellite_info_s); diff --git a/src/modules/uORB/topics/vehicle_land_detected.h b/src/modules/uORB/topics/vehicle_land_detected.h new file mode 100644 index 0000000000..0de29498d4 --- /dev/null +++ b/src/modules/uORB/topics/vehicle_land_detected.h @@ -0,0 +1,23 @@ +#ifndef __TOPIC_VEHICLE_LANDED_H__ +#define __TOPIC_VEHICLE_LANDED_H__ + +#include "../uORB.h" + +/** + * @addtogroup topics + * @{ + */ + +struct vehicle_land_detected_s { + uint64_t timestamp; /**< timestamp of the setpoint */ + bool landed; /**< true if vehicle is currently landed on the ground*/ +}; + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(vehicle_land_detected); + +#endif //__TOPIC_VEHICLE_LANDED_H__ From 642063c3b8fdcd3f5e748666d1bb0412ea434b5f Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Tue, 6 Jan 2015 11:40:11 +0100 Subject: [PATCH 483/652] LandDetector: Added crude land detectors for multicopter and fixedwing --- makefiles/config_px4fmu-v2_default.mk | 6 + .../FixedwingLandDetector.cpp | 171 ++++++++++++++ .../fw_land_detector/FixedwingLandDetector.h | 107 +++++++++ .../fw_land_detector_main.cpp | 195 ++++++++++++++++ src/modules/fw_land_detector/module.mk | 10 + .../MulticopterLandDetector.cpp | 212 ++++++++++++++++++ .../MulticopterLandDetector.h | 116 ++++++++++ .../mc_land_detector_main.cpp | 195 ++++++++++++++++ src/modules/mc_land_detector/module.mk | 10 + 9 files changed, 1022 insertions(+) create mode 100644 src/modules/fw_land_detector/FixedwingLandDetector.cpp create mode 100644 src/modules/fw_land_detector/FixedwingLandDetector.h create mode 100644 src/modules/fw_land_detector/fw_land_detector_main.cpp create mode 100644 src/modules/fw_land_detector/module.mk create mode 100644 src/modules/mc_land_detector/MulticopterLandDetector.cpp create mode 100644 src/modules/mc_land_detector/MulticopterLandDetector.h create mode 100644 src/modules/mc_land_detector/mc_land_detector_main.cpp create mode 100644 src/modules/mc_land_detector/module.mk diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index 577140f054..8ef1c333ae 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -72,6 +72,12 @@ MODULES += modules/mavlink MODULES += modules/gpio_led MODULES += modules/uavcan +# +# Vehicle land detection +# +MODULES += modules/mc_land_detector +MODULES += modules/fw_land_detector + # # Estimation modules (EKF/ SO3 / other filters) # diff --git a/src/modules/fw_land_detector/FixedwingLandDetector.cpp b/src/modules/fw_land_detector/FixedwingLandDetector.cpp new file mode 100644 index 0000000000..6f5eca3ce0 --- /dev/null +++ b/src/modules/fw_land_detector/FixedwingLandDetector.cpp @@ -0,0 +1,171 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file FixedwingLandDetector.cpp + * Land detection algorithm for fixedwings + * + * @author Johan Jansen + */ + +#include "FixedwingLandDetector.h" + +#include +#include +#include +#include //usleep + +FixedwingLandDetector::FixedwingLandDetector() : + _landDetectedPub(-1), + _landDetected({0,false}), + + _vehicleLocalPositionSub(-1), + _vehicleLocalPosition({}), + _airspeedSub(-1), + _airspeed({}), + + _velocity_xy_filtered(0.0f), + _velocity_z_filtered(0.0f), + _airspeed_filtered(0.0f), + _landDetectTrigger(0), + + _taskShouldExit(false), + _taskIsRunning(false) +{ + //Advertise the first land detected uORB + _landDetected.timestamp = hrt_absolute_time(); + _landDetected.landed = false; + _landDetectedPub = orb_advertise(ORB_ID(vehicle_land_detected), &_landDetected); +} + +FixedwingLandDetector::~FixedwingLandDetector() +{ + _taskShouldExit = true; + close(_landDetectedPub); + } + +void FixedwingLandDetector::shutdown() +{ + _taskShouldExit = true; +} + +/** +* @brief Convinience function for polling uORB subscriptions +* @return true if there was new data and it was successfully copied +**/ +static bool orb_update(const struct orb_metadata *meta, int handle, void *buffer) +{ + bool newData = false; + + //Check if there is new data to grab + if(orb_check(handle, &newData) != OK) { + return false; + } + + if(!newData) { + return false; + } + + if(orb_copy(meta, handle, buffer) != OK) { + return false; + } + + return true; +} + +void FixedwingLandDetector::updateSubscriptions() +{ + orb_update(ORB_ID(vehicle_local_position), _vehicleLocalPositionSub, &_vehicleLocalPosition); + orb_update(ORB_ID(airspeed), _airspeedSub, &_airspeed); +} + +void FixedwingLandDetector::landDetectorLoop() +{ + //This should never happen! + if(_taskIsRunning) return; + + //Subscribe to local position and airspeed data + _vehicleLocalPositionSub = orb_subscribe(ORB_ID(vehicle_local_position)); + _airspeedSub = orb_subscribe(ORB_ID(airspeed)); + + _taskIsRunning = true; + _taskShouldExit = false; + while (!_taskShouldExit) { + + //First poll for new data from our subscriptions + updateSubscriptions(); + + const uint64_t now = hrt_absolute_time(); + bool landDetected = false; + + //TODO: reset filtered values on arming? + _velocity_xy_filtered = 0.95f*_velocity_xy_filtered + 0.05f*sqrtf(_vehicleLocalPosition.vx*_vehicleLocalPosition.vx + _vehicleLocalPosition.vy*_vehicleLocalPosition.vy); + _velocity_z_filtered = 0.95f*_velocity_z_filtered + 0.05f*fabsf(_vehicleLocalPosition.vz); + _airspeed_filtered = 0.95f*_airspeed_filtered + 0.05f*_airspeed.true_airspeed_m_s; + + /* crude land detector for fixedwing */ + if (_velocity_xy_filtered < FW_LAND_DETECTOR_VELOCITY_MAX + && _velocity_z_filtered < FW_LAND_DETECTOR_CLIMBRATE_MAX + && _airspeed_filtered < FW_LAND_DETECTOR_AIRSPEED_MAX) { + + //These conditions need to be stable for a period of time before we trust them + if(now > _landDetectTrigger) { + landDetected = true; + } + } + else { + //reset land detect trigger + _landDetectTrigger = now + FW_LAND_DETECTOR_TRIGGER_TIME; + } + + //Publish if land detection state has changed + if(_landDetected.landed != landDetected) { + _landDetected.timestamp = now; + _landDetected.landed = landDetected; + + /* publish the land detected broadcast */ + orb_publish(ORB_ID(vehicle_land_detected), _landDetectedPub, &_landDetected); + } + + //Limit loop rate + usleep(1000000 / FW_LAND_DETECTOR_UPDATE_RATE); + } + + _taskIsRunning = false; + _exit(0); +} + +bool FixedwingLandDetector::isRunning() const +{ + return _taskIsRunning; +} diff --git a/src/modules/fw_land_detector/FixedwingLandDetector.h b/src/modules/fw_land_detector/FixedwingLandDetector.h new file mode 100644 index 0000000000..6a0f951dd1 --- /dev/null +++ b/src/modules/fw_land_detector/FixedwingLandDetector.h @@ -0,0 +1,107 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file FixedwingLandDetector.h + * Land detection algorithm for multicopters + * + * @author Johan Jansen + * @author Morten Lysgaard + */ + +#ifndef __FIXED_WING_LAND_DETECTOR_H__ +#define __FIXED_WING_LAND_DETECTOR_H__ + +#include +#include +#include +#include + +class FixedwingLandDetector { +public: + FixedwingLandDetector(); + ~FixedwingLandDetector(); + + /** + * @brief Executes the main loop of the land detector in a separate deamon thread + * @returns OK if task was successfully launched + **/ + int createDeamonThread(); + + /** + * @returns true if this task is currently running + **/ + bool isRunning() const; + + /** + * @brief blocking loop, should be run in a separate thread or task. Runs at 50Hz + **/ + void landDetectorLoop(); + + /** + * @brief Tells the Land Detector task that it should exit + **/ + void shutdown(); + +protected: + /** + * @brief polls all subscriptions and pulls any data that has changed + **/ + void updateSubscriptions(); + + //Algorithm parameters (TODO: should probably be externalized) + static constexpr uint32_t FW_LAND_DETECTOR_UPDATE_RATE = 50; /**< Run algorithm at 50Hz */ + static constexpr uint64_t FW_LAND_DETECTOR_TRIGGER_TIME = 2000000; /**< usec that landing conditions have to hold before triggering a land */ + static constexpr float FW_LAND_DETECTOR_VELOCITY_MAX = 5.0f; /**< maximum horizontal movement m/s*/ + static constexpr float FW_LAND_DETECTOR_CLIMBRATE_MAX = 10.00f; /**< +- climb rate in m/s*/ + static constexpr float FW_LAND_DETECTOR_AIRSPEED_MAX = 10.00f; /**< airspeed max m/s*/ + +private: + orb_advert_t _landDetectedPub; /**< publisher for position in local frame */ + struct vehicle_land_detected_s _landDetected; /**< local vehicle position */ + + int _vehicleLocalPositionSub; /**< notification of local position */ + struct vehicle_local_position_s _vehicleLocalPosition; /**< the result from local position subscription */ + int _airspeedSub; + struct airspeed_s _airspeed; + + float _velocity_xy_filtered; + float _velocity_z_filtered; + float _airspeed_filtered; + uint64_t _landDetectTrigger; + + bool _taskShouldExit; /**< true if it is requested that this task should exit */ + bool _taskIsRunning; /**< task has reached main loop and is currently running */ +}; + +#endif //__FIXED_WING_LAND_DETECTOR_H__ diff --git a/src/modules/fw_land_detector/fw_land_detector_main.cpp b/src/modules/fw_land_detector/fw_land_detector_main.cpp new file mode 100644 index 0000000000..a9eefc4064 --- /dev/null +++ b/src/modules/fw_land_detector/fw_land_detector_main.cpp @@ -0,0 +1,195 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file fw_land_detector_main.cpp + * Land detection algorithm for fixed wings + * + * @author Johan Jansen + */ + +#include //usleep +#include +#include +#include +#include +#include +#include //Scheduler +#include //print to console + +#include "FixedwingLandDetector.h" + +//Function prototypes +static int fw_land_detector_start(); +static void fw_land_detector_stop(); + +/** + * land detector app start / stop handling function + * This makes the land detector module accessible from the nuttx shell + * @ingroup apps + */ +extern "C" __EXPORT int fw_land_detector_main(int argc, char *argv[]); + +//Private variables +static FixedwingLandDetector* fw_land_detector_task = nullptr; +static int _landDetectorTaskID = -1; + +/** +* Deamon thread function +**/ +static void fw_land_detector_deamon_thread(int argc, char *argv[]) +{ + fw_land_detector_task->landDetectorLoop(); +} + +/** +* Stop the task, force killing it if it doesn't stop by itself +**/ +static void fw_land_detector_stop() +{ + if(fw_land_detector_task == nullptr || _landDetectorTaskID == -1) { + errx(1, "not running"); + return; + } + + fw_land_detector_task->shutdown(); + + //Wait for task to die + int i = 0; + do { + /* wait 20ms */ + usleep(20000); + + /* if we have given up, kill it */ + if (++i > 50) { + task_delete(_landDetectorTaskID); + break; + } + } while (fw_land_detector_task->isRunning()); + + + delete fw_land_detector_task; + fw_land_detector_task = nullptr; + _landDetectorTaskID = -1; + warn("fw_land_detector has been stopped"); +} + +/** +* Start new task, fails if it is already running. Returns OK if successful +**/ +static int fw_land_detector_start() +{ + if(fw_land_detector_task != nullptr || _landDetectorTaskID != -1) { + errx(1, "already running"); + return -1; + } + + //Allocate memory + fw_land_detector_task = new FixedwingLandDetector(); + if (fw_land_detector_task == nullptr) { + errx(1, "alloc failed"); + return -1; + } + + //Start new thread task + _landDetectorTaskID = task_spawn_cmd("fw_land_detector", + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, + 1024, + (main_t)&fw_land_detector_deamon_thread, + nullptr); + + if (_landDetectorTaskID < 0) { + errx(1, "task start failed: %d", -errno); + return -1; + } + + /* avoid memory fragmentation by not exiting start handler until the task has fully started */ + const uint32_t timeout = hrt_absolute_time() + 5000000; //5 second timeout + while (!fw_land_detector_task->isRunning()) { + usleep(50000); + printf("."); + fflush(stdout); + + if(hrt_absolute_time() > timeout) { + err(1, "start failed - timeout"); + fw_land_detector_stop(); + exit(1); + } + } + printf("\n"); + + exit(0); + return 0; +} + +/** +* Main entry point for this module +**/ +int fw_land_detector_main(int argc, char *argv[]) +{ + + if (argc < 1) { + warnx("usage: fw_land_detector {start|stop|status}"); + exit(0); + } + + if (!strcmp(argv[1], "start")) { + fw_land_detector_start(); + } + + if (!strcmp(argv[1], "stop")) { + fw_land_detector_stop(); + exit(0); + } + + if (!strcmp(argv[1], "status")) { + if (fw_land_detector_task) { + + if(fw_land_detector_task->isRunning()) { + warnx("running"); + } + else { + errx(1, "exists, but not running"); + } + + exit(0); + + } else { + errx(1, "not running"); + } + } + + warn("usage: fw_land_detector {start|stop|status}"); + return 1; +} diff --git a/src/modules/fw_land_detector/module.mk b/src/modules/fw_land_detector/module.mk new file mode 100644 index 0000000000..894b29a7b5 --- /dev/null +++ b/src/modules/fw_land_detector/module.mk @@ -0,0 +1,10 @@ +# +# Fixedwing land detector +# + +MODULE_COMMAND = fw_land_detector + +SRCS = fw_land_detector_main.cpp \ + FixedwingLandDetector.cpp + +EXTRACXXFLAGS = -Weffc++ \ No newline at end of file diff --git a/src/modules/mc_land_detector/MulticopterLandDetector.cpp b/src/modules/mc_land_detector/MulticopterLandDetector.cpp new file mode 100644 index 0000000000..39064de6b5 --- /dev/null +++ b/src/modules/mc_land_detector/MulticopterLandDetector.cpp @@ -0,0 +1,212 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file MultiCopterLandDetector.cpp + * Land detection algorithm for multicopters + * + * @author Johan Jansen + * @author Morten Lysgaard + */ + +#include "MulticopterLandDetector.h" + +#include +#include +#include +#include //usleep + +MulticopterLandDetector::MulticopterLandDetector() : + _landDetectedPub(-1), + _landDetected({0,false}), + + _vehicleGlobalPositionSub(-1), + _sensorsCombinedSub(-1), + _waypointSub(-1), + _actuatorsSub(-1), + _armingSub(-1), + + _vehicleGlobalPosition({}), + _sensors({}), + _waypoint({}), + _actuators({}), + _arming({}), + + _taskShouldExit(false), + _taskIsRunning(false), + _landTimer(0) +{ + //Advertise the first land detected uORB + _landDetected.timestamp = hrt_absolute_time(); + _landDetected.landed = false; + _landDetectedPub = orb_advertise(ORB_ID(vehicle_land_detected), &_landDetected); +} + +MulticopterLandDetector::~MulticopterLandDetector() +{ + _taskShouldExit = true; + close(_landDetectedPub); +} + +/** +* @brief Convinience function for polling uORB subscriptions +* @return true if there was new data and it was successfully copied +**/ +static bool orb_update(const struct orb_metadata *meta, int handle, void *buffer) +{ + bool newData = false; + + //Check if there is new data to grab + if(orb_check(handle, &newData) != OK) { + return false; + } + + if(!newData) { + return false; + } + + if(orb_copy(meta, handle, buffer) != OK) { + return false; + } + + return true; +} + +void MulticopterLandDetector::shutdown() +{ + _taskShouldExit = true; +} + +void MulticopterLandDetector::updateSubscriptions() +{ + orb_update(ORB_ID(vehicle_global_position), _vehicleGlobalPositionSub, &_vehicleGlobalPosition); + orb_update(ORB_ID(sensor_combined), _sensorsCombinedSub, &_sensors); + orb_update(ORB_ID(position_setpoint_triplet), _waypointSub, &_waypoint); + orb_update(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _actuatorsSub, &_actuators); + orb_update(ORB_ID(actuator_armed), _armingSub, &_arming); +} + +void MulticopterLandDetector::landDetectorLoop() +{ + //This should never happen! + if(_taskIsRunning) return; + + //Subscribe to position, attitude, arming and velocity changes + _vehicleGlobalPositionSub = orb_subscribe(ORB_ID(vehicle_global_position)); + _sensorsCombinedSub = orb_subscribe(ORB_ID(sensor_combined)); + _waypointSub = orb_subscribe(ORB_ID(position_setpoint_triplet)); + _actuatorsSub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); + _armingSub = orb_subscribe(ORB_ID(actuator_armed)); + + //Begin task + _taskIsRunning = true; + _taskShouldExit = false; + while (!_taskShouldExit) { + + //First poll for new data from our subscriptions + updateSubscriptions(); + + const uint64_t now = hrt_absolute_time(); + + //only detect landing if the autopilot is actively trying to land + if(!_waypoint.current.valid || _waypoint.current.type != SETPOINT_TYPE_LAND) { + _landTimer = now; + } + else { + /* + static int debugcnt = 0; + if(debugcnt++ > 12) { + debugcnt = 0; + mavlink_log_critical(_mavlinkFd, "T: %.4f R: %.4f", (double)_actuators.control[3], + sqrt( _sensors.gyro_rad_s[0]*_sensors.gyro_rad_s[0]+ + _sensors.gyro_rad_s[1]*_sensors.gyro_rad_s[1]+ + _sensors.gyro_rad_s[2]*_sensors.gyro_rad_s[2])); + } + */ + + //Check if we are moving vertically + bool verticalMovement = fabsf(_vehicleGlobalPosition.vel_d) > MC_LAND_DETECTOR_CLIMBRATE_MAX; + + bool horizontalMovement = sqrtf(_vehicleGlobalPosition.vel_n*_vehicleGlobalPosition.vel_n + + _vehicleGlobalPosition.vel_e*_vehicleGlobalPosition.vel_e) > MC_LAND_DETECTOR_VELOCITY_MAX; + + //Next look if all rotation angles are not moving + bool rotating = sqrtf(_sensors.gyro_rad_s[0]*_sensors.gyro_rad_s[0]+ + _sensors.gyro_rad_s[1]*_sensors.gyro_rad_s[1]+ + _sensors.gyro_rad_s[2]*_sensors.gyro_rad_s[2]) > MC_LAND_DETECTOR_ROTATION_MAX; + + //Check if thrust output is minimal (about half of default) + bool minimalThrust = _arming.armed && _actuators.control[3] <= MC_LAND_DETECTOR_THRUST_MAX; + + if(verticalMovement || rotating || !minimalThrust || horizontalMovement) { + //Sensed movement, so reset the land detector + _landTimer = now; + } + + } + + // if we have detected a landing for 2 continious seconds + if(now-_landTimer > 2000000) { + if(!_landDetected.landed) + { + _landDetected.timestamp = now; + _landDetected.landed = true; + + /* publish the land detected broadcast */ + orb_publish(ORB_ID(vehicle_land_detected), _landDetectedPub, &_landDetected); + } + } + else { + // if we currently think we have landed, but the latest data says we are flying + if(_landDetected.landed) + { + _landDetected.timestamp = now; + _landDetected.landed = false; + + /* publish the land detected broadcast */ + orb_publish(ORB_ID(vehicle_land_detected), _landDetectedPub, &_landDetected); + } + } + + //Limit loop rate + usleep(1000000 / MC_LAND_DETECTOR_UPDATE_RATE); + } + + _taskIsRunning = false; + _exit(0); +} + +bool MulticopterLandDetector::isRunning() const +{ + return _taskIsRunning; +} diff --git a/src/modules/mc_land_detector/MulticopterLandDetector.h b/src/modules/mc_land_detector/MulticopterLandDetector.h new file mode 100644 index 0000000000..7421898c12 --- /dev/null +++ b/src/modules/mc_land_detector/MulticopterLandDetector.h @@ -0,0 +1,116 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file MultiCopterLandDetector.h + * Land detection algorithm for multicopters + * + * @author Johan Jansen + * @author Morten Lysgaard + */ + +#ifndef __MULTICOPTER_LAND_DETECTOR_H__ +#define __MULTICOPTER_LAND_DETECTOR_H__ + +#include +#include +#include +#include +#include +#include +#include + +//TODO: add crash detection to this module? +class MulticopterLandDetector { +public: + MulticopterLandDetector(); + ~MulticopterLandDetector(); + + /** + * @brief Executes the main loop of the land detector in a separate deamon thread + * @returns OK if task was successfully launched + **/ + int createDeamonThread(); + + /** + * @returns true if this task is currently running + **/ + bool isRunning() const; + + /** + * @brief blocking loop, should be run in a separate thread or task. Runs at 50Hz + **/ + void landDetectorLoop(); + + /** + * @brief Tells the Land Detector task that it should exit + **/ + void shutdown(); + +protected: + /** + * @brief polls all subscriptions and pulls any data that has changed + **/ + void updateSubscriptions(); + + //Algorithm parameters (TODO: should probably be externalized) + static constexpr float MC_LAND_DETECTOR_CLIMBRATE_MAX = 0.30f; /**< max +- climb rate in m/s */ + static constexpr float MC_LAND_DETECTOR_ROTATION_MAX = 0.5f; /**< max rotation in rad/sec (= 30 deg/s) */ + static constexpr float MC_LAND_DETECTOR_THRUST_MAX = 0.2f; + static constexpr float MC_LAND_DETECTOR_VELOCITY_MAX = 1.0f; /**< max +- horizontal movement in m/s */ + static constexpr uint32_t MC_LAND_DETECTOR_UPDATE_RATE = 50; /**< Run algorithm at 50Hz */ + static constexpr uint32_t MC_LAND_DETECTOR_TRIGGER_TIME = 2000000; /**< usec that landing conditions have to hold before triggering a land */ + +private: + orb_advert_t _landDetectedPub; /**< publisher for position in local frame */ + struct vehicle_land_detected_s _landDetected; /**< local vehicle position */ + + int _vehicleGlobalPositionSub; /**< notification of global position */ + int _sensorsCombinedSub; + int _waypointSub; + int _actuatorsSub; + int _armingSub; + + struct vehicle_global_position_s _vehicleGlobalPosition; /**< the result from global position subscription */ + struct sensor_combined_s _sensors; /**< subscribe to sensor readings */ + struct position_setpoint_triplet_s _waypoint; /**< subscribe to autopilot navigation */ + struct actuator_controls_s _actuators; + struct actuator_armed_s _arming; + + bool _taskShouldExit; /**< true if it is requested that this task should exit */ + bool _taskIsRunning; /**< task has reached main loop and is currently running */ + + uint64_t _landTimer; /**< timestamp in microseconds since a possible land was detected*/ +}; + +#endif //__MULTICOPTER_LAND_DETECTOR_H__ diff --git a/src/modules/mc_land_detector/mc_land_detector_main.cpp b/src/modules/mc_land_detector/mc_land_detector_main.cpp new file mode 100644 index 0000000000..2992ead99c --- /dev/null +++ b/src/modules/mc_land_detector/mc_land_detector_main.cpp @@ -0,0 +1,195 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mc_land_detector_main.cpp + * Land detection algorithm for multicopters + * + * @author Johan Jansen + */ + +#include //usleep +#include +#include +#include +#include +#include +#include //Scheduler +#include //print to console + +#include "MulticopterLandDetector.h" + +//Function prototypes +static int mc_land_detector_start(); +static void mc_land_detector_stop(); + +/** + * land detector app start / stop handling function + * This makes the land detector module accessible from the nuttx shell + * @ingroup apps + */ +extern "C" __EXPORT int mc_land_detector_main(int argc, char *argv[]); + +//Private variables +static MulticopterLandDetector* mc_land_detector_task = nullptr; +static int _landDetectorTaskID = -1; + +/** +* Deamon thread function +**/ +static void mc_land_detector_deamon_thread(int argc, char *argv[]) +{ + mc_land_detector_task->landDetectorLoop(); +} + +/** +* Stop the task, force killing it if it doesn't stop by itself +**/ +static void mc_land_detector_stop() +{ + if(mc_land_detector_task == nullptr || _landDetectorTaskID == -1) { + errx(1, "not running"); + return; + } + + mc_land_detector_task->shutdown(); + + //Wait for task to die + int i = 0; + do { + /* wait 20ms */ + usleep(20000); + + /* if we have given up, kill it */ + if (++i > 50) { + task_delete(_landDetectorTaskID); + break; + } + } while (mc_land_detector_task->isRunning()); + + + delete mc_land_detector_task; + mc_land_detector_task = nullptr; + _landDetectorTaskID = -1; + warn("mc_land_detector has been stopped"); +} + +/** +* Start new task, fails if it is already running. Returns OK if successful +**/ +static int mc_land_detector_start() +{ + if(mc_land_detector_task != nullptr || _landDetectorTaskID != -1) { + errx(1, "already running"); + return -1; + } + + //Allocate memory + mc_land_detector_task = new MulticopterLandDetector(); + if (mc_land_detector_task == nullptr) { + errx(1, "alloc failed"); + return -1; + } + + //Start new thread task + _landDetectorTaskID = task_spawn_cmd("mc_land_detector", + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, + 1024, + (main_t)&mc_land_detector_deamon_thread, + nullptr); + + if (_landDetectorTaskID < 0) { + errx(1, "task start failed: %d", -errno); + return -1; + } + + /* avoid memory fragmentation by not exiting start handler until the task has fully started */ + const uint32_t timeout = hrt_absolute_time() + 5000000; //5 second timeout + while (!mc_land_detector_task->isRunning()) { + usleep(50000); + printf("."); + fflush(stdout); + + if(hrt_absolute_time() > timeout) { + err(1, "start failed - timeout"); + mc_land_detector_stop(); + exit(1); + } + } + printf("\n"); + + exit(0); + return 0; +} + +/** +* Main entry point for this module +**/ +int mc_land_detector_main(int argc, char *argv[]) +{ + + if (argc < 1) { + warnx("usage: mc_land_detector {start|stop|status}"); + exit(0); + } + + if (!strcmp(argv[1], "start")) { + mc_land_detector_start(); + } + + if (!strcmp(argv[1], "stop")) { + mc_land_detector_stop(); + exit(0); + } + + if (!strcmp(argv[1], "status")) { + if (mc_land_detector_task) { + + if(mc_land_detector_task->isRunning()) { + warnx("running"); + } + else { + errx(1, "exists, but not running"); + } + + exit(0); + + } else { + errx(1, "not running"); + } + } + + warn("usage: mc_land_detector {start|stop|status}"); + return 1; +} diff --git a/src/modules/mc_land_detector/module.mk b/src/modules/mc_land_detector/module.mk new file mode 100644 index 0000000000..023d48979a --- /dev/null +++ b/src/modules/mc_land_detector/module.mk @@ -0,0 +1,10 @@ +# +# Multirotor land detector +# + +MODULE_COMMAND = mc_land_detector + +SRCS = mc_land_detector_main.cpp \ + MulticopterLandDetector.cpp + +EXTRACXXFLAGS = -Weffc++ From cffba8440e056b71a01003a0d90a7e6f4cffd648 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Tue, 6 Jan 2015 11:46:59 +0100 Subject: [PATCH 484/652] EKF: Removed the fixed wing land detector in the EKF module --- .../ekf_att_pos_estimator_main.cpp | 25 +------------------ 1 file changed, 1 insertion(+), 24 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index c41777968e..866e5dc841 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -290,10 +290,6 @@ private: AttPosEKF *_ekf; - float _velocity_xy_filtered; - float _velocity_z_filtered; - float _airspeed_filtered; - /** * Update our local parameter cache. */ @@ -422,10 +418,7 @@ FixedwingEstimator::FixedwingEstimator() : _mavlink_fd(-1), _parameters{}, _parameter_handles{}, - _ekf(nullptr), - _velocity_xy_filtered(0.0f), - _velocity_z_filtered(0.0f), - _airspeed_filtered(0.0f) + _ekf(nullptr) { _last_run = hrt_absolute_time(); @@ -1434,22 +1427,6 @@ FixedwingEstimator::task_main() _local_pos.v_z_valid = true; _local_pos.xy_global = true; - _velocity_xy_filtered = 0.95f*_velocity_xy_filtered + 0.05f*sqrtf(_local_pos.vx*_local_pos.vx + _local_pos.vy*_local_pos.vy); - _velocity_z_filtered = 0.95f*_velocity_z_filtered + 0.05f*fabsf(_local_pos.vz); - _airspeed_filtered = 0.95f*_airspeed_filtered + 0.05f*_airspeed.true_airspeed_m_s; - - - /* crude land detector for fixedwing only, - * TODO: adapt so that it works for both, maybe move to another location - */ - if (_velocity_xy_filtered < 5 - && _velocity_z_filtered < 10 - && _airspeed_filtered < 10) { - _local_pos.landed = true; - } else { - _local_pos.landed = false; - } - _local_pos.z_global = false; _local_pos.yaw = _att.yaw; From 051a6972281dc9f8c15b5d8c73ac808416944932 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Tue, 6 Jan 2015 11:53:00 +0100 Subject: [PATCH 485/652] uORB: Added missing license header --- .../uORB/topics/vehicle_land_detected.h | 40 +++++++++++++++++++ 1 file changed, 40 insertions(+) diff --git a/src/modules/uORB/topics/vehicle_land_detected.h b/src/modules/uORB/topics/vehicle_land_detected.h index 0de29498d4..51b3568e82 100644 --- a/src/modules/uORB/topics/vehicle_land_detected.h +++ b/src/modules/uORB/topics/vehicle_land_detected.h @@ -1,3 +1,43 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file vehicle_land_detected.h + * Land detected uORB topic + * + * @author Johan Jansen + */ + #ifndef __TOPIC_VEHICLE_LANDED_H__ #define __TOPIC_VEHICLE_LANDED_H__ From 3a4b3d094a07d41c84efabdc134763bec9372597 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Tue, 6 Jan 2015 11:53:47 +0100 Subject: [PATCH 486/652] LandDetector: Removed commented debug info --- .../mc_land_detector/MulticopterLandDetector.cpp | 10 ---------- 1 file changed, 10 deletions(-) diff --git a/src/modules/mc_land_detector/MulticopterLandDetector.cpp b/src/modules/mc_land_detector/MulticopterLandDetector.cpp index 39064de6b5..63dc54d5ef 100644 --- a/src/modules/mc_land_detector/MulticopterLandDetector.cpp +++ b/src/modules/mc_land_detector/MulticopterLandDetector.cpp @@ -143,16 +143,6 @@ void MulticopterLandDetector::landDetectorLoop() _landTimer = now; } else { - /* - static int debugcnt = 0; - if(debugcnt++ > 12) { - debugcnt = 0; - mavlink_log_critical(_mavlinkFd, "T: %.4f R: %.4f", (double)_actuators.control[3], - sqrt( _sensors.gyro_rad_s[0]*_sensors.gyro_rad_s[0]+ - _sensors.gyro_rad_s[1]*_sensors.gyro_rad_s[1]+ - _sensors.gyro_rad_s[2]*_sensors.gyro_rad_s[2])); - } - */ //Check if we are moving vertically bool verticalMovement = fabsf(_vehicleGlobalPosition.vel_d) > MC_LAND_DETECTOR_CLIMBRATE_MAX; From b5c7c6a15b4badf56c335746b32ef138afaca539 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Tue, 6 Jan 2015 11:59:15 +0100 Subject: [PATCH 487/652] ROMFS: Added the respective land detector to the startup scripts --- ROMFS/px4fmu_common/init.d/rc.fw_apps | 9 +++++++++ ROMFS/px4fmu_common/init.d/rc.mc_apps | 5 +++++ 2 files changed, 14 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_apps b/ROMFS/px4fmu_common/init.d/rc.fw_apps index 9aca3fc5f4..c6947009bb 100644 --- a/ROMFS/px4fmu_common/init.d/rc.fw_apps +++ b/ROMFS/px4fmu_common/init.d/rc.fw_apps @@ -13,3 +13,12 @@ ekf_att_pos_estimator start # fw_att_control start fw_pos_control_l1 start + +# +# Start Land Detector +# +fw_land_detector start + +# +# Misc apps +# \ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_apps b/ROMFS/px4fmu_common/init.d/rc.mc_apps index 268eb9bba0..9c3d6352b3 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_apps +++ b/ROMFS/px4fmu_common/init.d/rc.mc_apps @@ -10,3 +10,8 @@ position_estimator_inav start mc_att_control start mc_pos_control start + +# +# Start Land Detector +# +mc_land_detector start From eefbf366fbc2bef58f0bc283f7d02fb49023faa6 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Tue, 6 Jan 2015 12:16:07 +0100 Subject: [PATCH 488/652] LandDetector: Fixed some typos and magic constant --- src/modules/fw_land_detector/FixedwingLandDetector.h | 3 +-- src/modules/mc_land_detector/MulticopterLandDetector.cpp | 5 +++-- src/modules/mc_land_detector/MulticopterLandDetector.h | 1 - 3 files changed, 4 insertions(+), 5 deletions(-) diff --git a/src/modules/fw_land_detector/FixedwingLandDetector.h b/src/modules/fw_land_detector/FixedwingLandDetector.h index 6a0f951dd1..9004457400 100644 --- a/src/modules/fw_land_detector/FixedwingLandDetector.h +++ b/src/modules/fw_land_detector/FixedwingLandDetector.h @@ -33,10 +33,9 @@ /** * @file FixedwingLandDetector.h - * Land detection algorithm for multicopters + * Land detection algorithm for fixedwing * * @author Johan Jansen - * @author Morten Lysgaard */ #ifndef __FIXED_WING_LAND_DETECTOR_H__ diff --git a/src/modules/mc_land_detector/MulticopterLandDetector.cpp b/src/modules/mc_land_detector/MulticopterLandDetector.cpp index 63dc54d5ef..90b36a795c 100644 --- a/src/modules/mc_land_detector/MulticopterLandDetector.cpp +++ b/src/modules/mc_land_detector/MulticopterLandDetector.cpp @@ -147,6 +147,7 @@ void MulticopterLandDetector::landDetectorLoop() //Check if we are moving vertically bool verticalMovement = fabsf(_vehicleGlobalPosition.vel_d) > MC_LAND_DETECTOR_CLIMBRATE_MAX; + //Check if we are moving horizontally bool horizontalMovement = sqrtf(_vehicleGlobalPosition.vel_n*_vehicleGlobalPosition.vel_n + _vehicleGlobalPosition.vel_e*_vehicleGlobalPosition.vel_e) > MC_LAND_DETECTOR_VELOCITY_MAX; @@ -165,8 +166,8 @@ void MulticopterLandDetector::landDetectorLoop() } - // if we have detected a landing for 2 continious seconds - if(now-_landTimer > 2000000) { + // if we have detected a landing for 2 continuous seconds + if(now-_landTimer > MC_LAND_DETECTOR_TRIGGER_TIME) { if(!_landDetected.landed) { _landDetected.timestamp = now; diff --git a/src/modules/mc_land_detector/MulticopterLandDetector.h b/src/modules/mc_land_detector/MulticopterLandDetector.h index 7421898c12..1aeaa0d62a 100644 --- a/src/modules/mc_land_detector/MulticopterLandDetector.h +++ b/src/modules/mc_land_detector/MulticopterLandDetector.h @@ -50,7 +50,6 @@ #include #include -//TODO: add crash detection to this module? class MulticopterLandDetector { public: MulticopterLandDetector(); From 28adc8850075da70206864c9f285456bb32c086c Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Tue, 6 Jan 2015 12:25:18 +0100 Subject: [PATCH 489/652] Commander: Subscribe and use land detector --- src/modules/commander/commander.cpp | 16 ++++++++++++++-- 1 file changed, 14 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index f511f38762..142ff978c4 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -84,6 +84,7 @@ #include #include #include + #include #include #include @@ -964,6 +965,11 @@ int commander_thread_main(int argc, char *argv[]) struct vehicle_local_position_s local_position; memset(&local_position, 0, sizeof(local_position)); + /* Subscribe to land detector */ + int land_detector_sub = orb_subscribe(ORB_ID(vehicle_land_detected)); + struct vehicle_land_detected_s land_detector; + memset(&land_detector, 0, sizeof(land_detector)); + /* * The home position is set based on GPS only, to prevent a dependency between * position estimator and commander. RAW GPS is more than good enough for a @@ -1379,9 +1385,15 @@ int commander_thread_main(int argc, char *argv[]) check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.z_valid, &(status.condition_local_altitude_valid), &status_changed); + /* Update land detector */ + orb_check(land_detector_sub, &updated); + if(updated) { + orb_copy(ORB_ID(vehicle_land_detected), land_detector_sub, &land_detector); + } + if (status.condition_local_altitude_valid) { - if (status.condition_landed != local_position.landed) { - status.condition_landed = local_position.landed; + if (status.condition_landed != land_detector.landed) { + status.condition_landed = land_detector.landed; status_changed = true; if (status.condition_landed) { From 98ab83142c1bf7e5170e7527dde1a9e5132b5422 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Tue, 6 Jan 2015 12:31:05 +0100 Subject: [PATCH 490/652] InertialNav: Removed land detector from position estimator --- .../position_estimator_inav_main.c | 15 +++++++++------ src/modules/uORB/topics/vehicle_local_position.h | 2 +- 2 files changed, 10 insertions(+), 7 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 2f972fc9f2..b418e3a76b 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -246,9 +246,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float baro_offset = 0.0f; // baro offset for reference altitude, initialized on start, then adjusted float surface_offset = 0.0f; // ground level offset from reference altitude float surface_offset_rate = 0.0f; // surface offset change rate - float alt_avg = 0.0f; - bool landed = true; - hrt_abstime landed_time = 0; + //float alt_avg = 0.0f; + //bool landed = true; + //hrt_abstime landed_time = 0; hrt_abstime accel_timestamp = 0; hrt_abstime baro_timestamp = 0; @@ -1068,12 +1068,14 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) inertial_filter_correct(-y_est[1], dt, y_est, 1, params.w_xy_res_v); } - /* detect land */ + +/* + // detect land alt_avg += (- z_est[0] - alt_avg) * dt / params.land_t; float alt_disp2 = - z_est[0] - alt_avg; alt_disp2 = alt_disp2 * alt_disp2; float land_disp2 = params.land_disp * params.land_disp; - /* get actual thrust output */ + // get actual thrust output float thrust = armed.armed ? actuator.control[3] : 0.0f; if (landed) { @@ -1097,6 +1099,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) landed_time = 0; } } +*/ if (verbose_mode) { /* print updates rate */ @@ -1148,7 +1151,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) local_pos.vy = y_est[1]; local_pos.z = z_est[0]; local_pos.vz = z_est[1]; - local_pos.landed = landed; + local_pos.landed = false; local_pos.yaw = att.yaw; local_pos.dist_bottom_valid = dist_bottom_valid; local_pos.eph = eph; diff --git a/src/modules/uORB/topics/vehicle_local_position.h b/src/modules/uORB/topics/vehicle_local_position.h index 5d39c897d4..0e0bc9781a 100644 --- a/src/modules/uORB/topics/vehicle_local_position.h +++ b/src/modules/uORB/topics/vehicle_local_position.h @@ -77,7 +77,7 @@ struct vehicle_local_position_s { double ref_lat; /**< Reference point latitude in degrees */ double ref_lon; /**< Reference point longitude in degrees */ float ref_alt; /**< Reference altitude AMSL in meters, MUST be set to current (not at reference point!) ground level */ - bool landed; /**< true if vehicle is landed */ + bool landed; /**< true if vehicle is landed TODO: deprecated, use vehicle_land_detected instead (remove this line?)*/ /* Distance to surface */ float dist_bottom; /**< Distance to bottom surface (ground) */ float dist_bottom_rate; /**< Distance to bottom surface (ground) change rate */ From 546b5727b442ac7520d7ce72e15732378a1a0799 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Tue, 6 Jan 2015 13:33:35 +0100 Subject: [PATCH 491/652] Formatting: Run AStyle to fix indenting --- .../FixedwingLandDetector.cpp | 156 +++++++------- .../fw_land_detector/FixedwingLandDetector.h | 88 ++++---- .../fw_land_detector_main.cpp | 172 +++++++-------- .../MulticopterLandDetector.cpp | 203 +++++++++--------- .../MulticopterLandDetector.h | 95 ++++---- .../mc_land_detector_main.cpp | 30 +-- 6 files changed, 378 insertions(+), 366 deletions(-) diff --git a/src/modules/fw_land_detector/FixedwingLandDetector.cpp b/src/modules/fw_land_detector/FixedwingLandDetector.cpp index 6f5eca3ce0..5fbeb2f45b 100644 --- a/src/modules/fw_land_detector/FixedwingLandDetector.cpp +++ b/src/modules/fw_land_detector/FixedwingLandDetector.cpp @@ -46,37 +46,37 @@ #include //usleep FixedwingLandDetector::FixedwingLandDetector() : - _landDetectedPub(-1), - _landDetected({0,false}), - - _vehicleLocalPositionSub(-1), - _vehicleLocalPosition({}), - _airspeedSub(-1), - _airspeed({}), + _landDetectedPub(-1), + _landDetected({0, false}), - _velocity_xy_filtered(0.0f), - _velocity_z_filtered(0.0f), - _airspeed_filtered(0.0f), - _landDetectTrigger(0), + _vehicleLocalPositionSub(-1), + _vehicleLocalPosition({}), + _airspeedSub(-1), + _airspeed({}), - _taskShouldExit(false), - _taskIsRunning(false) + _velocity_xy_filtered(0.0f), + _velocity_z_filtered(0.0f), + _airspeed_filtered(0.0f), + _landDetectTrigger(0), + + _taskShouldExit(false), + _taskIsRunning(false) { - //Advertise the first land detected uORB - _landDetected.timestamp = hrt_absolute_time(); - _landDetected.landed = false; - _landDetectedPub = orb_advertise(ORB_ID(vehicle_land_detected), &_landDetected); + //Advertise the first land detected uORB + _landDetected.timestamp = hrt_absolute_time(); + _landDetected.landed = false; + _landDetectedPub = orb_advertise(ORB_ID(vehicle_land_detected), &_landDetected); } FixedwingLandDetector::~FixedwingLandDetector() { - _taskShouldExit = true; - close(_landDetectedPub); - } + _taskShouldExit = true; + close(_landDetectedPub); +} void FixedwingLandDetector::shutdown() { - _taskShouldExit = true; + _taskShouldExit = true; } /** @@ -85,87 +85,89 @@ void FixedwingLandDetector::shutdown() **/ static bool orb_update(const struct orb_metadata *meta, int handle, void *buffer) { - bool newData = false; + bool newData = false; - //Check if there is new data to grab - if(orb_check(handle, &newData) != OK) { - return false; - } + //Check if there is new data to grab + if (orb_check(handle, &newData) != OK) { + return false; + } - if(!newData) { - return false; - } + if (!newData) { + return false; + } - if(orb_copy(meta, handle, buffer) != OK) { - return false; - } + if (orb_copy(meta, handle, buffer) != OK) { + return false; + } - return true; + return true; } void FixedwingLandDetector::updateSubscriptions() { - orb_update(ORB_ID(vehicle_local_position), _vehicleLocalPositionSub, &_vehicleLocalPosition); - orb_update(ORB_ID(airspeed), _airspeedSub, &_airspeed); + orb_update(ORB_ID(vehicle_local_position), _vehicleLocalPositionSub, &_vehicleLocalPosition); + orb_update(ORB_ID(airspeed), _airspeedSub, &_airspeed); } void FixedwingLandDetector::landDetectorLoop() { - //This should never happen! - if(_taskIsRunning) return; + //This should never happen! + if (_taskIsRunning) { return; } - //Subscribe to local position and airspeed data - _vehicleLocalPositionSub = orb_subscribe(ORB_ID(vehicle_local_position)); - _airspeedSub = orb_subscribe(ORB_ID(airspeed)); + //Subscribe to local position and airspeed data + _vehicleLocalPositionSub = orb_subscribe(ORB_ID(vehicle_local_position)); + _airspeedSub = orb_subscribe(ORB_ID(airspeed)); - _taskIsRunning = true; - _taskShouldExit = false; - while (!_taskShouldExit) { + _taskIsRunning = true; + _taskShouldExit = false; - //First poll for new data from our subscriptions - updateSubscriptions(); + while (!_taskShouldExit) { - const uint64_t now = hrt_absolute_time(); - bool landDetected = false; + //First poll for new data from our subscriptions + updateSubscriptions(); - //TODO: reset filtered values on arming? - _velocity_xy_filtered = 0.95f*_velocity_xy_filtered + 0.05f*sqrtf(_vehicleLocalPosition.vx*_vehicleLocalPosition.vx + _vehicleLocalPosition.vy*_vehicleLocalPosition.vy); - _velocity_z_filtered = 0.95f*_velocity_z_filtered + 0.05f*fabsf(_vehicleLocalPosition.vz); - _airspeed_filtered = 0.95f*_airspeed_filtered + 0.05f*_airspeed.true_airspeed_m_s; + const uint64_t now = hrt_absolute_time(); + bool landDetected = false; - /* crude land detector for fixedwing */ - if (_velocity_xy_filtered < FW_LAND_DETECTOR_VELOCITY_MAX - && _velocity_z_filtered < FW_LAND_DETECTOR_CLIMBRATE_MAX - && _airspeed_filtered < FW_LAND_DETECTOR_AIRSPEED_MAX) { + //TODO: reset filtered values on arming? + _velocity_xy_filtered = 0.95f * _velocity_xy_filtered + 0.05f * sqrtf(_vehicleLocalPosition.vx * + _vehicleLocalPosition.vx + _vehicleLocalPosition.vy * _vehicleLocalPosition.vy); + _velocity_z_filtered = 0.95f * _velocity_z_filtered + 0.05f * fabsf(_vehicleLocalPosition.vz); + _airspeed_filtered = 0.95f * _airspeed_filtered + 0.05f * _airspeed.true_airspeed_m_s; - //These conditions need to be stable for a period of time before we trust them - if(now > _landDetectTrigger) { - landDetected = true; - } - } - else { - //reset land detect trigger - _landDetectTrigger = now + FW_LAND_DETECTOR_TRIGGER_TIME; - } + /* crude land detector for fixedwing */ + if (_velocity_xy_filtered < FW_LAND_DETECTOR_VELOCITY_MAX + && _velocity_z_filtered < FW_LAND_DETECTOR_CLIMBRATE_MAX + && _airspeed_filtered < FW_LAND_DETECTOR_AIRSPEED_MAX) { - //Publish if land detection state has changed - if(_landDetected.landed != landDetected) { - _landDetected.timestamp = now; - _landDetected.landed = landDetected; + //These conditions need to be stable for a period of time before we trust them + if (now > _landDetectTrigger) { + landDetected = true; + } - /* publish the land detected broadcast */ - orb_publish(ORB_ID(vehicle_land_detected), _landDetectedPub, &_landDetected); - } + } else { + //reset land detect trigger + _landDetectTrigger = now + FW_LAND_DETECTOR_TRIGGER_TIME; + } - //Limit loop rate - usleep(1000000 / FW_LAND_DETECTOR_UPDATE_RATE); - } + //Publish if land detection state has changed + if (_landDetected.landed != landDetected) { + _landDetected.timestamp = now; + _landDetected.landed = landDetected; - _taskIsRunning = false; - _exit(0); + /* publish the land detected broadcast */ + orb_publish(ORB_ID(vehicle_land_detected), _landDetectedPub, &_landDetected); + } + + //Limit loop rate + usleep(1000000 / FW_LAND_DETECTOR_UPDATE_RATE); + } + + _taskIsRunning = false; + _exit(0); } bool FixedwingLandDetector::isRunning() const { - return _taskIsRunning; + return _taskIsRunning; } diff --git a/src/modules/fw_land_detector/FixedwingLandDetector.h b/src/modules/fw_land_detector/FixedwingLandDetector.h index 9004457400..be9b17b74b 100644 --- a/src/modules/fw_land_detector/FixedwingLandDetector.h +++ b/src/modules/fw_land_detector/FixedwingLandDetector.h @@ -46,61 +46,63 @@ #include #include -class FixedwingLandDetector { +class FixedwingLandDetector +{ public: - FixedwingLandDetector(); - ~FixedwingLandDetector(); + FixedwingLandDetector(); + ~FixedwingLandDetector(); - /** - * @brief Executes the main loop of the land detector in a separate deamon thread - * @returns OK if task was successfully launched - **/ - int createDeamonThread(); + /** + * @brief Executes the main loop of the land detector in a separate deamon thread + * @returns OK if task was successfully launched + **/ + int createDeamonThread(); - /** - * @returns true if this task is currently running - **/ - bool isRunning() const; + /** + * @returns true if this task is currently running + **/ + bool isRunning() const; - /** - * @brief blocking loop, should be run in a separate thread or task. Runs at 50Hz - **/ - void landDetectorLoop(); + /** + * @brief blocking loop, should be run in a separate thread or task. Runs at 50Hz + **/ + void landDetectorLoop(); - /** - * @brief Tells the Land Detector task that it should exit - **/ - void shutdown(); + /** + * @brief Tells the Land Detector task that it should exit + **/ + void shutdown(); protected: - /** - * @brief polls all subscriptions and pulls any data that has changed - **/ - void updateSubscriptions(); + /** + * @brief polls all subscriptions and pulls any data that has changed + **/ + void updateSubscriptions(); - //Algorithm parameters (TODO: should probably be externalized) - static constexpr uint32_t FW_LAND_DETECTOR_UPDATE_RATE = 50; /**< Run algorithm at 50Hz */ - static constexpr uint64_t FW_LAND_DETECTOR_TRIGGER_TIME = 2000000; /**< usec that landing conditions have to hold before triggering a land */ - static constexpr float FW_LAND_DETECTOR_VELOCITY_MAX = 5.0f; /**< maximum horizontal movement m/s*/ - static constexpr float FW_LAND_DETECTOR_CLIMBRATE_MAX = 10.00f; /**< +- climb rate in m/s*/ - static constexpr float FW_LAND_DETECTOR_AIRSPEED_MAX = 10.00f; /**< airspeed max m/s*/ + //Algorithm parameters (TODO: should probably be externalized) + static constexpr uint32_t FW_LAND_DETECTOR_UPDATE_RATE = 50; /**< Run algorithm at 50Hz */ + static constexpr uint64_t FW_LAND_DETECTOR_TRIGGER_TIME = + 2000000; /**< usec that landing conditions have to hold before triggering a land */ + static constexpr float FW_LAND_DETECTOR_VELOCITY_MAX = 5.0f; /**< maximum horizontal movement m/s*/ + static constexpr float FW_LAND_DETECTOR_CLIMBRATE_MAX = 10.00f; /**< +- climb rate in m/s*/ + static constexpr float FW_LAND_DETECTOR_AIRSPEED_MAX = 10.00f; /**< airspeed max m/s*/ private: - orb_advert_t _landDetectedPub; /**< publisher for position in local frame */ - struct vehicle_land_detected_s _landDetected; /**< local vehicle position */ + orb_advert_t _landDetectedPub; /**< publisher for position in local frame */ + struct vehicle_land_detected_s _landDetected; /**< local vehicle position */ - int _vehicleLocalPositionSub; /**< notification of local position */ - struct vehicle_local_position_s _vehicleLocalPosition; /**< the result from local position subscription */ - int _airspeedSub; - struct airspeed_s _airspeed; + int _vehicleLocalPositionSub; /**< notification of local position */ + struct vehicle_local_position_s _vehicleLocalPosition; /**< the result from local position subscription */ + int _airspeedSub; + struct airspeed_s _airspeed; - float _velocity_xy_filtered; - float _velocity_z_filtered; - float _airspeed_filtered; - uint64_t _landDetectTrigger; - - bool _taskShouldExit; /**< true if it is requested that this task should exit */ - bool _taskIsRunning; /**< task has reached main loop and is currently running */ + float _velocity_xy_filtered; + float _velocity_z_filtered; + float _airspeed_filtered; + uint64_t _landDetectTrigger; + + bool _taskShouldExit; /**< true if it is requested that this task should exit */ + bool _taskIsRunning; /**< task has reached main loop and is currently running */ }; #endif //__FIXED_WING_LAND_DETECTOR_H__ diff --git a/src/modules/fw_land_detector/fw_land_detector_main.cpp b/src/modules/fw_land_detector/fw_land_detector_main.cpp index a9eefc4064..a45a518639 100644 --- a/src/modules/fw_land_detector/fw_land_detector_main.cpp +++ b/src/modules/fw_land_detector/fw_land_detector_main.cpp @@ -61,7 +61,7 @@ static void fw_land_detector_stop(); extern "C" __EXPORT int fw_land_detector_main(int argc, char *argv[]); //Private variables -static FixedwingLandDetector* fw_land_detector_task = nullptr; +static FixedwingLandDetector *fw_land_detector_task = nullptr; static int _landDetectorTaskID = -1; /** @@ -69,7 +69,7 @@ static int _landDetectorTaskID = -1; **/ static void fw_land_detector_deamon_thread(int argc, char *argv[]) { - fw_land_detector_task->landDetectorLoop(); + fw_land_detector_task->landDetectorLoop(); } /** @@ -77,31 +77,32 @@ static void fw_land_detector_deamon_thread(int argc, char *argv[]) **/ static void fw_land_detector_stop() { - if(fw_land_detector_task == nullptr || _landDetectorTaskID == -1) { - errx(1, "not running"); - return; - } + if (fw_land_detector_task == nullptr || _landDetectorTaskID == -1) { + errx(1, "not running"); + return; + } - fw_land_detector_task->shutdown(); + fw_land_detector_task->shutdown(); - //Wait for task to die - int i = 0; - do { - /* wait 20ms */ - usleep(20000); + //Wait for task to die + int i = 0; - /* if we have given up, kill it */ - if (++i > 50) { - task_delete(_landDetectorTaskID); - break; - } - } while (fw_land_detector_task->isRunning()); + do { + /* wait 20ms */ + usleep(20000); + + /* if we have given up, kill it */ + if (++i > 50) { + task_delete(_landDetectorTaskID); + break; + } + } while (fw_land_detector_task->isRunning()); - delete fw_land_detector_task; - fw_land_detector_task = nullptr; - _landDetectorTaskID = -1; - warn("fw_land_detector has been stopped"); + delete fw_land_detector_task; + fw_land_detector_task = nullptr; + _landDetectorTaskID = -1; + warn("fw_land_detector has been stopped"); } /** @@ -109,48 +110,51 @@ static void fw_land_detector_stop() **/ static int fw_land_detector_start() { - if(fw_land_detector_task != nullptr || _landDetectorTaskID != -1) { - errx(1, "already running"); - return -1; - } + if (fw_land_detector_task != nullptr || _landDetectorTaskID != -1) { + errx(1, "already running"); + return -1; + } - //Allocate memory - fw_land_detector_task = new FixedwingLandDetector(); - if (fw_land_detector_task == nullptr) { - errx(1, "alloc failed"); - return -1; - } + //Allocate memory + fw_land_detector_task = new FixedwingLandDetector(); - //Start new thread task - _landDetectorTaskID = task_spawn_cmd("fw_land_detector", - SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT, - 1024, - (main_t)&fw_land_detector_deamon_thread, - nullptr); + if (fw_land_detector_task == nullptr) { + errx(1, "alloc failed"); + return -1; + } - if (_landDetectorTaskID < 0) { - errx(1, "task start failed: %d", -errno); - return -1; - } + //Start new thread task + _landDetectorTaskID = task_spawn_cmd("fw_land_detector", + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, + 1024, + (main_t)&fw_land_detector_deamon_thread, + nullptr); - /* avoid memory fragmentation by not exiting start handler until the task has fully started */ - const uint32_t timeout = hrt_absolute_time() + 5000000; //5 second timeout - while (!fw_land_detector_task->isRunning()) { - usleep(50000); - printf("."); - fflush(stdout); + if (_landDetectorTaskID < 0) { + errx(1, "task start failed: %d", -errno); + return -1; + } - if(hrt_absolute_time() > timeout) { - err(1, "start failed - timeout"); - fw_land_detector_stop(); - exit(1); - } - } - printf("\n"); + /* avoid memory fragmentation by not exiting start handler until the task has fully started */ + const uint32_t timeout = hrt_absolute_time() + 5000000; //5 second timeout - exit(0); - return 0; + while (!fw_land_detector_task->isRunning()) { + usleep(50000); + printf("."); + fflush(stdout); + + if (hrt_absolute_time() > timeout) { + err(1, "start failed - timeout"); + fw_land_detector_stop(); + exit(1); + } + } + + printf("\n"); + + exit(0); + return 0; } /** @@ -159,37 +163,37 @@ static int fw_land_detector_start() int fw_land_detector_main(int argc, char *argv[]) { - if (argc < 1) { - warnx("usage: fw_land_detector {start|stop|status}"); - exit(0); - } + if (argc < 1) { + warnx("usage: fw_land_detector {start|stop|status}"); + exit(0); + } - if (!strcmp(argv[1], "start")) { - fw_land_detector_start(); - } + if (!strcmp(argv[1], "start")) { + fw_land_detector_start(); + } - if (!strcmp(argv[1], "stop")) { - fw_land_detector_stop(); - exit(0); - } + if (!strcmp(argv[1], "stop")) { + fw_land_detector_stop(); + exit(0); + } - if (!strcmp(argv[1], "status")) { - if (fw_land_detector_task) { + if (!strcmp(argv[1], "status")) { + if (fw_land_detector_task) { - if(fw_land_detector_task->isRunning()) { - warnx("running"); - } - else { - errx(1, "exists, but not running"); - } + if (fw_land_detector_task->isRunning()) { + warnx("running"); - exit(0); + } else { + errx(1, "exists, but not running"); + } - } else { - errx(1, "not running"); - } - } + exit(0); - warn("usage: fw_land_detector {start|stop|status}"); - return 1; + } else { + errx(1, "not running"); + } + } + + warn("usage: fw_land_detector {start|stop|status}"); + return 1; } diff --git a/src/modules/mc_land_detector/MulticopterLandDetector.cpp b/src/modules/mc_land_detector/MulticopterLandDetector.cpp index 90b36a795c..d23ff9017a 100644 --- a/src/modules/mc_land_detector/MulticopterLandDetector.cpp +++ b/src/modules/mc_land_detector/MulticopterLandDetector.cpp @@ -47,35 +47,35 @@ #include //usleep MulticopterLandDetector::MulticopterLandDetector() : - _landDetectedPub(-1), - _landDetected({0,false}), - - _vehicleGlobalPositionSub(-1), - _sensorsCombinedSub(-1), - _waypointSub(-1), - _actuatorsSub(-1), - _armingSub(-1), + _landDetectedPub(-1), + _landDetected({0, false}), - _vehicleGlobalPosition({}), - _sensors({}), - _waypoint({}), - _actuators({}), - _arming({}), + _vehicleGlobalPositionSub(-1), + _sensorsCombinedSub(-1), + _waypointSub(-1), + _actuatorsSub(-1), + _armingSub(-1), - _taskShouldExit(false), - _taskIsRunning(false), - _landTimer(0) + _vehicleGlobalPosition({}), + _sensors({}), + _waypoint({}), + _actuators({}), + _arming({}), + + _taskShouldExit(false), + _taskIsRunning(false), + _landTimer(0) { - //Advertise the first land detected uORB - _landDetected.timestamp = hrt_absolute_time(); - _landDetected.landed = false; - _landDetectedPub = orb_advertise(ORB_ID(vehicle_land_detected), &_landDetected); + //Advertise the first land detected uORB + _landDetected.timestamp = hrt_absolute_time(); + _landDetected.landed = false; + _landDetectedPub = orb_advertise(ORB_ID(vehicle_land_detected), &_landDetected); } MulticopterLandDetector::~MulticopterLandDetector() { - _taskShouldExit = true; - close(_landDetectedPub); + _taskShouldExit = true; + close(_landDetectedPub); } /** @@ -84,120 +84,119 @@ MulticopterLandDetector::~MulticopterLandDetector() **/ static bool orb_update(const struct orb_metadata *meta, int handle, void *buffer) { - bool newData = false; + bool newData = false; - //Check if there is new data to grab - if(orb_check(handle, &newData) != OK) { - return false; - } + //Check if there is new data to grab + if (orb_check(handle, &newData) != OK) { + return false; + } - if(!newData) { - return false; - } + if (!newData) { + return false; + } - if(orb_copy(meta, handle, buffer) != OK) { - return false; - } + if (orb_copy(meta, handle, buffer) != OK) { + return false; + } - return true; + return true; } void MulticopterLandDetector::shutdown() { - _taskShouldExit = true; + _taskShouldExit = true; } void MulticopterLandDetector::updateSubscriptions() { - orb_update(ORB_ID(vehicle_global_position), _vehicleGlobalPositionSub, &_vehicleGlobalPosition); - orb_update(ORB_ID(sensor_combined), _sensorsCombinedSub, &_sensors); - orb_update(ORB_ID(position_setpoint_triplet), _waypointSub, &_waypoint); - orb_update(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _actuatorsSub, &_actuators); - orb_update(ORB_ID(actuator_armed), _armingSub, &_arming); + orb_update(ORB_ID(vehicle_global_position), _vehicleGlobalPositionSub, &_vehicleGlobalPosition); + orb_update(ORB_ID(sensor_combined), _sensorsCombinedSub, &_sensors); + orb_update(ORB_ID(position_setpoint_triplet), _waypointSub, &_waypoint); + orb_update(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _actuatorsSub, &_actuators); + orb_update(ORB_ID(actuator_armed), _armingSub, &_arming); } void MulticopterLandDetector::landDetectorLoop() { - //This should never happen! - if(_taskIsRunning) return; + //This should never happen! + if (_taskIsRunning) { return; } - //Subscribe to position, attitude, arming and velocity changes - _vehicleGlobalPositionSub = orb_subscribe(ORB_ID(vehicle_global_position)); - _sensorsCombinedSub = orb_subscribe(ORB_ID(sensor_combined)); - _waypointSub = orb_subscribe(ORB_ID(position_setpoint_triplet)); - _actuatorsSub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); - _armingSub = orb_subscribe(ORB_ID(actuator_armed)); + //Subscribe to position, attitude, arming and velocity changes + _vehicleGlobalPositionSub = orb_subscribe(ORB_ID(vehicle_global_position)); + _sensorsCombinedSub = orb_subscribe(ORB_ID(sensor_combined)); + _waypointSub = orb_subscribe(ORB_ID(position_setpoint_triplet)); + _actuatorsSub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); + _armingSub = orb_subscribe(ORB_ID(actuator_armed)); - //Begin task - _taskIsRunning = true; - _taskShouldExit = false; - while (!_taskShouldExit) { + //Begin task + _taskIsRunning = true; + _taskShouldExit = false; - //First poll for new data from our subscriptions - updateSubscriptions(); + while (!_taskShouldExit) { - const uint64_t now = hrt_absolute_time(); + //First poll for new data from our subscriptions + updateSubscriptions(); - //only detect landing if the autopilot is actively trying to land - if(!_waypoint.current.valid || _waypoint.current.type != SETPOINT_TYPE_LAND) { - _landTimer = now; - } - else { + const uint64_t now = hrt_absolute_time(); - //Check if we are moving vertically - bool verticalMovement = fabsf(_vehicleGlobalPosition.vel_d) > MC_LAND_DETECTOR_CLIMBRATE_MAX; + //only detect landing if the autopilot is actively trying to land + if (!_waypoint.current.valid || _waypoint.current.type != SETPOINT_TYPE_LAND) { + _landTimer = now; - //Check if we are moving horizontally - bool horizontalMovement = sqrtf(_vehicleGlobalPosition.vel_n*_vehicleGlobalPosition.vel_n - + _vehicleGlobalPosition.vel_e*_vehicleGlobalPosition.vel_e) > MC_LAND_DETECTOR_VELOCITY_MAX; + } else { - //Next look if all rotation angles are not moving - bool rotating = sqrtf(_sensors.gyro_rad_s[0]*_sensors.gyro_rad_s[0]+ - _sensors.gyro_rad_s[1]*_sensors.gyro_rad_s[1]+ - _sensors.gyro_rad_s[2]*_sensors.gyro_rad_s[2]) > MC_LAND_DETECTOR_ROTATION_MAX; + //Check if we are moving vertically + bool verticalMovement = fabsf(_vehicleGlobalPosition.vel_d) > MC_LAND_DETECTOR_CLIMBRATE_MAX; - //Check if thrust output is minimal (about half of default) - bool minimalThrust = _arming.armed && _actuators.control[3] <= MC_LAND_DETECTOR_THRUST_MAX; + //Check if we are moving horizontally + bool horizontalMovement = sqrtf(_vehicleGlobalPosition.vel_n * _vehicleGlobalPosition.vel_n + + _vehicleGlobalPosition.vel_e * _vehicleGlobalPosition.vel_e) > MC_LAND_DETECTOR_VELOCITY_MAX; - if(verticalMovement || rotating || !minimalThrust || horizontalMovement) { - //Sensed movement, so reset the land detector - _landTimer = now; - } + //Next look if all rotation angles are not moving + bool rotating = sqrtf(_sensors.gyro_rad_s[0] * _sensors.gyro_rad_s[0] + + _sensors.gyro_rad_s[1] * _sensors.gyro_rad_s[1] + + _sensors.gyro_rad_s[2] * _sensors.gyro_rad_s[2]) > MC_LAND_DETECTOR_ROTATION_MAX; - } + //Check if thrust output is minimal (about half of default) + bool minimalThrust = _arming.armed && _actuators.control[3] <= MC_LAND_DETECTOR_THRUST_MAX; - // if we have detected a landing for 2 continuous seconds - if(now-_landTimer > MC_LAND_DETECTOR_TRIGGER_TIME) { - if(!_landDetected.landed) - { - _landDetected.timestamp = now; - _landDetected.landed = true; + if (verticalMovement || rotating || !minimalThrust || horizontalMovement) { + //Sensed movement, so reset the land detector + _landTimer = now; + } - /* publish the land detected broadcast */ - orb_publish(ORB_ID(vehicle_land_detected), _landDetectedPub, &_landDetected); - } - } - else { - // if we currently think we have landed, but the latest data says we are flying - if(_landDetected.landed) - { - _landDetected.timestamp = now; - _landDetected.landed = false; + } - /* publish the land detected broadcast */ - orb_publish(ORB_ID(vehicle_land_detected), _landDetectedPub, &_landDetected); - } - } + // if we have detected a landing for 2 continuous seconds + if (now - _landTimer > MC_LAND_DETECTOR_TRIGGER_TIME) { + if (!_landDetected.landed) { + _landDetected.timestamp = now; + _landDetected.landed = true; - //Limit loop rate - usleep(1000000 / MC_LAND_DETECTOR_UPDATE_RATE); - } + /* publish the land detected broadcast */ + orb_publish(ORB_ID(vehicle_land_detected), _landDetectedPub, &_landDetected); + } - _taskIsRunning = false; - _exit(0); + } else { + // if we currently think we have landed, but the latest data says we are flying + if (_landDetected.landed) { + _landDetected.timestamp = now; + _landDetected.landed = false; + + /* publish the land detected broadcast */ + orb_publish(ORB_ID(vehicle_land_detected), _landDetectedPub, &_landDetected); + } + } + + //Limit loop rate + usleep(1000000 / MC_LAND_DETECTOR_UPDATE_RATE); + } + + _taskIsRunning = false; + _exit(0); } bool MulticopterLandDetector::isRunning() const { - return _taskIsRunning; + return _taskIsRunning; } diff --git a/src/modules/mc_land_detector/MulticopterLandDetector.h b/src/modules/mc_land_detector/MulticopterLandDetector.h index 1aeaa0d62a..fe18374c3b 100644 --- a/src/modules/mc_land_detector/MulticopterLandDetector.h +++ b/src/modules/mc_land_detector/MulticopterLandDetector.h @@ -50,66 +50,67 @@ #include #include -class MulticopterLandDetector { +class MulticopterLandDetector +{ public: - MulticopterLandDetector(); - ~MulticopterLandDetector(); + MulticopterLandDetector(); + ~MulticopterLandDetector(); - /** - * @brief Executes the main loop of the land detector in a separate deamon thread - * @returns OK if task was successfully launched - **/ - int createDeamonThread(); + /** + * @brief Executes the main loop of the land detector in a separate deamon thread + * @returns OK if task was successfully launched + **/ + int createDeamonThread(); - /** - * @returns true if this task is currently running - **/ - bool isRunning() const; + /** + * @returns true if this task is currently running + **/ + bool isRunning() const; - /** - * @brief blocking loop, should be run in a separate thread or task. Runs at 50Hz - **/ - void landDetectorLoop(); + /** + * @brief blocking loop, should be run in a separate thread or task. Runs at 50Hz + **/ + void landDetectorLoop(); - /** - * @brief Tells the Land Detector task that it should exit - **/ - void shutdown(); + /** + * @brief Tells the Land Detector task that it should exit + **/ + void shutdown(); protected: - /** - * @brief polls all subscriptions and pulls any data that has changed - **/ - void updateSubscriptions(); + /** + * @brief polls all subscriptions and pulls any data that has changed + **/ + void updateSubscriptions(); - //Algorithm parameters (TODO: should probably be externalized) - static constexpr float MC_LAND_DETECTOR_CLIMBRATE_MAX = 0.30f; /**< max +- climb rate in m/s */ - static constexpr float MC_LAND_DETECTOR_ROTATION_MAX = 0.5f; /**< max rotation in rad/sec (= 30 deg/s) */ - static constexpr float MC_LAND_DETECTOR_THRUST_MAX = 0.2f; - static constexpr float MC_LAND_DETECTOR_VELOCITY_MAX = 1.0f; /**< max +- horizontal movement in m/s */ - static constexpr uint32_t MC_LAND_DETECTOR_UPDATE_RATE = 50; /**< Run algorithm at 50Hz */ - static constexpr uint32_t MC_LAND_DETECTOR_TRIGGER_TIME = 2000000; /**< usec that landing conditions have to hold before triggering a land */ + //Algorithm parameters (TODO: should probably be externalized) + static constexpr float MC_LAND_DETECTOR_CLIMBRATE_MAX = 0.30f; /**< max +- climb rate in m/s */ + static constexpr float MC_LAND_DETECTOR_ROTATION_MAX = 0.5f; /**< max rotation in rad/sec (= 30 deg/s) */ + static constexpr float MC_LAND_DETECTOR_THRUST_MAX = 0.2f; + static constexpr float MC_LAND_DETECTOR_VELOCITY_MAX = 1.0f; /**< max +- horizontal movement in m/s */ + static constexpr uint32_t MC_LAND_DETECTOR_UPDATE_RATE = 50; /**< Run algorithm at 50Hz */ + static constexpr uint32_t MC_LAND_DETECTOR_TRIGGER_TIME = 2000000; /**< usec that landing conditions have to hold before triggering a land */ private: - orb_advert_t _landDetectedPub; /**< publisher for position in local frame */ - struct vehicle_land_detected_s _landDetected; /**< local vehicle position */ + orb_advert_t _landDetectedPub; /**< publisher for position in local frame */ + struct vehicle_land_detected_s _landDetected; /**< local vehicle position */ - int _vehicleGlobalPositionSub; /**< notification of global position */ - int _sensorsCombinedSub; - int _waypointSub; - int _actuatorsSub; - int _armingSub; + int _vehicleGlobalPositionSub; /**< notification of global position */ + int _sensorsCombinedSub; + int _waypointSub; + int _actuatorsSub; + int _armingSub; - struct vehicle_global_position_s _vehicleGlobalPosition; /**< the result from global position subscription */ - struct sensor_combined_s _sensors; /**< subscribe to sensor readings */ - struct position_setpoint_triplet_s _waypoint; /**< subscribe to autopilot navigation */ - struct actuator_controls_s _actuators; - struct actuator_armed_s _arming; - - bool _taskShouldExit; /**< true if it is requested that this task should exit */ - bool _taskIsRunning; /**< task has reached main loop and is currently running */ + struct vehicle_global_position_s _vehicleGlobalPosition; /**< the result from global position subscription */ + struct sensor_combined_s _sensors; /**< subscribe to sensor readings */ + struct position_setpoint_triplet_s _waypoint; /**< subscribe to autopilot navigation */ + struct actuator_controls_s _actuators; + struct actuator_armed_s _arming; - uint64_t _landTimer; /**< timestamp in microseconds since a possible land was detected*/ + bool _taskShouldExit; /**< true if it is requested that this task should exit */ + bool _taskIsRunning; /**< task has reached main loop and is currently running */ + + uint64_t _landTimer; /**< timestamp in microseconds since a possible land was detected*/ }; #endif //__MULTICOPTER_LAND_DETECTOR_H__ diff --git a/src/modules/mc_land_detector/mc_land_detector_main.cpp b/src/modules/mc_land_detector/mc_land_detector_main.cpp index 2992ead99c..6ad3f82a45 100644 --- a/src/modules/mc_land_detector/mc_land_detector_main.cpp +++ b/src/modules/mc_land_detector/mc_land_detector_main.cpp @@ -61,7 +61,7 @@ static void mc_land_detector_stop(); extern "C" __EXPORT int mc_land_detector_main(int argc, char *argv[]); //Private variables -static MulticopterLandDetector* mc_land_detector_task = nullptr; +static MulticopterLandDetector *mc_land_detector_task = nullptr; static int _landDetectorTaskID = -1; /** @@ -77,7 +77,7 @@ static void mc_land_detector_deamon_thread(int argc, char *argv[]) **/ static void mc_land_detector_stop() { - if(mc_land_detector_task == nullptr || _landDetectorTaskID == -1) { + if (mc_land_detector_task == nullptr || _landDetectorTaskID == -1) { errx(1, "not running"); return; } @@ -86,6 +86,7 @@ static void mc_land_detector_stop() //Wait for task to die int i = 0; + do { /* wait 20ms */ usleep(20000); @@ -109,13 +110,14 @@ static void mc_land_detector_stop() **/ static int mc_land_detector_start() { - if(mc_land_detector_task != nullptr || _landDetectorTaskID != -1) { + if (mc_land_detector_task != nullptr || _landDetectorTaskID != -1) { errx(1, "already running"); return -1; } //Allocate memory mc_land_detector_task = new MulticopterLandDetector(); + if (mc_land_detector_task == nullptr) { errx(1, "alloc failed"); return -1; @@ -123,11 +125,11 @@ static int mc_land_detector_start() //Start new thread task _landDetectorTaskID = task_spawn_cmd("mc_land_detector", - SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT, - 1024, - (main_t)&mc_land_detector_deamon_thread, - nullptr); + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, + 1024, + (main_t)&mc_land_detector_deamon_thread, + nullptr); if (_landDetectorTaskID < 0) { errx(1, "task start failed: %d", -errno); @@ -136,17 +138,19 @@ static int mc_land_detector_start() /* avoid memory fragmentation by not exiting start handler until the task has fully started */ const uint32_t timeout = hrt_absolute_time() + 5000000; //5 second timeout + while (!mc_land_detector_task->isRunning()) { usleep(50000); printf("."); fflush(stdout); - if(hrt_absolute_time() > timeout) { + if (hrt_absolute_time() > timeout) { err(1, "start failed - timeout"); mc_land_detector_stop(); exit(1); } } + printf("\n"); exit(0); @@ -170,16 +174,16 @@ int mc_land_detector_main(int argc, char *argv[]) if (!strcmp(argv[1], "stop")) { mc_land_detector_stop(); - exit(0); + exit(0); } if (!strcmp(argv[1], "status")) { if (mc_land_detector_task) { - if(mc_land_detector_task->isRunning()) { + if (mc_land_detector_task->isRunning()) { warnx("running"); - } - else { + + } else { errx(1, "exists, but not running"); } From 57ed27304a394fb9fec8e2ae4bfca9b2a77d6c7e Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Tue, 6 Jan 2015 13:56:06 +0100 Subject: [PATCH 492/652] HIL: Added land detector to HIL simulation --- src/modules/mavlink/mavlink_receiver.cpp | 21 ++++++++++++++++++--- src/modules/mavlink/mavlink_receiver.h | 3 +++ 2 files changed, 21 insertions(+), 3 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 97108270c5..dfbf00b664 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -92,6 +92,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _mavlink(parent), status{}, hil_local_pos{}, + hil_land_detector{}, _control_mode{}, _global_pos_pub(-1), _local_pos_pub(-1), @@ -118,6 +119,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _telemetry_status_pub(-1), _rc_pub(-1), _manual_pub(-1), + _land_detector_pub(-1), _control_mode_sub(orb_subscribe(ORB_ID(vehicle_control_mode))), _hil_frames(0), _old_timestamp(0), @@ -1353,9 +1355,6 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) hil_local_pos.xy_global = true; hil_local_pos.z_global = true; - bool landed = (float)(hil_state.alt) / 1000.0f < (_hil_local_alt0 + 0.1f); // XXX improve? - hil_local_pos.landed = landed; - if (_local_pos_pub < 0) { _local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &hil_local_pos); @@ -1364,6 +1363,22 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) } } + /* land detector */ + { + bool landed = (float)(hil_state.alt) / 1000.0f < (_hil_local_alt0 + 0.1f); // XXX improve? + if(hil_land_detector.landed != landed) { + hil_land_detector.landed = landed; + hil_land_detector.timestamp = hrt_absolute_time(); + + if (_land_detector_pub < 0) { + _land_detector_pub = orb_advertise(ORB_ID(vehicle_land_detected), &hil_land_detector); + + } else { + orb_publish(ORB_ID(vehicle_land_detected), _land_detector_pub, &hil_land_detector); + } + } + } + /* accelerometer */ { struct accel_report accel; diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 4afc9b3721..699996860a 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -50,6 +50,7 @@ #include #include #include +#include #include #include #include @@ -145,6 +146,7 @@ private: mavlink_status_t status; struct vehicle_local_position_s hil_local_pos; + struct vehicle_land_detected_s hil_land_detector; struct vehicle_control_mode_s _control_mode; orb_advert_t _global_pos_pub; orb_advert_t _local_pos_pub; @@ -171,6 +173,7 @@ private: orb_advert_t _telemetry_status_pub; orb_advert_t _rc_pub; orb_advert_t _manual_pub; + orb_advert_t _land_detector_pub; int _control_mode_sub; int _hil_frames; uint64_t _old_timestamp; From 6978ed6a61212859d1c58c769ce75f343bc2e4ca Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Tue, 6 Jan 2015 13:56:36 +0100 Subject: [PATCH 493/652] INAV: Removed all references to land detector logic --- .../position_estimator_inav_main.c | 37 ------------------- 1 file changed, 37 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 b418e3a76b..39294e3c00 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -246,9 +246,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float baro_offset = 0.0f; // baro offset for reference altitude, initialized on start, then adjusted float surface_offset = 0.0f; // ground level offset from reference altitude float surface_offset_rate = 0.0f; // surface offset change rate - //float alt_avg = 0.0f; - //bool landed = true; - //hrt_abstime landed_time = 0; hrt_abstime accel_timestamp = 0; hrt_abstime baro_timestamp = 0; @@ -1068,39 +1065,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) inertial_filter_correct(-y_est[1], dt, y_est, 1, params.w_xy_res_v); } - -/* - // detect land - alt_avg += (- z_est[0] - alt_avg) * dt / params.land_t; - float alt_disp2 = - z_est[0] - alt_avg; - alt_disp2 = alt_disp2 * alt_disp2; - float land_disp2 = params.land_disp * params.land_disp; - // get actual thrust output - float thrust = armed.armed ? actuator.control[3] : 0.0f; - - if (landed) { - if (alt_disp2 > land_disp2 || thrust > params.land_thr) { - landed = false; - landed_time = 0; - } - } else { - if (alt_disp2 < land_disp2 && thrust < params.land_thr) { - if (landed_time == 0) { - landed_time = t; // land detected first time - - } else { - if (t > landed_time + params.land_t * 1000000.0f) { - landed = true; - landed_time = 0; - } - } - - } else { - landed_time = 0; - } - } -*/ - if (verbose_mode) { /* print updates rate */ if (t > updates_counter_start + updates_counter_len) { @@ -1151,7 +1115,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) local_pos.vy = y_est[1]; local_pos.z = z_est[0]; local_pos.vz = z_est[1]; - local_pos.landed = false; local_pos.yaw = att.yaw; local_pos.dist_bottom_valid = dist_bottom_valid; local_pos.eph = eph; From 6edb54ff7755fbb30c695984cf53b246ff497141 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Tue, 6 Jan 2015 13:57:27 +0100 Subject: [PATCH 494/652] sdlog2: Added land detector log message (removed from local pos) --- src/modules/sdlog2/sdlog2.c | 12 +++++++++++- src/modules/sdlog2/sdlog2_messages.h | 7 +++++++ 2 files changed, 18 insertions(+), 1 deletion(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index d35b702391..b470eefacc 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -72,6 +72,7 @@ #include #include #include +#include #include #include #include @@ -981,6 +982,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct vehicle_local_position_s local_pos; struct vehicle_local_position_setpoint_s local_pos_sp; struct vehicle_global_position_s global_pos; + struct vehicle_land_detected_s land_detector; struct position_setpoint_triplet_s triplet; struct vehicle_vicon_position_s vicon_pos; struct vision_position_estimate vision_pos; @@ -1016,6 +1018,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_SENS_s log_SENS; struct log_LPOS_s log_LPOS; struct log_LPSP_s log_LPSP; + struct log_LAND_s log_LAND; struct log_GPS_s log_GPS; struct log_ATTC_s log_ATTC; struct log_STAT_s log_STAT; @@ -1082,6 +1085,7 @@ int sdlog2_thread_main(int argc, char *argv[]) int servorail_status_sub; int wind_sub; int encoders_sub; + int land_detector_sub; } subs; subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); @@ -1112,6 +1116,8 @@ int sdlog2_thread_main(int argc, char *argv[]) subs.system_power_sub = orb_subscribe(ORB_ID(system_power)); subs.servorail_status_sub = orb_subscribe(ORB_ID(servorail_status)); subs.wind_sub = orb_subscribe(ORB_ID(wind_estimate)); + subs.land_detector_sub = orb_subscribe(ORB_ID(vehicle_land_detected)); + /* we need to rate-limit wind, as we do not need the full update rate */ orb_set_interval(subs.wind_sub, 90); subs.encoders_sub = orb_subscribe(ORB_ID(encoders)); @@ -1514,13 +1520,17 @@ int sdlog2_thread_main(int argc, char *argv[]) (buf.local_pos.v_z_valid ? 8 : 0) | (buf.local_pos.xy_global ? 16 : 0) | (buf.local_pos.z_global ? 32 : 0); - log_msg.body.log_LPOS.landed = buf.local_pos.landed; log_msg.body.log_LPOS.ground_dist_flags = (buf.local_pos.dist_bottom_valid ? 1 : 0); log_msg.body.log_LPOS.eph = buf.local_pos.eph; log_msg.body.log_LPOS.epv = buf.local_pos.epv; LOGBUFFER_WRITE_AND_COUNT(LPOS); } + /* --- LAND DETECTED --- */ + if (copy_if_updated(ORB_ID(vehicle_land_detected), subs.land_detector_sub, &buf.land_detector)) { + log_msg.body.log_LAND.landed = buf.land_detector.landed; + } + /* --- LOCAL POSITION SETPOINT --- */ if (copy_if_updated(ORB_ID(vehicle_local_position_setpoint), subs.local_pos_sp_sub, &buf.local_pos_sp)) { log_msg.msg_type = LOG_LPSP_MSG; diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 5941bfac0e..246f5129f0 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -427,6 +427,12 @@ struct log_ENCD_s { /* --- AIR SPEED SENSORS - DIFF. PRESSURE --- */ #define LOG_AIR1_MSG 40 +/* --- LAND - LAND DETECTOR --- */ +#define LOG_LAND_MSG 41 +struct log_LAND_s { + uint8_t landed; +}; + /********** SYSTEM MESSAGES, ID > 0x80 **********/ /* --- TIME - TIME STAMP --- */ @@ -495,6 +501,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(TECS, "fffffffffffffB", "ASP,AF,FSP,F,FF,AsSP,AsF,AsDSP,AsD,TERSP,TER,EDRSP,EDR,M"), LOG_FORMAT(WIND, "ffff", "X,Y,CovX,CovY"), LOG_FORMAT(ENCD, "qfqf", "cnt0,vel0,cnt1,vel1"), + LOG_FORMAT(LAND, "B", "landed"), /* system-level messages, ID >= 0x80 */ /* FMT: don't write format of format message, it's useless */ From 2da6439f742f7743adca22ad3a887e936e6c2277 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Tue, 6 Jan 2015 13:57:46 +0100 Subject: [PATCH 495/652] uORB: Removed landed boolean flag from vehicle_local_position topic --- src/modules/uORB/topics/vehicle_local_position.h | 1 - 1 file changed, 1 deletion(-) diff --git a/src/modules/uORB/topics/vehicle_local_position.h b/src/modules/uORB/topics/vehicle_local_position.h index 0e0bc9781a..8b46c5a3f3 100644 --- a/src/modules/uORB/topics/vehicle_local_position.h +++ b/src/modules/uORB/topics/vehicle_local_position.h @@ -77,7 +77,6 @@ struct vehicle_local_position_s { double ref_lat; /**< Reference point latitude in degrees */ double ref_lon; /**< Reference point longitude in degrees */ float ref_alt; /**< Reference altitude AMSL in meters, MUST be set to current (not at reference point!) ground level */ - bool landed; /**< true if vehicle is landed TODO: deprecated, use vehicle_land_detected instead (remove this line?)*/ /* Distance to surface */ float dist_bottom; /**< Distance to bottom surface (ground) */ float dist_bottom_rate; /**< Distance to bottom surface (ground) change rate */ From 784fa9f4699c434671edfd1e4fb48897bea54d8f Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Wed, 7 Jan 2015 22:12:48 +0100 Subject: [PATCH 496/652] sdlog2: Removed vehicle_land_detected topic from logging --- src/modules/sdlog2/sdlog2.c | 10 ---------- src/modules/sdlog2/sdlog2_messages.h | 9 +-------- 2 files changed, 1 insertion(+), 18 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index b470eefacc..7b7949239e 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -72,7 +72,6 @@ #include #include #include -#include #include #include #include @@ -982,7 +981,6 @@ int sdlog2_thread_main(int argc, char *argv[]) struct vehicle_local_position_s local_pos; struct vehicle_local_position_setpoint_s local_pos_sp; struct vehicle_global_position_s global_pos; - struct vehicle_land_detected_s land_detector; struct position_setpoint_triplet_s triplet; struct vehicle_vicon_position_s vicon_pos; struct vision_position_estimate vision_pos; @@ -1018,7 +1016,6 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_SENS_s log_SENS; struct log_LPOS_s log_LPOS; struct log_LPSP_s log_LPSP; - struct log_LAND_s log_LAND; struct log_GPS_s log_GPS; struct log_ATTC_s log_ATTC; struct log_STAT_s log_STAT; @@ -1085,7 +1082,6 @@ int sdlog2_thread_main(int argc, char *argv[]) int servorail_status_sub; int wind_sub; int encoders_sub; - int land_detector_sub; } subs; subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); @@ -1116,7 +1112,6 @@ int sdlog2_thread_main(int argc, char *argv[]) subs.system_power_sub = orb_subscribe(ORB_ID(system_power)); subs.servorail_status_sub = orb_subscribe(ORB_ID(servorail_status)); subs.wind_sub = orb_subscribe(ORB_ID(wind_estimate)); - subs.land_detector_sub = orb_subscribe(ORB_ID(vehicle_land_detected)); /* we need to rate-limit wind, as we do not need the full update rate */ orb_set_interval(subs.wind_sub, 90); @@ -1526,11 +1521,6 @@ int sdlog2_thread_main(int argc, char *argv[]) LOGBUFFER_WRITE_AND_COUNT(LPOS); } - /* --- LAND DETECTED --- */ - if (copy_if_updated(ORB_ID(vehicle_land_detected), subs.land_detector_sub, &buf.land_detector)) { - log_msg.body.log_LAND.landed = buf.land_detector.landed; - } - /* --- LOCAL POSITION SETPOINT --- */ if (copy_if_updated(ORB_ID(vehicle_local_position_setpoint), subs.local_pos_sp_sub, &buf.local_pos_sp)) { log_msg.msg_type = LOG_LPSP_MSG; diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 246f5129f0..99f70a948e 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -113,7 +113,6 @@ struct log_LPOS_s { int32_t ref_lon; float ref_alt; uint8_t pos_flags; - uint8_t landed; uint8_t ground_dist_flags; float eph; float epv; @@ -427,11 +426,6 @@ struct log_ENCD_s { /* --- AIR SPEED SENSORS - DIFF. PRESSURE --- */ #define LOG_AIR1_MSG 40 -/* --- LAND - LAND DETECTOR --- */ -#define LOG_LAND_MSG 41 -struct log_LAND_s { - uint8_t landed; -}; /********** SYSTEM MESSAGES, ID > 0x80 **********/ @@ -468,7 +462,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT_S(IMU2, IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"), LOG_FORMAT_S(SENS, SENS, "fffff", "BaroPres,BaroAlt,BaroTemp,DiffPres,DiffPresFilt"), LOG_FORMAT_S(AIR1, SENS, "fffff", "BaroPa,BaroAlt,BaroTmp,DiffPres,DiffPresF"), - LOG_FORMAT(LPOS, "ffffffffLLfBBBff", "X,Y,Z,Dist,DistR,VX,VY,VZ,RLat,RLon,RAlt,PFlg,LFlg,GFlg,EPH,EPV"), + LOG_FORMAT(LPOS, "ffffffffLLfBBff", "X,Y,Z,Dist,DistR,VX,VY,VZ,RLat,RLon,RAlt,PFlg,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_S(ATTC, ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"), @@ -501,7 +495,6 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(TECS, "fffffffffffffB", "ASP,AF,FSP,F,FF,AsSP,AsF,AsDSP,AsD,TERSP,TER,EDRSP,EDR,M"), LOG_FORMAT(WIND, "ffff", "X,Y,CovX,CovY"), LOG_FORMAT(ENCD, "qfqf", "cnt0,vel0,cnt1,vel1"), - LOG_FORMAT(LAND, "B", "landed"), /* system-level messages, ID >= 0x80 */ /* FMT: don't write format of format message, it's useless */ From 10a2dd8a346a6a08a0d9b52739f20b842d460646 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Wed, 7 Jan 2015 23:19:35 +0100 Subject: [PATCH 497/652] LandDetector: Merged fixedwing and multicopter into one module handling both algorithms --- makefiles/config_px4fmu-v2_default.mk | 7 +- .../FixedwingLandDetector.cpp | 173 --------------- .../fw_land_detector/FixedwingLandDetector.h | 108 ---------- .../fw_land_detector_main.cpp | 199 ----------------- src/modules/fw_land_detector/module.mk | 10 - .../land_detector/FixedwingLandDetector.cpp | 103 +++++++++ .../land_detector/FixedwingLandDetector.h | 88 ++++++++ src/modules/land_detector/LandDetector.cpp | 77 +++++++ src/modules/land_detector/LandDetector.h | 101 +++++++++ .../land_detector/MulticopterLandDetector.cpp | 120 +++++++++++ .../MulticopterLandDetector.h | 48 ++--- .../land_detector_main.cpp} | 81 ++++--- src/modules/land_detector/module.mk | 12 ++ .../MulticopterLandDetector.cpp | 202 ------------------ src/modules/mc_land_detector/module.mk | 10 - 15 files changed, 564 insertions(+), 775 deletions(-) delete mode 100644 src/modules/fw_land_detector/FixedwingLandDetector.cpp delete mode 100644 src/modules/fw_land_detector/FixedwingLandDetector.h delete mode 100644 src/modules/fw_land_detector/fw_land_detector_main.cpp delete mode 100644 src/modules/fw_land_detector/module.mk create mode 100644 src/modules/land_detector/FixedwingLandDetector.cpp create mode 100644 src/modules/land_detector/FixedwingLandDetector.h create mode 100644 src/modules/land_detector/LandDetector.cpp create mode 100644 src/modules/land_detector/LandDetector.h create mode 100644 src/modules/land_detector/MulticopterLandDetector.cpp rename src/modules/{mc_land_detector => land_detector}/MulticopterLandDetector.h (75%) rename src/modules/{mc_land_detector/mc_land_detector_main.cpp => land_detector/land_detector_main.cpp} (65%) create mode 100644 src/modules/land_detector/module.mk delete mode 100644 src/modules/mc_land_detector/MulticopterLandDetector.cpp delete mode 100644 src/modules/mc_land_detector/module.mk diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index 8ef1c333ae..e13a88ca57 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -71,12 +71,7 @@ MODULES += modules/navigator MODULES += modules/mavlink MODULES += modules/gpio_led MODULES += modules/uavcan - -# -# Vehicle land detection -# -MODULES += modules/mc_land_detector -MODULES += modules/fw_land_detector +MODULES += modules/land_detector # # Estimation modules (EKF/ SO3 / other filters) diff --git a/src/modules/fw_land_detector/FixedwingLandDetector.cpp b/src/modules/fw_land_detector/FixedwingLandDetector.cpp deleted file mode 100644 index 5fbeb2f45b..0000000000 --- a/src/modules/fw_land_detector/FixedwingLandDetector.cpp +++ /dev/null @@ -1,173 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file FixedwingLandDetector.cpp - * Land detection algorithm for fixedwings - * - * @author Johan Jansen - */ - -#include "FixedwingLandDetector.h" - -#include -#include -#include -#include //usleep - -FixedwingLandDetector::FixedwingLandDetector() : - _landDetectedPub(-1), - _landDetected({0, false}), - - _vehicleLocalPositionSub(-1), - _vehicleLocalPosition({}), - _airspeedSub(-1), - _airspeed({}), - - _velocity_xy_filtered(0.0f), - _velocity_z_filtered(0.0f), - _airspeed_filtered(0.0f), - _landDetectTrigger(0), - - _taskShouldExit(false), - _taskIsRunning(false) -{ - //Advertise the first land detected uORB - _landDetected.timestamp = hrt_absolute_time(); - _landDetected.landed = false; - _landDetectedPub = orb_advertise(ORB_ID(vehicle_land_detected), &_landDetected); -} - -FixedwingLandDetector::~FixedwingLandDetector() -{ - _taskShouldExit = true; - close(_landDetectedPub); -} - -void FixedwingLandDetector::shutdown() -{ - _taskShouldExit = true; -} - -/** -* @brief Convinience function for polling uORB subscriptions -* @return true if there was new data and it was successfully copied -**/ -static bool orb_update(const struct orb_metadata *meta, int handle, void *buffer) -{ - bool newData = false; - - //Check if there is new data to grab - if (orb_check(handle, &newData) != OK) { - return false; - } - - if (!newData) { - return false; - } - - if (orb_copy(meta, handle, buffer) != OK) { - return false; - } - - return true; -} - -void FixedwingLandDetector::updateSubscriptions() -{ - orb_update(ORB_ID(vehicle_local_position), _vehicleLocalPositionSub, &_vehicleLocalPosition); - orb_update(ORB_ID(airspeed), _airspeedSub, &_airspeed); -} - -void FixedwingLandDetector::landDetectorLoop() -{ - //This should never happen! - if (_taskIsRunning) { return; } - - //Subscribe to local position and airspeed data - _vehicleLocalPositionSub = orb_subscribe(ORB_ID(vehicle_local_position)); - _airspeedSub = orb_subscribe(ORB_ID(airspeed)); - - _taskIsRunning = true; - _taskShouldExit = false; - - while (!_taskShouldExit) { - - //First poll for new data from our subscriptions - updateSubscriptions(); - - const uint64_t now = hrt_absolute_time(); - bool landDetected = false; - - //TODO: reset filtered values on arming? - _velocity_xy_filtered = 0.95f * _velocity_xy_filtered + 0.05f * sqrtf(_vehicleLocalPosition.vx * - _vehicleLocalPosition.vx + _vehicleLocalPosition.vy * _vehicleLocalPosition.vy); - _velocity_z_filtered = 0.95f * _velocity_z_filtered + 0.05f * fabsf(_vehicleLocalPosition.vz); - _airspeed_filtered = 0.95f * _airspeed_filtered + 0.05f * _airspeed.true_airspeed_m_s; - - /* crude land detector for fixedwing */ - if (_velocity_xy_filtered < FW_LAND_DETECTOR_VELOCITY_MAX - && _velocity_z_filtered < FW_LAND_DETECTOR_CLIMBRATE_MAX - && _airspeed_filtered < FW_LAND_DETECTOR_AIRSPEED_MAX) { - - //These conditions need to be stable for a period of time before we trust them - if (now > _landDetectTrigger) { - landDetected = true; - } - - } else { - //reset land detect trigger - _landDetectTrigger = now + FW_LAND_DETECTOR_TRIGGER_TIME; - } - - //Publish if land detection state has changed - if (_landDetected.landed != landDetected) { - _landDetected.timestamp = now; - _landDetected.landed = landDetected; - - /* publish the land detected broadcast */ - orb_publish(ORB_ID(vehicle_land_detected), _landDetectedPub, &_landDetected); - } - - //Limit loop rate - usleep(1000000 / FW_LAND_DETECTOR_UPDATE_RATE); - } - - _taskIsRunning = false; - _exit(0); -} - -bool FixedwingLandDetector::isRunning() const -{ - return _taskIsRunning; -} diff --git a/src/modules/fw_land_detector/FixedwingLandDetector.h b/src/modules/fw_land_detector/FixedwingLandDetector.h deleted file mode 100644 index be9b17b74b..0000000000 --- a/src/modules/fw_land_detector/FixedwingLandDetector.h +++ /dev/null @@ -1,108 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file FixedwingLandDetector.h - * Land detection algorithm for fixedwing - * - * @author Johan Jansen - */ - -#ifndef __FIXED_WING_LAND_DETECTOR_H__ -#define __FIXED_WING_LAND_DETECTOR_H__ - -#include -#include -#include -#include - -class FixedwingLandDetector -{ -public: - FixedwingLandDetector(); - ~FixedwingLandDetector(); - - /** - * @brief Executes the main loop of the land detector in a separate deamon thread - * @returns OK if task was successfully launched - **/ - int createDeamonThread(); - - /** - * @returns true if this task is currently running - **/ - bool isRunning() const; - - /** - * @brief blocking loop, should be run in a separate thread or task. Runs at 50Hz - **/ - void landDetectorLoop(); - - /** - * @brief Tells the Land Detector task that it should exit - **/ - void shutdown(); - -protected: - /** - * @brief polls all subscriptions and pulls any data that has changed - **/ - void updateSubscriptions(); - - //Algorithm parameters (TODO: should probably be externalized) - static constexpr uint32_t FW_LAND_DETECTOR_UPDATE_RATE = 50; /**< Run algorithm at 50Hz */ - static constexpr uint64_t FW_LAND_DETECTOR_TRIGGER_TIME = - 2000000; /**< usec that landing conditions have to hold before triggering a land */ - static constexpr float FW_LAND_DETECTOR_VELOCITY_MAX = 5.0f; /**< maximum horizontal movement m/s*/ - static constexpr float FW_LAND_DETECTOR_CLIMBRATE_MAX = 10.00f; /**< +- climb rate in m/s*/ - static constexpr float FW_LAND_DETECTOR_AIRSPEED_MAX = 10.00f; /**< airspeed max m/s*/ - -private: - orb_advert_t _landDetectedPub; /**< publisher for position in local frame */ - struct vehicle_land_detected_s _landDetected; /**< local vehicle position */ - - int _vehicleLocalPositionSub; /**< notification of local position */ - struct vehicle_local_position_s _vehicleLocalPosition; /**< the result from local position subscription */ - int _airspeedSub; - struct airspeed_s _airspeed; - - float _velocity_xy_filtered; - float _velocity_z_filtered; - float _airspeed_filtered; - uint64_t _landDetectTrigger; - - bool _taskShouldExit; /**< true if it is requested that this task should exit */ - bool _taskIsRunning; /**< task has reached main loop and is currently running */ -}; - -#endif //__FIXED_WING_LAND_DETECTOR_H__ diff --git a/src/modules/fw_land_detector/fw_land_detector_main.cpp b/src/modules/fw_land_detector/fw_land_detector_main.cpp deleted file mode 100644 index a45a518639..0000000000 --- a/src/modules/fw_land_detector/fw_land_detector_main.cpp +++ /dev/null @@ -1,199 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file fw_land_detector_main.cpp - * Land detection algorithm for fixed wings - * - * @author Johan Jansen - */ - -#include //usleep -#include -#include -#include -#include -#include -#include //Scheduler -#include //print to console - -#include "FixedwingLandDetector.h" - -//Function prototypes -static int fw_land_detector_start(); -static void fw_land_detector_stop(); - -/** - * land detector app start / stop handling function - * This makes the land detector module accessible from the nuttx shell - * @ingroup apps - */ -extern "C" __EXPORT int fw_land_detector_main(int argc, char *argv[]); - -//Private variables -static FixedwingLandDetector *fw_land_detector_task = nullptr; -static int _landDetectorTaskID = -1; - -/** -* Deamon thread function -**/ -static void fw_land_detector_deamon_thread(int argc, char *argv[]) -{ - fw_land_detector_task->landDetectorLoop(); -} - -/** -* Stop the task, force killing it if it doesn't stop by itself -**/ -static void fw_land_detector_stop() -{ - if (fw_land_detector_task == nullptr || _landDetectorTaskID == -1) { - errx(1, "not running"); - return; - } - - fw_land_detector_task->shutdown(); - - //Wait for task to die - int i = 0; - - do { - /* wait 20ms */ - usleep(20000); - - /* if we have given up, kill it */ - if (++i > 50) { - task_delete(_landDetectorTaskID); - break; - } - } while (fw_land_detector_task->isRunning()); - - - delete fw_land_detector_task; - fw_land_detector_task = nullptr; - _landDetectorTaskID = -1; - warn("fw_land_detector has been stopped"); -} - -/** -* Start new task, fails if it is already running. Returns OK if successful -**/ -static int fw_land_detector_start() -{ - if (fw_land_detector_task != nullptr || _landDetectorTaskID != -1) { - errx(1, "already running"); - return -1; - } - - //Allocate memory - fw_land_detector_task = new FixedwingLandDetector(); - - if (fw_land_detector_task == nullptr) { - errx(1, "alloc failed"); - return -1; - } - - //Start new thread task - _landDetectorTaskID = task_spawn_cmd("fw_land_detector", - SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT, - 1024, - (main_t)&fw_land_detector_deamon_thread, - nullptr); - - if (_landDetectorTaskID < 0) { - errx(1, "task start failed: %d", -errno); - return -1; - } - - /* avoid memory fragmentation by not exiting start handler until the task has fully started */ - const uint32_t timeout = hrt_absolute_time() + 5000000; //5 second timeout - - while (!fw_land_detector_task->isRunning()) { - usleep(50000); - printf("."); - fflush(stdout); - - if (hrt_absolute_time() > timeout) { - err(1, "start failed - timeout"); - fw_land_detector_stop(); - exit(1); - } - } - - printf("\n"); - - exit(0); - return 0; -} - -/** -* Main entry point for this module -**/ -int fw_land_detector_main(int argc, char *argv[]) -{ - - if (argc < 1) { - warnx("usage: fw_land_detector {start|stop|status}"); - exit(0); - } - - if (!strcmp(argv[1], "start")) { - fw_land_detector_start(); - } - - if (!strcmp(argv[1], "stop")) { - fw_land_detector_stop(); - exit(0); - } - - if (!strcmp(argv[1], "status")) { - if (fw_land_detector_task) { - - if (fw_land_detector_task->isRunning()) { - warnx("running"); - - } else { - errx(1, "exists, but not running"); - } - - exit(0); - - } else { - errx(1, "not running"); - } - } - - warn("usage: fw_land_detector {start|stop|status}"); - return 1; -} diff --git a/src/modules/fw_land_detector/module.mk b/src/modules/fw_land_detector/module.mk deleted file mode 100644 index 894b29a7b5..0000000000 --- a/src/modules/fw_land_detector/module.mk +++ /dev/null @@ -1,10 +0,0 @@ -# -# Fixedwing land detector -# - -MODULE_COMMAND = fw_land_detector - -SRCS = fw_land_detector_main.cpp \ - FixedwingLandDetector.cpp - -EXTRACXXFLAGS = -Weffc++ \ No newline at end of file diff --git a/src/modules/land_detector/FixedwingLandDetector.cpp b/src/modules/land_detector/FixedwingLandDetector.cpp new file mode 100644 index 0000000000..42735f38c8 --- /dev/null +++ b/src/modules/land_detector/FixedwingLandDetector.cpp @@ -0,0 +1,103 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file FixedwingLandDetector.cpp + * Land detection algorithm for fixedwings + * + * @author Johan Jansen + */ + +#include "FixedwingLandDetector.h" + +#include +#include + +FixedwingLandDetector::FixedwingLandDetector() : + _vehicleLocalPositionSub(-1), + _vehicleLocalPosition({}), + _airspeedSub(-1), + _airspeed({}), + + _velocity_xy_filtered(0.0f), + _velocity_z_filtered(0.0f), + _airspeed_filtered(0.0f), + _landDetectTrigger(0) +{ + //ctor +} + +void FixedwingLandDetector::initialize() +{ + //Subscribe to local position and airspeed data + _vehicleLocalPositionSub = orb_subscribe(ORB_ID(vehicle_local_position)); + _airspeedSub = orb_subscribe(ORB_ID(airspeed)); +} + +void FixedwingLandDetector::updateSubscriptions() +{ + orb_update(ORB_ID(vehicle_local_position), _vehicleLocalPositionSub, &_vehicleLocalPosition); + orb_update(ORB_ID(airspeed), _airspeedSub, &_airspeed); +} + +bool FixedwingLandDetector::update() +{ + //First poll for new data from our subscriptions + updateSubscriptions(); + + const uint64_t now = hrt_absolute_time(); + bool landDetected = false; + + //TODO: reset filtered values on arming? + _velocity_xy_filtered = 0.95f * _velocity_xy_filtered + 0.05f * sqrtf(_vehicleLocalPosition.vx * + _vehicleLocalPosition.vx + _vehicleLocalPosition.vy * _vehicleLocalPosition.vy); + _velocity_z_filtered = 0.95f * _velocity_z_filtered + 0.05f * fabsf(_vehicleLocalPosition.vz); + _airspeed_filtered = 0.95f * _airspeed_filtered + 0.05f * _airspeed.true_airspeed_m_s; + + /* crude land detector for fixedwing */ + if (_velocity_xy_filtered < FW_LAND_DETECTOR_VELOCITY_MAX + && _velocity_z_filtered < FW_LAND_DETECTOR_CLIMBRATE_MAX + && _airspeed_filtered < FW_LAND_DETECTOR_AIRSPEED_MAX) { + + //These conditions need to be stable for a period of time before we trust them + if (now > _landDetectTrigger) { + landDetected = true; + } + + } else { + //reset land detect trigger + _landDetectTrigger = now + FW_LAND_DETECTOR_TRIGGER_TIME; + } + + return landDetected; +} diff --git a/src/modules/land_detector/FixedwingLandDetector.h b/src/modules/land_detector/FixedwingLandDetector.h new file mode 100644 index 0000000000..faeece6f3c --- /dev/null +++ b/src/modules/land_detector/FixedwingLandDetector.h @@ -0,0 +1,88 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file FixedwingLandDetector.h + * Land detection algorithm for fixedwing + * + * @author Johan Jansen + */ + +#ifndef __FIXED_WING_LAND_DETECTOR_H__ +#define __FIXED_WING_LAND_DETECTOR_H__ + +#include "LandDetector.h" +#include +#include + +class FixedwingLandDetector : public LandDetector +{ +public: + FixedwingLandDetector(); + +protected: + /** + * @brief blocking loop, should be run in a separate thread or task. Runs at 50Hz + **/ + bool update() override; + + /** + * @brief Initializes the land detection algorithm + **/ + void initialize() override; + + /** + * @brief polls all subscriptions and pulls any data that has changed + **/ + void updateSubscriptions(); + + //Algorithm parameters (TODO: should probably be externalized) + static constexpr uint64_t FW_LAND_DETECTOR_TRIGGER_TIME = 2000000; /**< usec that landing conditions have to hold + before triggering a land */ + static constexpr float FW_LAND_DETECTOR_VELOCITY_MAX = 5.0f; /**< maximum horizontal movement m/s*/ + static constexpr float FW_LAND_DETECTOR_CLIMBRATE_MAX = 10.00f; /**< +- climb rate in m/s*/ + static constexpr float FW_LAND_DETECTOR_AIRSPEED_MAX = 10.00f; /**< airspeed max m/s*/ + +private: + int _vehicleLocalPositionSub; /**< notification of local position */ + struct vehicle_local_position_s _vehicleLocalPosition; /**< the result from local position subscription */ + int _airspeedSub; + struct airspeed_s _airspeed; + + float _velocity_xy_filtered; + float _velocity_z_filtered; + float _airspeed_filtered; + uint64_t _landDetectTrigger; +}; + +#endif //__FIXED_WING_LAND_DETECTOR_H__ diff --git a/src/modules/land_detector/LandDetector.cpp b/src/modules/land_detector/LandDetector.cpp new file mode 100644 index 0000000000..a39e538185 --- /dev/null +++ b/src/modules/land_detector/LandDetector.cpp @@ -0,0 +1,77 @@ +#include "LandDetector.h" +#include //usleep +#include + +LandDetector::LandDetector() : + _landDetectedPub(-1), + _landDetected({0, false}), + _taskShouldExit(false), + _taskIsRunning(false) +{ + //Advertise the first land detected uORB + _landDetected.timestamp = hrt_absolute_time(); + _landDetected.landed = false; + _landDetectedPub = orb_advertise(ORB_ID(vehicle_land_detected), &_landDetected); +} + +LandDetector::~LandDetector() +{ + _taskShouldExit = true; + close(_landDetectedPub); +} + +void LandDetector::shutdown() +{ + _taskShouldExit = true; +} + +void LandDetector::start() +{ + //Make sure this method has not already been called by another thread + if(isRunning()) { + return; + } + + //Task is now running, keep doing so until shutdown() has been called + _taskIsRunning = true; + _taskShouldExit = false; + while(isRunning()) { + + bool landDetected = update(); + + //Publish if land detection state has changed + if (_landDetected.landed != landDetected) { + _landDetected.timestamp = hrt_absolute_time(); + _landDetected.landed = landDetected; + + /* publish the land detected broadcast */ + orb_publish(ORB_ID(vehicle_land_detected), _landDetectedPub, &_landDetected); + } + + //Limit loop rate + usleep(1000000 / LAND_DETECTOR_UPDATE_RATE); + } + + _taskIsRunning = false; + _exit(0); +} + +bool LandDetector::orb_update(const struct orb_metadata *meta, int handle, void *buffer) +{ + bool newData = false; + + //Check if there is new data to grab + if (orb_check(handle, &newData) != OK) { + return false; + } + + if (!newData) { + return false; + } + + if (orb_copy(meta, handle, buffer) != OK) { + return false; + } + + return true; +} diff --git a/src/modules/land_detector/LandDetector.h b/src/modules/land_detector/LandDetector.h new file mode 100644 index 0000000000..6d74656917 --- /dev/null +++ b/src/modules/land_detector/LandDetector.h @@ -0,0 +1,101 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file LandDetector.h + * Abstract interface for land detector algorithms + * + * @author Johan Jansen + */ + +#ifndef __LAND_DETECTOR_H__ +#define __LAND_DETECTOR_H__ + +#include +#include + +class LandDetector +{ +public: + + LandDetector(); + virtual ~LandDetector(); + + /** + * @return true if this task is currently running + **/ + inline bool isRunning() const {return _taskIsRunning;} + + /** + * @brief Tells the Land Detector task that it should exit + **/ + void shutdown(); + + /** + * @brief Blocking function that should be called from it's own task thread. This method will + * run the underlying algorithm at the desired update rate and publish if the landing state changes. + **/ + void start(); + +protected: + + /** + * @brief Pure abstract method that must be overriden by sub-classes. This actually runs the underlying algorithm + * @return true if a landing was detected and this should be broadcast to the rest of the system + **/ + virtual bool update() = 0; + + /** + * @brief Pure abstract method that is called by this class once for initializing the uderlying algorithm (memory allocation, + * uORB topic subscription, creating file descriptors, etc.) + **/ + virtual void initialize() = 0; + + /** + * @brief Convinience function for polling uORB subscriptions + * @return true if there was new data and it was successfully copied + **/ + bool orb_update(const struct orb_metadata *meta, int handle, void *buffer); + + static constexpr uint32_t LAND_DETECTOR_UPDATE_RATE = 50; /**< Run algorithm at 50Hz */ + +protected: + orb_advert_t _landDetectedPub; /**< publisher for position in local frame */ + struct vehicle_land_detected_s _landDetected; /**< local vehicle position */ + +private: + bool _taskShouldExit; /**< true if it is requested that this task should exit */ + bool _taskIsRunning; /**< task has reached main loop and is currently running */ +}; + +#endif //__LAND_DETECTOR_H__ \ No newline at end of file diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp new file mode 100644 index 0000000000..36d92fd798 --- /dev/null +++ b/src/modules/land_detector/MulticopterLandDetector.cpp @@ -0,0 +1,120 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file MulticopterLandDetector.cpp + * Land detection algorithm + * + * @author Johan Jansen + * @author Morten Lysgaard + */ + +#include "MulticopterLandDetector.h" + +#include +#include + +MulticopterLandDetector::MulticopterLandDetector() : + _vehicleGlobalPositionSub(-1), + _sensorsCombinedSub(-1), + _waypointSub(-1), + _actuatorsSub(-1), + _armingSub(-1), + + _vehicleGlobalPosition({}), + _sensors({}), + _waypoint({}), + _actuators({}), + _arming({}), + + _landTimer(0) +{ + //ctor +} + +void MulticopterLandDetector::initialize() +{ + //Subscribe to position, attitude, arming and velocity changes + _vehicleGlobalPositionSub = orb_subscribe(ORB_ID(vehicle_global_position)); + _sensorsCombinedSub = orb_subscribe(ORB_ID(sensor_combined)); + _waypointSub = orb_subscribe(ORB_ID(position_setpoint_triplet)); + _actuatorsSub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); + _armingSub = orb_subscribe(ORB_ID(actuator_armed)); +} + +void MulticopterLandDetector::updateSubscriptions() +{ + orb_update(ORB_ID(vehicle_global_position), _vehicleGlobalPositionSub, &_vehicleGlobalPosition); + orb_update(ORB_ID(sensor_combined), _sensorsCombinedSub, &_sensors); + orb_update(ORB_ID(position_setpoint_triplet), _waypointSub, &_waypoint); + orb_update(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _actuatorsSub, &_actuators); + orb_update(ORB_ID(actuator_armed), _armingSub, &_arming); +} + +bool MulticopterLandDetector::update() +{ + //First poll for new data from our subscriptions + updateSubscriptions(); + + const uint64_t now = hrt_absolute_time(); + + //only detect landing if the autopilot is actively trying to land + if (!_waypoint.current.valid || _waypoint.current.type != SETPOINT_TYPE_LAND) { + _landTimer = now; + + } else { + + //Check if we are moving vertically + bool verticalMovement = fabsf(_vehicleGlobalPosition.vel_d) > MC_LAND_DETECTOR_CLIMBRATE_MAX; + + //Check if we are moving horizontally + bool horizontalMovement = sqrtf(_vehicleGlobalPosition.vel_n * _vehicleGlobalPosition.vel_n + + _vehicleGlobalPosition.vel_e * _vehicleGlobalPosition.vel_e) > MC_LAND_DETECTOR_VELOCITY_MAX; + + //Next look if all rotation angles are not moving + bool rotating = sqrtf(_sensors.gyro_rad_s[0] * _sensors.gyro_rad_s[0] + + _sensors.gyro_rad_s[1] * _sensors.gyro_rad_s[1] + + _sensors.gyro_rad_s[2] * _sensors.gyro_rad_s[2]) > MC_LAND_DETECTOR_ROTATION_MAX; + + //Check if thrust output is minimal (about half of default) + bool minimalThrust = _arming.armed && _actuators.control[3] <= MC_LAND_DETECTOR_THRUST_MAX; + + if (verticalMovement || rotating || !minimalThrust || horizontalMovement) { + //Sensed movement, so reset the land detector + _landTimer = now; + } + + } + + return now - _landTimer > MC_LAND_DETECTOR_TRIGGER_TIME; +} diff --git a/src/modules/mc_land_detector/MulticopterLandDetector.h b/src/modules/land_detector/MulticopterLandDetector.h similarity index 75% rename from src/modules/mc_land_detector/MulticopterLandDetector.h rename to src/modules/land_detector/MulticopterLandDetector.h index fe18374c3b..08a8132ba5 100644 --- a/src/modules/mc_land_detector/MulticopterLandDetector.h +++ b/src/modules/land_detector/MulticopterLandDetector.h @@ -32,50 +32,27 @@ ****************************************************************************/ /** - * @file MultiCopterLandDetector.h + * @file MulticopterLandDetector.h * Land detection algorithm for multicopters * * @author Johan Jansen - * @author Morten Lysgaard + * @author Morten Lysgaard */ #ifndef __MULTICOPTER_LAND_DETECTOR_H__ #define __MULTICOPTER_LAND_DETECTOR_H__ -#include -#include +#include "LandDetector.h" #include #include #include #include #include -class MulticopterLandDetector +class MulticopterLandDetector : public LandDetector { public: MulticopterLandDetector(); - ~MulticopterLandDetector(); - - /** - * @brief Executes the main loop of the land detector in a separate deamon thread - * @returns OK if task was successfully launched - **/ - int createDeamonThread(); - - /** - * @returns true if this task is currently running - **/ - bool isRunning() const; - - /** - * @brief blocking loop, should be run in a separate thread or task. Runs at 50Hz - **/ - void landDetectorLoop(); - - /** - * @brief Tells the Land Detector task that it should exit - **/ - void shutdown(); protected: /** @@ -83,18 +60,24 @@ protected: **/ void updateSubscriptions(); + /** + * @brief Runs one iteration of the land detection algorithm + **/ + bool update() override; + + /** + * @brief Initializes the land detection algorithm + **/ + void initialize() override; + //Algorithm parameters (TODO: should probably be externalized) static constexpr float MC_LAND_DETECTOR_CLIMBRATE_MAX = 0.30f; /**< max +- climb rate in m/s */ static constexpr float MC_LAND_DETECTOR_ROTATION_MAX = 0.5f; /**< max rotation in rad/sec (= 30 deg/s) */ static constexpr float MC_LAND_DETECTOR_THRUST_MAX = 0.2f; static constexpr float MC_LAND_DETECTOR_VELOCITY_MAX = 1.0f; /**< max +- horizontal movement in m/s */ - static constexpr uint32_t MC_LAND_DETECTOR_UPDATE_RATE = 50; /**< Run algorithm at 50Hz */ static constexpr uint32_t MC_LAND_DETECTOR_TRIGGER_TIME = 2000000; /**< usec that landing conditions have to hold before triggering a land */ private: - orb_advert_t _landDetectedPub; /**< publisher for position in local frame */ - struct vehicle_land_detected_s _landDetected; /**< local vehicle position */ - int _vehicleGlobalPositionSub; /**< notification of global position */ int _sensorsCombinedSub; int _waypointSub; @@ -107,9 +90,6 @@ private: struct actuator_controls_s _actuators; struct actuator_armed_s _arming; - bool _taskShouldExit; /**< true if it is requested that this task should exit */ - bool _taskIsRunning; /**< task has reached main loop and is currently running */ - uint64_t _landTimer; /**< timestamp in microseconds since a possible land was detected*/ }; diff --git a/src/modules/mc_land_detector/mc_land_detector_main.cpp b/src/modules/land_detector/land_detector_main.cpp similarity index 65% rename from src/modules/mc_land_detector/mc_land_detector_main.cpp rename to src/modules/land_detector/land_detector_main.cpp index 6ad3f82a45..2bc89e7526 100644 --- a/src/modules/mc_land_detector/mc_land_detector_main.cpp +++ b/src/modules/land_detector/land_detector_main.cpp @@ -32,8 +32,8 @@ ****************************************************************************/ /** - * @file mc_land_detector_main.cpp - * Land detection algorithm for multicopters + * @file land_detector_main.cpp + * Land detection algorithm * * @author Johan Jansen */ @@ -47,42 +47,44 @@ #include //Scheduler #include //print to console +#include "FixedwingLandDetector.h" #include "MulticopterLandDetector.h" //Function prototypes -static int mc_land_detector_start(); -static void mc_land_detector_stop(); +static int land_detector_start(const char* mode); +static void land_detector_stop(); /** * land detector app start / stop handling function * This makes the land detector module accessible from the nuttx shell * @ingroup apps */ -extern "C" __EXPORT int mc_land_detector_main(int argc, char *argv[]); +extern "C" __EXPORT int land_detector_main(int argc, char *argv[]); //Private variables -static MulticopterLandDetector *mc_land_detector_task = nullptr; +static LandDetector *land_detector_task = nullptr; static int _landDetectorTaskID = -1; +static char _currentMode[12]; /** * Deamon thread function **/ -static void mc_land_detector_deamon_thread(int argc, char *argv[]) +static void land_detector_deamon_thread(int argc, char *argv[]) { - mc_land_detector_task->landDetectorLoop(); + land_detector_task->start(); } /** * Stop the task, force killing it if it doesn't stop by itself **/ -static void mc_land_detector_stop() +static void land_detector_stop() { - if (mc_land_detector_task == nullptr || _landDetectorTaskID == -1) { + if (land_detector_task == nullptr || _landDetectorTaskID == -1) { errx(1, "not running"); return; } - mc_land_detector_task->shutdown(); + land_detector_task->shutdown(); //Wait for task to die int i = 0; @@ -96,39 +98,49 @@ static void mc_land_detector_stop() task_delete(_landDetectorTaskID); break; } - } while (mc_land_detector_task->isRunning()); + } while (land_detector_task->isRunning()); - delete mc_land_detector_task; - mc_land_detector_task = nullptr; + delete land_detector_task; + land_detector_task = nullptr; _landDetectorTaskID = -1; - warn("mc_land_detector has been stopped"); + warn("land_detector has been stopped"); } /** * Start new task, fails if it is already running. Returns OK if successful **/ -static int mc_land_detector_start() +static int land_detector_start(const char* mode) { - if (mc_land_detector_task != nullptr || _landDetectorTaskID != -1) { + if (land_detector_task != nullptr || _landDetectorTaskID != -1) { errx(1, "already running"); return -1; } //Allocate memory - mc_land_detector_task = new MulticopterLandDetector(); + if(!strcmp(mode, "fixedwing")) { + land_detector_task = new FixedwingLandDetector(); + } + else if(!strcmp(mode, "multicopter")) { + land_detector_task = new MulticopterLandDetector(); + } + else { + errx(1, "[mode] must be either 'fixedwing' or 'multicopter'"); + return -1; + } - if (mc_land_detector_task == nullptr) { + //Check if alloc worked + if (land_detector_task == nullptr) { errx(1, "alloc failed"); return -1; } //Start new thread task - _landDetectorTaskID = task_spawn_cmd("mc_land_detector", + _landDetectorTaskID = task_spawn_cmd("land_detector", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, 1024, - (main_t)&mc_land_detector_deamon_thread, + (main_t)&land_detector_deamon_thread, nullptr); if (_landDetectorTaskID < 0) { @@ -139,20 +151,23 @@ static int mc_land_detector_start() /* avoid memory fragmentation by not exiting start handler until the task has fully started */ const uint32_t timeout = hrt_absolute_time() + 5000000; //5 second timeout - while (!mc_land_detector_task->isRunning()) { + while (!land_detector_task->isRunning()) { usleep(50000); printf("."); fflush(stdout); if (hrt_absolute_time() > timeout) { err(1, "start failed - timeout"); - mc_land_detector_stop(); + land_detector_stop(); exit(1); } } printf("\n"); + //Remember current active mode + strncpy(_currentMode, mode, 12); + exit(0); return 0; } @@ -160,31 +175,31 @@ static int mc_land_detector_start() /** * Main entry point for this module **/ -int mc_land_detector_main(int argc, char *argv[]) +int land_detector_main(int argc, char *argv[]) { if (argc < 1) { - warnx("usage: mc_land_detector {start|stop|status}"); + warnx("usage: land_detector {start|stop|status} [mode]\nmode can either be 'fixedwing' or 'multicopter'"); exit(0); } - if (!strcmp(argv[1], "start")) { - mc_land_detector_start(); + if (argc >= 2 && !strcmp(argv[1], "start")) { + land_detector_start(argv[2]); } if (!strcmp(argv[1], "stop")) { - mc_land_detector_stop(); + land_detector_stop(); exit(0); } if (!strcmp(argv[1], "status")) { - if (mc_land_detector_task) { + if (land_detector_task) { - if (mc_land_detector_task->isRunning()) { - warnx("running"); + if (land_detector_task->isRunning()) { + warnx("running (%s)", _currentMode); } else { - errx(1, "exists, but not running"); + errx(1, "exists, but not running (%s)", _currentMode); } exit(0); @@ -194,6 +209,6 @@ int mc_land_detector_main(int argc, char *argv[]) } } - warn("usage: mc_land_detector {start|stop|status}"); + warn("usage: land_detector {start|stop|status} [mode]\nmode can either be 'fixedwing' or 'multicopter'"); return 1; } diff --git a/src/modules/land_detector/module.mk b/src/modules/land_detector/module.mk new file mode 100644 index 0000000000..db0e184472 --- /dev/null +++ b/src/modules/land_detector/module.mk @@ -0,0 +1,12 @@ +# +# Land detector +# + +MODULE_COMMAND = land_detector + +SRCS = land_detector_main.cpp \ + LandDetector.cpp \ + MulticopterLandDetector.cpp \ + FixedwingLandDetector.cpp + +EXTRACXXFLAGS = -Weffc++ -Os diff --git a/src/modules/mc_land_detector/MulticopterLandDetector.cpp b/src/modules/mc_land_detector/MulticopterLandDetector.cpp deleted file mode 100644 index d23ff9017a..0000000000 --- a/src/modules/mc_land_detector/MulticopterLandDetector.cpp +++ /dev/null @@ -1,202 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file MultiCopterLandDetector.cpp - * Land detection algorithm for multicopters - * - * @author Johan Jansen - * @author Morten Lysgaard - */ - -#include "MulticopterLandDetector.h" - -#include -#include -#include -#include //usleep - -MulticopterLandDetector::MulticopterLandDetector() : - _landDetectedPub(-1), - _landDetected({0, false}), - - _vehicleGlobalPositionSub(-1), - _sensorsCombinedSub(-1), - _waypointSub(-1), - _actuatorsSub(-1), - _armingSub(-1), - - _vehicleGlobalPosition({}), - _sensors({}), - _waypoint({}), - _actuators({}), - _arming({}), - - _taskShouldExit(false), - _taskIsRunning(false), - _landTimer(0) -{ - //Advertise the first land detected uORB - _landDetected.timestamp = hrt_absolute_time(); - _landDetected.landed = false; - _landDetectedPub = orb_advertise(ORB_ID(vehicle_land_detected), &_landDetected); -} - -MulticopterLandDetector::~MulticopterLandDetector() -{ - _taskShouldExit = true; - close(_landDetectedPub); -} - -/** -* @brief Convinience function for polling uORB subscriptions -* @return true if there was new data and it was successfully copied -**/ -static bool orb_update(const struct orb_metadata *meta, int handle, void *buffer) -{ - bool newData = false; - - //Check if there is new data to grab - if (orb_check(handle, &newData) != OK) { - return false; - } - - if (!newData) { - return false; - } - - if (orb_copy(meta, handle, buffer) != OK) { - return false; - } - - return true; -} - -void MulticopterLandDetector::shutdown() -{ - _taskShouldExit = true; -} - -void MulticopterLandDetector::updateSubscriptions() -{ - orb_update(ORB_ID(vehicle_global_position), _vehicleGlobalPositionSub, &_vehicleGlobalPosition); - orb_update(ORB_ID(sensor_combined), _sensorsCombinedSub, &_sensors); - orb_update(ORB_ID(position_setpoint_triplet), _waypointSub, &_waypoint); - orb_update(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _actuatorsSub, &_actuators); - orb_update(ORB_ID(actuator_armed), _armingSub, &_arming); -} - -void MulticopterLandDetector::landDetectorLoop() -{ - //This should never happen! - if (_taskIsRunning) { return; } - - //Subscribe to position, attitude, arming and velocity changes - _vehicleGlobalPositionSub = orb_subscribe(ORB_ID(vehicle_global_position)); - _sensorsCombinedSub = orb_subscribe(ORB_ID(sensor_combined)); - _waypointSub = orb_subscribe(ORB_ID(position_setpoint_triplet)); - _actuatorsSub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); - _armingSub = orb_subscribe(ORB_ID(actuator_armed)); - - //Begin task - _taskIsRunning = true; - _taskShouldExit = false; - - while (!_taskShouldExit) { - - //First poll for new data from our subscriptions - updateSubscriptions(); - - const uint64_t now = hrt_absolute_time(); - - //only detect landing if the autopilot is actively trying to land - if (!_waypoint.current.valid || _waypoint.current.type != SETPOINT_TYPE_LAND) { - _landTimer = now; - - } else { - - //Check if we are moving vertically - bool verticalMovement = fabsf(_vehicleGlobalPosition.vel_d) > MC_LAND_DETECTOR_CLIMBRATE_MAX; - - //Check if we are moving horizontally - bool horizontalMovement = sqrtf(_vehicleGlobalPosition.vel_n * _vehicleGlobalPosition.vel_n - + _vehicleGlobalPosition.vel_e * _vehicleGlobalPosition.vel_e) > MC_LAND_DETECTOR_VELOCITY_MAX; - - //Next look if all rotation angles are not moving - bool rotating = sqrtf(_sensors.gyro_rad_s[0] * _sensors.gyro_rad_s[0] + - _sensors.gyro_rad_s[1] * _sensors.gyro_rad_s[1] + - _sensors.gyro_rad_s[2] * _sensors.gyro_rad_s[2]) > MC_LAND_DETECTOR_ROTATION_MAX; - - //Check if thrust output is minimal (about half of default) - bool minimalThrust = _arming.armed && _actuators.control[3] <= MC_LAND_DETECTOR_THRUST_MAX; - - if (verticalMovement || rotating || !minimalThrust || horizontalMovement) { - //Sensed movement, so reset the land detector - _landTimer = now; - } - - } - - // if we have detected a landing for 2 continuous seconds - if (now - _landTimer > MC_LAND_DETECTOR_TRIGGER_TIME) { - if (!_landDetected.landed) { - _landDetected.timestamp = now; - _landDetected.landed = true; - - /* publish the land detected broadcast */ - orb_publish(ORB_ID(vehicle_land_detected), _landDetectedPub, &_landDetected); - } - - } else { - // if we currently think we have landed, but the latest data says we are flying - if (_landDetected.landed) { - _landDetected.timestamp = now; - _landDetected.landed = false; - - /* publish the land detected broadcast */ - orb_publish(ORB_ID(vehicle_land_detected), _landDetectedPub, &_landDetected); - } - } - - //Limit loop rate - usleep(1000000 / MC_LAND_DETECTOR_UPDATE_RATE); - } - - _taskIsRunning = false; - _exit(0); -} - -bool MulticopterLandDetector::isRunning() const -{ - return _taskIsRunning; -} diff --git a/src/modules/mc_land_detector/module.mk b/src/modules/mc_land_detector/module.mk deleted file mode 100644 index 023d48979a..0000000000 --- a/src/modules/mc_land_detector/module.mk +++ /dev/null @@ -1,10 +0,0 @@ -# -# Multirotor land detector -# - -MODULE_COMMAND = mc_land_detector - -SRCS = mc_land_detector_main.cpp \ - MulticopterLandDetector.cpp - -EXTRACXXFLAGS = -Weffc++ From 9ea086bf2d9b3d9d3d480f6ae83447b9669f3603 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Wed, 7 Jan 2015 23:28:20 +0100 Subject: [PATCH 498/652] Astyle: Run astyle to fix code formatting --- makefiles/config_px4fmu-v2_default.mk | 2 +- .../land_detector/FixedwingLandDetector.cpp | 74 +++++++-------- .../land_detector/FixedwingLandDetector.h | 52 +++++------ src/modules/land_detector/LandDetector.cpp | 89 ++++++++++--------- src/modules/land_detector/LandDetector.h | 70 +++++++-------- .../land_detector/MulticopterLandDetector.h | 3 +- .../land_detector/land_detector_main.cpp | 16 ++-- 7 files changed, 154 insertions(+), 152 deletions(-) diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index e13a88ca57..3abebd82fa 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -71,7 +71,7 @@ MODULES += modules/navigator MODULES += modules/mavlink MODULES += modules/gpio_led MODULES += modules/uavcan -MODULES += modules/land_detector +MODULES += modules/land_detector # # Estimation modules (EKF/ SO3 / other filters) diff --git a/src/modules/land_detector/FixedwingLandDetector.cpp b/src/modules/land_detector/FixedwingLandDetector.cpp index 42735f38c8..7df89257f7 100644 --- a/src/modules/land_detector/FixedwingLandDetector.cpp +++ b/src/modules/land_detector/FixedwingLandDetector.cpp @@ -39,65 +39,65 @@ */ #include "FixedwingLandDetector.h" - + #include #include FixedwingLandDetector::FixedwingLandDetector() : - _vehicleLocalPositionSub(-1), - _vehicleLocalPosition({}), - _airspeedSub(-1), - _airspeed({}), + _vehicleLocalPositionSub(-1), + _vehicleLocalPosition({}), + _airspeedSub(-1), + _airspeed({}), - _velocity_xy_filtered(0.0f), - _velocity_z_filtered(0.0f), - _airspeed_filtered(0.0f), - _landDetectTrigger(0) + _velocity_xy_filtered(0.0f), + _velocity_z_filtered(0.0f), + _airspeed_filtered(0.0f), + _landDetectTrigger(0) { - //ctor + //ctor } void FixedwingLandDetector::initialize() { - //Subscribe to local position and airspeed data - _vehicleLocalPositionSub = orb_subscribe(ORB_ID(vehicle_local_position)); - _airspeedSub = orb_subscribe(ORB_ID(airspeed)); + //Subscribe to local position and airspeed data + _vehicleLocalPositionSub = orb_subscribe(ORB_ID(vehicle_local_position)); + _airspeedSub = orb_subscribe(ORB_ID(airspeed)); } void FixedwingLandDetector::updateSubscriptions() { - orb_update(ORB_ID(vehicle_local_position), _vehicleLocalPositionSub, &_vehicleLocalPosition); - orb_update(ORB_ID(airspeed), _airspeedSub, &_airspeed); + orb_update(ORB_ID(vehicle_local_position), _vehicleLocalPositionSub, &_vehicleLocalPosition); + orb_update(ORB_ID(airspeed), _airspeedSub, &_airspeed); } bool FixedwingLandDetector::update() { - //First poll for new data from our subscriptions - updateSubscriptions(); + //First poll for new data from our subscriptions + updateSubscriptions(); - const uint64_t now = hrt_absolute_time(); - bool landDetected = false; + const uint64_t now = hrt_absolute_time(); + bool landDetected = false; - //TODO: reset filtered values on arming? - _velocity_xy_filtered = 0.95f * _velocity_xy_filtered + 0.05f * sqrtf(_vehicleLocalPosition.vx * - _vehicleLocalPosition.vx + _vehicleLocalPosition.vy * _vehicleLocalPosition.vy); - _velocity_z_filtered = 0.95f * _velocity_z_filtered + 0.05f * fabsf(_vehicleLocalPosition.vz); - _airspeed_filtered = 0.95f * _airspeed_filtered + 0.05f * _airspeed.true_airspeed_m_s; + //TODO: reset filtered values on arming? + _velocity_xy_filtered = 0.95f * _velocity_xy_filtered + 0.05f * sqrtf(_vehicleLocalPosition.vx * + _vehicleLocalPosition.vx + _vehicleLocalPosition.vy * _vehicleLocalPosition.vy); + _velocity_z_filtered = 0.95f * _velocity_z_filtered + 0.05f * fabsf(_vehicleLocalPosition.vz); + _airspeed_filtered = 0.95f * _airspeed_filtered + 0.05f * _airspeed.true_airspeed_m_s; - /* crude land detector for fixedwing */ - if (_velocity_xy_filtered < FW_LAND_DETECTOR_VELOCITY_MAX - && _velocity_z_filtered < FW_LAND_DETECTOR_CLIMBRATE_MAX - && _airspeed_filtered < FW_LAND_DETECTOR_AIRSPEED_MAX) { + /* crude land detector for fixedwing */ + if (_velocity_xy_filtered < FW_LAND_DETECTOR_VELOCITY_MAX + && _velocity_z_filtered < FW_LAND_DETECTOR_CLIMBRATE_MAX + && _airspeed_filtered < FW_LAND_DETECTOR_AIRSPEED_MAX) { - //These conditions need to be stable for a period of time before we trust them - if (now > _landDetectTrigger) { - landDetected = true; - } + //These conditions need to be stable for a period of time before we trust them + if (now > _landDetectTrigger) { + landDetected = true; + } - } else { - //reset land detect trigger - _landDetectTrigger = now + FW_LAND_DETECTOR_TRIGGER_TIME; - } + } else { + //reset land detect trigger + _landDetectTrigger = now + FW_LAND_DETECTOR_TRIGGER_TIME; + } - return landDetected; + return landDetected; } diff --git a/src/modules/land_detector/FixedwingLandDetector.h b/src/modules/land_detector/FixedwingLandDetector.h index faeece6f3c..6737af68a1 100644 --- a/src/modules/land_detector/FixedwingLandDetector.h +++ b/src/modules/land_detector/FixedwingLandDetector.h @@ -48,41 +48,41 @@ class FixedwingLandDetector : public LandDetector { public: - FixedwingLandDetector(); + FixedwingLandDetector(); protected: - /** - * @brief blocking loop, should be run in a separate thread or task. Runs at 50Hz - **/ - bool update() override; + /** + * @brief blocking loop, should be run in a separate thread or task. Runs at 50Hz + **/ + bool update() override; - /** - * @brief Initializes the land detection algorithm - **/ - void initialize() override; + /** + * @brief Initializes the land detection algorithm + **/ + void initialize() override; - /** - * @brief polls all subscriptions and pulls any data that has changed - **/ - void updateSubscriptions(); + /** + * @brief polls all subscriptions and pulls any data that has changed + **/ + void updateSubscriptions(); - //Algorithm parameters (TODO: should probably be externalized) - static constexpr uint64_t FW_LAND_DETECTOR_TRIGGER_TIME = 2000000; /**< usec that landing conditions have to hold + //Algorithm parameters (TODO: should probably be externalized) + static constexpr uint64_t FW_LAND_DETECTOR_TRIGGER_TIME = 2000000; /**< usec that landing conditions have to hold before triggering a land */ - static constexpr float FW_LAND_DETECTOR_VELOCITY_MAX = 5.0f; /**< maximum horizontal movement m/s*/ - static constexpr float FW_LAND_DETECTOR_CLIMBRATE_MAX = 10.00f; /**< +- climb rate in m/s*/ - static constexpr float FW_LAND_DETECTOR_AIRSPEED_MAX = 10.00f; /**< airspeed max m/s*/ + static constexpr float FW_LAND_DETECTOR_VELOCITY_MAX = 5.0f; /**< maximum horizontal movement m/s*/ + static constexpr float FW_LAND_DETECTOR_CLIMBRATE_MAX = 10.00f; /**< +- climb rate in m/s*/ + static constexpr float FW_LAND_DETECTOR_AIRSPEED_MAX = 10.00f; /**< airspeed max m/s*/ private: - int _vehicleLocalPositionSub; /**< notification of local position */ - struct vehicle_local_position_s _vehicleLocalPosition; /**< the result from local position subscription */ - int _airspeedSub; - struct airspeed_s _airspeed; + int _vehicleLocalPositionSub; /**< notification of local position */ + struct vehicle_local_position_s _vehicleLocalPosition; /**< the result from local position subscription */ + int _airspeedSub; + struct airspeed_s _airspeed; - float _velocity_xy_filtered; - float _velocity_z_filtered; - float _airspeed_filtered; - uint64_t _landDetectTrigger; + float _velocity_xy_filtered; + float _velocity_z_filtered; + float _airspeed_filtered; + uint64_t _landDetectTrigger; }; #endif //__FIXED_WING_LAND_DETECTOR_H__ diff --git a/src/modules/land_detector/LandDetector.cpp b/src/modules/land_detector/LandDetector.cpp index a39e538185..5029ce185b 100644 --- a/src/modules/land_detector/LandDetector.cpp +++ b/src/modules/land_detector/LandDetector.cpp @@ -3,75 +3,76 @@ #include LandDetector::LandDetector() : - _landDetectedPub(-1), - _landDetected({0, false}), - _taskShouldExit(false), - _taskIsRunning(false) + _landDetectedPub(-1), + _landDetected({0, false}), + _taskShouldExit(false), + _taskIsRunning(false) { - //Advertise the first land detected uORB - _landDetected.timestamp = hrt_absolute_time(); - _landDetected.landed = false; - _landDetectedPub = orb_advertise(ORB_ID(vehicle_land_detected), &_landDetected); + //Advertise the first land detected uORB + _landDetected.timestamp = hrt_absolute_time(); + _landDetected.landed = false; + _landDetectedPub = orb_advertise(ORB_ID(vehicle_land_detected), &_landDetected); } LandDetector::~LandDetector() { - _taskShouldExit = true; - close(_landDetectedPub); + _taskShouldExit = true; + close(_landDetectedPub); } void LandDetector::shutdown() { - _taskShouldExit = true; + _taskShouldExit = true; } void LandDetector::start() { - //Make sure this method has not already been called by another thread - if(isRunning()) { - return; - } + //Make sure this method has not already been called by another thread + if (isRunning()) { + return; + } - //Task is now running, keep doing so until shutdown() has been called - _taskIsRunning = true; - _taskShouldExit = false; - while(isRunning()) { + //Task is now running, keep doing so until shutdown() has been called + _taskIsRunning = true; + _taskShouldExit = false; - bool landDetected = update(); + while (isRunning()) { - //Publish if land detection state has changed - if (_landDetected.landed != landDetected) { - _landDetected.timestamp = hrt_absolute_time(); - _landDetected.landed = landDetected; + bool landDetected = update(); - /* publish the land detected broadcast */ - orb_publish(ORB_ID(vehicle_land_detected), _landDetectedPub, &_landDetected); - } + //Publish if land detection state has changed + if (_landDetected.landed != landDetected) { + _landDetected.timestamp = hrt_absolute_time(); + _landDetected.landed = landDetected; - //Limit loop rate - usleep(1000000 / LAND_DETECTOR_UPDATE_RATE); - } + /* publish the land detected broadcast */ + orb_publish(ORB_ID(vehicle_land_detected), _landDetectedPub, &_landDetected); + } - _taskIsRunning = false; - _exit(0); + //Limit loop rate + usleep(1000000 / LAND_DETECTOR_UPDATE_RATE); + } + + _taskIsRunning = false; + _exit(0); } bool LandDetector::orb_update(const struct orb_metadata *meta, int handle, void *buffer) { - bool newData = false; + bool newData = false; - //Check if there is new data to grab - if (orb_check(handle, &newData) != OK) { - return false; - } + //Check if there is new data to grab + if (orb_check(handle, &newData) != OK) { + return false; + } - if (!newData) { - return false; - } + if (!newData) { + return false; + } - if (orb_copy(meta, handle, buffer) != OK) { - return false; - } + if (orb_copy(meta, handle, buffer) != OK) { + return false; + } - return true; + return true; } diff --git a/src/modules/land_detector/LandDetector.h b/src/modules/land_detector/LandDetector.h index 6d74656917..f7120434c4 100644 --- a/src/modules/land_detector/LandDetector.h +++ b/src/modules/land_detector/LandDetector.h @@ -48,54 +48,54 @@ class LandDetector { public: - LandDetector(); - virtual ~LandDetector(); + LandDetector(); + virtual ~LandDetector(); - /** - * @return true if this task is currently running - **/ - inline bool isRunning() const {return _taskIsRunning;} + /** + * @return true if this task is currently running + **/ + inline bool isRunning() const {return _taskIsRunning;} - /** - * @brief Tells the Land Detector task that it should exit - **/ - void shutdown(); + /** + * @brief Tells the Land Detector task that it should exit + **/ + void shutdown(); - /** - * @brief Blocking function that should be called from it's own task thread. This method will - * run the underlying algorithm at the desired update rate and publish if the landing state changes. - **/ - void start(); + /** + * @brief Blocking function that should be called from it's own task thread. This method will + * run the underlying algorithm at the desired update rate and publish if the landing state changes. + **/ + void start(); protected: - /** - * @brief Pure abstract method that must be overriden by sub-classes. This actually runs the underlying algorithm - * @return true if a landing was detected and this should be broadcast to the rest of the system - **/ - virtual bool update() = 0; + /** + * @brief Pure abstract method that must be overriden by sub-classes. This actually runs the underlying algorithm + * @return true if a landing was detected and this should be broadcast to the rest of the system + **/ + virtual bool update() = 0; - /** - * @brief Pure abstract method that is called by this class once for initializing the uderlying algorithm (memory allocation, - * uORB topic subscription, creating file descriptors, etc.) - **/ - virtual void initialize() = 0; + /** + * @brief Pure abstract method that is called by this class once for initializing the uderlying algorithm (memory allocation, + * uORB topic subscription, creating file descriptors, etc.) + **/ + virtual void initialize() = 0; - /** - * @brief Convinience function for polling uORB subscriptions - * @return true if there was new data and it was successfully copied - **/ - bool orb_update(const struct orb_metadata *meta, int handle, void *buffer); + /** + * @brief Convinience function for polling uORB subscriptions + * @return true if there was new data and it was successfully copied + **/ + bool orb_update(const struct orb_metadata *meta, int handle, void *buffer); - static constexpr uint32_t LAND_DETECTOR_UPDATE_RATE = 50; /**< Run algorithm at 50Hz */ + static constexpr uint32_t LAND_DETECTOR_UPDATE_RATE = 50; /**< Run algorithm at 50Hz */ protected: - orb_advert_t _landDetectedPub; /**< publisher for position in local frame */ - struct vehicle_land_detected_s _landDetected; /**< local vehicle position */ + orb_advert_t _landDetectedPub; /**< publisher for position in local frame */ + struct vehicle_land_detected_s _landDetected; /**< local vehicle position */ private: - bool _taskShouldExit; /**< true if it is requested that this task should exit */ - bool _taskIsRunning; /**< task has reached main loop and is currently running */ + bool _taskShouldExit; /**< true if it is requested that this task should exit */ + bool _taskIsRunning; /**< task has reached main loop and is currently running */ }; #endif //__LAND_DETECTOR_H__ \ No newline at end of file diff --git a/src/modules/land_detector/MulticopterLandDetector.h b/src/modules/land_detector/MulticopterLandDetector.h index 08a8132ba5..5b4172a36d 100644 --- a/src/modules/land_detector/MulticopterLandDetector.h +++ b/src/modules/land_detector/MulticopterLandDetector.h @@ -75,7 +75,8 @@ protected: static constexpr float MC_LAND_DETECTOR_ROTATION_MAX = 0.5f; /**< max rotation in rad/sec (= 30 deg/s) */ static constexpr float MC_LAND_DETECTOR_THRUST_MAX = 0.2f; static constexpr float MC_LAND_DETECTOR_VELOCITY_MAX = 1.0f; /**< max +- horizontal movement in m/s */ - static constexpr uint32_t MC_LAND_DETECTOR_TRIGGER_TIME = 2000000; /**< usec that landing conditions have to hold before triggering a land */ + static constexpr uint32_t MC_LAND_DETECTOR_TRIGGER_TIME = + 2000000; /**< usec that landing conditions have to hold before triggering a land */ private: int _vehicleGlobalPositionSub; /**< notification of global position */ diff --git a/src/modules/land_detector/land_detector_main.cpp b/src/modules/land_detector/land_detector_main.cpp index 2bc89e7526..7c0ffb82ad 100644 --- a/src/modules/land_detector/land_detector_main.cpp +++ b/src/modules/land_detector/land_detector_main.cpp @@ -51,7 +51,7 @@ #include "MulticopterLandDetector.h" //Function prototypes -static int land_detector_start(const char* mode); +static int land_detector_start(const char *mode); static void land_detector_stop(); /** @@ -110,7 +110,7 @@ static void land_detector_stop() /** * Start new task, fails if it is already running. Returns OK if successful **/ -static int land_detector_start(const char* mode) +static int land_detector_start(const char *mode) { if (land_detector_task != nullptr || _landDetectorTaskID != -1) { errx(1, "already running"); @@ -118,15 +118,15 @@ static int land_detector_start(const char* mode) } //Allocate memory - if(!strcmp(mode, "fixedwing")) { + if (!strcmp(mode, "fixedwing")) { land_detector_task = new FixedwingLandDetector(); - } - else if(!strcmp(mode, "multicopter")) { + + } else if (!strcmp(mode, "multicopter")) { land_detector_task = new MulticopterLandDetector(); - } - else { + + } else { errx(1, "[mode] must be either 'fixedwing' or 'multicopter'"); - return -1; + return -1; } //Check if alloc worked From 73c7b44f6a684df80b550a4ce52c883348684045 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Thu, 8 Jan 2015 14:22:39 +0100 Subject: [PATCH 499/652] ROMFS: Corrected land detector startup scripts --- ROMFS/px4fmu_common/init.d/rc.fw_apps | 2 +- ROMFS/px4fmu_common/init.d/rc.mc_apps | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_apps b/ROMFS/px4fmu_common/init.d/rc.fw_apps index c6947009bb..173c1846b2 100644 --- a/ROMFS/px4fmu_common/init.d/rc.fw_apps +++ b/ROMFS/px4fmu_common/init.d/rc.fw_apps @@ -17,7 +17,7 @@ fw_pos_control_l1 start # # Start Land Detector # -fw_land_detector start +land_detector start fixedwing # # Misc apps diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_apps b/ROMFS/px4fmu_common/init.d/rc.mc_apps index 9c3d6352b3..033b3b6406 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_apps +++ b/ROMFS/px4fmu_common/init.d/rc.mc_apps @@ -14,4 +14,4 @@ mc_pos_control start # # Start Land Detector # -mc_land_detector start +land_detector start multicopter From 1356c44f0e27e9ab4d1c2df4cccbe4ac6fb2f1c4 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Wed, 14 Jan 2015 17:41:26 +0100 Subject: [PATCH 500/652] LandDetector: Fix land detection algorithm not being initialized --- src/modules/land_detector/LandDetector.cpp | 13 +++++++++---- 1 file changed, 9 insertions(+), 4 deletions(-) diff --git a/src/modules/land_detector/LandDetector.cpp b/src/modules/land_detector/LandDetector.cpp index 5029ce185b..2f1310cab0 100644 --- a/src/modules/land_detector/LandDetector.cpp +++ b/src/modules/land_detector/LandDetector.cpp @@ -8,10 +8,7 @@ LandDetector::LandDetector() : _taskShouldExit(false), _taskIsRunning(false) { - //Advertise the first land detected uORB - _landDetected.timestamp = hrt_absolute_time(); - _landDetected.landed = false; - _landDetectedPub = orb_advertise(ORB_ID(vehicle_land_detected), &_landDetected); + //ctor } LandDetector::~LandDetector() @@ -32,6 +29,14 @@ void LandDetector::start() return; } + //Advertise the first land detected uORB + _landDetected.timestamp = hrt_absolute_time(); + _landDetected.landed = false; + _landDetectedPub = orb_advertise(ORB_ID(vehicle_land_detected), &_landDetected); + + //Initialize land detection algorithm + initialize(); + //Task is now running, keep doing so until shutdown() has been called _taskIsRunning = true; _taskShouldExit = false; From f1587da4c46e92474687f37ad49fbd003b7c91db Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Wed, 14 Jan 2015 17:47:17 +0100 Subject: [PATCH 501/652] MulticopterLandDetector: Detect land even if autopilot is not landing --- .../land_detector/FixedwingLandDetector.cpp | 2 +- .../land_detector/MulticopterLandDetector.cpp | 50 +++++++++---------- .../land_detector/land_detector_main.cpp | 2 +- 3 files changed, 26 insertions(+), 28 deletions(-) diff --git a/src/modules/land_detector/FixedwingLandDetector.cpp b/src/modules/land_detector/FixedwingLandDetector.cpp index 7df89257f7..38f6c00a95 100644 --- a/src/modules/land_detector/FixedwingLandDetector.cpp +++ b/src/modules/land_detector/FixedwingLandDetector.cpp @@ -43,7 +43,7 @@ #include #include -FixedwingLandDetector::FixedwingLandDetector() : +FixedwingLandDetector::FixedwingLandDetector() : LandDetector(), _vehicleLocalPositionSub(-1), _vehicleLocalPosition({}), _airspeedSub(-1), diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp index 36d92fd798..81af029fb1 100644 --- a/src/modules/land_detector/MulticopterLandDetector.cpp +++ b/src/modules/land_detector/MulticopterLandDetector.cpp @@ -44,7 +44,7 @@ #include #include -MulticopterLandDetector::MulticopterLandDetector() : +MulticopterLandDetector::MulticopterLandDetector() : LandDetector(), _vehicleGlobalPositionSub(-1), _sensorsCombinedSub(-1), _waypointSub(-1), @@ -86,34 +86,32 @@ bool MulticopterLandDetector::update() //First poll for new data from our subscriptions updateSubscriptions(); + //Only trigger flight conditions if we are armed + if(!_arming.armed) { + return true; + } + const uint64_t now = hrt_absolute_time(); - //only detect landing if the autopilot is actively trying to land - if (!_waypoint.current.valid || _waypoint.current.type != SETPOINT_TYPE_LAND) { + //Check if we are moving vertically + bool verticalMovement = fabsf(_vehicleGlobalPosition.vel_d) > MC_LAND_DETECTOR_CLIMBRATE_MAX; + + //Check if we are moving horizontally + bool horizontalMovement = sqrtf(_vehicleGlobalPosition.vel_n * _vehicleGlobalPosition.vel_n + + _vehicleGlobalPosition.vel_e * _vehicleGlobalPosition.vel_e) > MC_LAND_DETECTOR_VELOCITY_MAX; + + //Next look if all rotation angles are not moving + bool rotating = sqrtf(_sensors.gyro_rad_s[0] * _sensors.gyro_rad_s[0] + + _sensors.gyro_rad_s[1] * _sensors.gyro_rad_s[1] + + _sensors.gyro_rad_s[2] * _sensors.gyro_rad_s[2]) > MC_LAND_DETECTOR_ROTATION_MAX; + + //Check if thrust output is minimal (about half of default) + bool minimalThrust = _actuators.control[3] <= MC_LAND_DETECTOR_THRUST_MAX; + + if (verticalMovement || rotating || !minimalThrust || horizontalMovement) { + //Sensed movement, so reset the land detector _landTimer = now; - - } else { - - //Check if we are moving vertically - bool verticalMovement = fabsf(_vehicleGlobalPosition.vel_d) > MC_LAND_DETECTOR_CLIMBRATE_MAX; - - //Check if we are moving horizontally - bool horizontalMovement = sqrtf(_vehicleGlobalPosition.vel_n * _vehicleGlobalPosition.vel_n - + _vehicleGlobalPosition.vel_e * _vehicleGlobalPosition.vel_e) > MC_LAND_DETECTOR_VELOCITY_MAX; - - //Next look if all rotation angles are not moving - bool rotating = sqrtf(_sensors.gyro_rad_s[0] * _sensors.gyro_rad_s[0] + - _sensors.gyro_rad_s[1] * _sensors.gyro_rad_s[1] + - _sensors.gyro_rad_s[2] * _sensors.gyro_rad_s[2]) > MC_LAND_DETECTOR_ROTATION_MAX; - - //Check if thrust output is minimal (about half of default) - bool minimalThrust = _arming.armed && _actuators.control[3] <= MC_LAND_DETECTOR_THRUST_MAX; - - if (verticalMovement || rotating || !minimalThrust || horizontalMovement) { - //Sensed movement, so reset the land detector - _landTimer = now; - } - + return false; } return now - _landTimer > MC_LAND_DETECTOR_TRIGGER_TIME; diff --git a/src/modules/land_detector/land_detector_main.cpp b/src/modules/land_detector/land_detector_main.cpp index 7c0ffb82ad..9e135b5f17 100644 --- a/src/modules/land_detector/land_detector_main.cpp +++ b/src/modules/land_detector/land_detector_main.cpp @@ -104,7 +104,7 @@ static void land_detector_stop() delete land_detector_task; land_detector_task = nullptr; _landDetectorTaskID = -1; - warn("land_detector has been stopped"); + errx(0, "land_detector has been stopped"); } /** From e40d207311126b05a7fd87739fb72d2ae8d7d500 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Thu, 15 Jan 2015 10:06:56 +0100 Subject: [PATCH 502/652] AStyle: Fixed file formatting --- ROMFS/px4fmu_common/init.d/rc.fw_apps | 2 +- src/modules/land_detector/LandDetector.cpp | 2 +- src/modules/land_detector/LandDetector.h | 2 +- src/modules/land_detector/MulticopterLandDetector.cpp | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_apps b/ROMFS/px4fmu_common/init.d/rc.fw_apps index 173c1846b2..9e2d1f6ffe 100644 --- a/ROMFS/px4fmu_common/init.d/rc.fw_apps +++ b/ROMFS/px4fmu_common/init.d/rc.fw_apps @@ -21,4 +21,4 @@ land_detector start fixedwing # # Misc apps -# \ No newline at end of file +# diff --git a/src/modules/land_detector/LandDetector.cpp b/src/modules/land_detector/LandDetector.cpp index 2f1310cab0..1dd09b6a67 100644 --- a/src/modules/land_detector/LandDetector.cpp +++ b/src/modules/land_detector/LandDetector.cpp @@ -32,7 +32,7 @@ void LandDetector::start() //Advertise the first land detected uORB _landDetected.timestamp = hrt_absolute_time(); _landDetected.landed = false; - _landDetectedPub = orb_advertise(ORB_ID(vehicle_land_detected), &_landDetected); + _landDetectedPub = orb_advertise(ORB_ID(vehicle_land_detected), &_landDetected); //Initialize land detection algorithm initialize(); diff --git a/src/modules/land_detector/LandDetector.h b/src/modules/land_detector/LandDetector.h index f7120434c4..ba15ad498b 100644 --- a/src/modules/land_detector/LandDetector.h +++ b/src/modules/land_detector/LandDetector.h @@ -98,4 +98,4 @@ private: bool _taskIsRunning; /**< task has reached main loop and is currently running */ }; -#endif //__LAND_DETECTOR_H__ \ No newline at end of file +#endif //__LAND_DETECTOR_H__ diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp index 81af029fb1..8c25ae5fe7 100644 --- a/src/modules/land_detector/MulticopterLandDetector.cpp +++ b/src/modules/land_detector/MulticopterLandDetector.cpp @@ -87,7 +87,7 @@ bool MulticopterLandDetector::update() updateSubscriptions(); //Only trigger flight conditions if we are armed - if(!_arming.armed) { + if (!_arming.armed) { return true; } From 92add9cf8003c4fd8b01143626c3a101062dd9dd Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Thu, 15 Jan 2015 14:36:04 +0100 Subject: [PATCH 503/652] LandDetector: Externalized algorithm parameters --- .../land_detector/FixedwingLandDetector.cpp | 33 +++++- .../land_detector/FixedwingLandDetector.h | 29 ++++- src/modules/land_detector/LandDetector.h | 3 + .../land_detector/MulticopterLandDetector.cpp | 41 ++++++- .../land_detector/MulticopterLandDetector.h | 33 ++++-- .../land_detector/land_detector_params.c | 105 ++++++++++++++++++ src/modules/land_detector/module.mk | 1 + 7 files changed, 221 insertions(+), 24 deletions(-) create mode 100644 src/modules/land_detector/land_detector_params.c diff --git a/src/modules/land_detector/FixedwingLandDetector.cpp b/src/modules/land_detector/FixedwingLandDetector.cpp index 38f6c00a95..0d6f373100 100644 --- a/src/modules/land_detector/FixedwingLandDetector.cpp +++ b/src/modules/land_detector/FixedwingLandDetector.cpp @@ -44,17 +44,22 @@ #include FixedwingLandDetector::FixedwingLandDetector() : LandDetector(), + _paramHandle(), + _params(), _vehicleLocalPositionSub(-1), _vehicleLocalPosition({}), _airspeedSub(-1), _airspeed({}), + _parameterSub(-1), _velocity_xy_filtered(0.0f), _velocity_z_filtered(0.0f), _airspeed_filtered(0.0f), _landDetectTrigger(0) { - //ctor + _paramHandle.maxVelocity = param_find("LAND_FW_VELOCITY_MAX"); + _paramHandle.maxClimbRate = param_find("LAND_FW_MAX_CLIMB_RATE"); + _paramHandle.maxAirSpeed = param_find("LAND_FW_AIR_SPEED_MAX"); } void FixedwingLandDetector::initialize() @@ -85,9 +90,9 @@ bool FixedwingLandDetector::update() _airspeed_filtered = 0.95f * _airspeed_filtered + 0.05f * _airspeed.true_airspeed_m_s; /* crude land detector for fixedwing */ - if (_velocity_xy_filtered < FW_LAND_DETECTOR_VELOCITY_MAX - && _velocity_z_filtered < FW_LAND_DETECTOR_CLIMBRATE_MAX - && _airspeed_filtered < FW_LAND_DETECTOR_AIRSPEED_MAX) { + if (_velocity_xy_filtered < _params.maxVelocity + && _velocity_z_filtered < _params.maxClimbRate + && _airspeed_filtered < _params.maxAirSpeed) { //These conditions need to be stable for a period of time before we trust them if (now > _landDetectTrigger) { @@ -96,8 +101,26 @@ bool FixedwingLandDetector::update() } else { //reset land detect trigger - _landDetectTrigger = now + FW_LAND_DETECTOR_TRIGGER_TIME; + _landDetectTrigger = now + LAND_DETECTOR_TRIGGER_TIME; } return landDetected; } + +void FixedwingLandDetector::updateParameterCache(const bool force) +{ + bool updated; + parameter_update_s paramUpdate; + + orb_check(_parameterSub, &updated); + + if (updated) { + orb_copy(ORB_ID(parameter_update), _parameterSub, ¶mUpdate); + } + + if (updated || force) { + param_get(_paramHandle.maxVelocity, &_params.maxVelocity); + param_get(_paramHandle.maxClimbRate, &_params.maxClimbRate); + param_get(_paramHandle.maxAirSpeed, &_params.maxAirSpeed); + } +} diff --git a/src/modules/land_detector/FixedwingLandDetector.h b/src/modules/land_detector/FixedwingLandDetector.h index 6737af68a1..0e9c092ee0 100644 --- a/src/modules/land_detector/FixedwingLandDetector.h +++ b/src/modules/land_detector/FixedwingLandDetector.h @@ -44,6 +44,8 @@ #include "LandDetector.h" #include #include +#include +#include class FixedwingLandDetector : public LandDetector { @@ -66,18 +68,33 @@ protected: **/ void updateSubscriptions(); - //Algorithm parameters (TODO: should probably be externalized) - static constexpr uint64_t FW_LAND_DETECTOR_TRIGGER_TIME = 2000000; /**< usec that landing conditions have to hold - before triggering a land */ - static constexpr float FW_LAND_DETECTOR_VELOCITY_MAX = 5.0f; /**< maximum horizontal movement m/s*/ - static constexpr float FW_LAND_DETECTOR_CLIMBRATE_MAX = 10.00f; /**< +- climb rate in m/s*/ - static constexpr float FW_LAND_DETECTOR_AIRSPEED_MAX = 10.00f; /**< airspeed max m/s*/ +private: + /** + * @brief download and update local parameter cache + **/ + void updateParameterCache(const bool force); + + /** + * @brief Handles for interesting parameters + **/ + struct { + param_t maxVelocity; + param_t maxClimbRate; + param_t maxAirSpeed; + } _paramHandle; + + struct { + float maxVelocity; + float maxClimbRate; + float maxAirSpeed; + } _params; private: int _vehicleLocalPositionSub; /**< notification of local position */ struct vehicle_local_position_s _vehicleLocalPosition; /**< the result from local position subscription */ int _airspeedSub; struct airspeed_s _airspeed; + int _parameterSub; float _velocity_xy_filtered; float _velocity_z_filtered; diff --git a/src/modules/land_detector/LandDetector.h b/src/modules/land_detector/LandDetector.h index ba15ad498b..09db6e4746 100644 --- a/src/modules/land_detector/LandDetector.h +++ b/src/modules/land_detector/LandDetector.h @@ -89,6 +89,9 @@ protected: static constexpr uint32_t LAND_DETECTOR_UPDATE_RATE = 50; /**< Run algorithm at 50Hz */ + static constexpr uint64_t LAND_DETECTOR_TRIGGER_TIME = 2000000; /**< usec that landing conditions have to hold + before triggering a land */ + protected: orb_advert_t _landDetectedPub; /**< publisher for position in local frame */ struct vehicle_land_detected_s _landDetected; /**< local vehicle position */ diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp index 8c25ae5fe7..278b0cd51e 100644 --- a/src/modules/land_detector/MulticopterLandDetector.cpp +++ b/src/modules/land_detector/MulticopterLandDetector.cpp @@ -45,11 +45,14 @@ #include MulticopterLandDetector::MulticopterLandDetector() : LandDetector(), + _paramHandle(), + _params(), _vehicleGlobalPositionSub(-1), _sensorsCombinedSub(-1), _waypointSub(-1), _actuatorsSub(-1), _armingSub(-1), + _parameterSub(-1), _vehicleGlobalPosition({}), _sensors({}), @@ -59,7 +62,10 @@ MulticopterLandDetector::MulticopterLandDetector() : LandDetector(), _landTimer(0) { - //ctor + _paramHandle.maxRotation = param_find("LAND_MC_MAX_CLIMB_RATE"); + _paramHandle.maxVelocity = param_find("LAND_MC_VELOCITY_MAX"); + _paramHandle.maxClimbRate = param_find("LAND_MC_ROTATION_MAX"); + _paramHandle.maxThrottle = param_find("LAND_MC_THRUST_MAX"); } void MulticopterLandDetector::initialize() @@ -70,6 +76,10 @@ void MulticopterLandDetector::initialize() _waypointSub = orb_subscribe(ORB_ID(position_setpoint_triplet)); _actuatorsSub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); _armingSub = orb_subscribe(ORB_ID(actuator_armed)); + _parameterSub = orb_subscribe(ORB_ID(parameter_update)); + + //Download parameters + updateParameterCache(true); } void MulticopterLandDetector::updateSubscriptions() @@ -94,19 +104,19 @@ bool MulticopterLandDetector::update() const uint64_t now = hrt_absolute_time(); //Check if we are moving vertically - bool verticalMovement = fabsf(_vehicleGlobalPosition.vel_d) > MC_LAND_DETECTOR_CLIMBRATE_MAX; + bool verticalMovement = fabsf(_vehicleGlobalPosition.vel_d) > _params.maxClimbRate; //Check if we are moving horizontally bool horizontalMovement = sqrtf(_vehicleGlobalPosition.vel_n * _vehicleGlobalPosition.vel_n - + _vehicleGlobalPosition.vel_e * _vehicleGlobalPosition.vel_e) > MC_LAND_DETECTOR_VELOCITY_MAX; + + _vehicleGlobalPosition.vel_e * _vehicleGlobalPosition.vel_e) > _params.maxVelocity; //Next look if all rotation angles are not moving bool rotating = sqrtf(_sensors.gyro_rad_s[0] * _sensors.gyro_rad_s[0] + _sensors.gyro_rad_s[1] * _sensors.gyro_rad_s[1] + - _sensors.gyro_rad_s[2] * _sensors.gyro_rad_s[2]) > MC_LAND_DETECTOR_ROTATION_MAX; + _sensors.gyro_rad_s[2] * _sensors.gyro_rad_s[2]) > _params.maxRotation; //Check if thrust output is minimal (about half of default) - bool minimalThrust = _actuators.control[3] <= MC_LAND_DETECTOR_THRUST_MAX; + bool minimalThrust = _actuators.control[3] <= _params.maxThrottle; if (verticalMovement || rotating || !minimalThrust || horizontalMovement) { //Sensed movement, so reset the land detector @@ -114,5 +124,24 @@ bool MulticopterLandDetector::update() return false; } - return now - _landTimer > MC_LAND_DETECTOR_TRIGGER_TIME; + return now - _landTimer > LAND_DETECTOR_TRIGGER_TIME; +} + +void MulticopterLandDetector::updateParameterCache(const bool force) +{ + bool updated; + parameter_update_s paramUpdate; + + orb_check(_parameterSub, &updated); + + if (updated) { + orb_copy(ORB_ID(parameter_update), _parameterSub, ¶mUpdate); + } + + if (updated || force) { + param_get(_paramHandle.maxClimbRate, &_params.maxClimbRate); + param_get(_paramHandle.maxVelocity, &_params.maxVelocity); + param_get(_paramHandle.maxRotation, &_params.maxRotation); + param_get(_paramHandle.maxThrottle, &_params.maxThrottle); + } } diff --git a/src/modules/land_detector/MulticopterLandDetector.h b/src/modules/land_detector/MulticopterLandDetector.h index 5b4172a36d..7bb7f1a907 100644 --- a/src/modules/land_detector/MulticopterLandDetector.h +++ b/src/modules/land_detector/MulticopterLandDetector.h @@ -48,6 +48,8 @@ #include #include #include +#include +#include class MulticopterLandDetector : public LandDetector { @@ -70,13 +72,29 @@ protected: **/ void initialize() override; - //Algorithm parameters (TODO: should probably be externalized) - static constexpr float MC_LAND_DETECTOR_CLIMBRATE_MAX = 0.30f; /**< max +- climb rate in m/s */ - static constexpr float MC_LAND_DETECTOR_ROTATION_MAX = 0.5f; /**< max rotation in rad/sec (= 30 deg/s) */ - static constexpr float MC_LAND_DETECTOR_THRUST_MAX = 0.2f; - static constexpr float MC_LAND_DETECTOR_VELOCITY_MAX = 1.0f; /**< max +- horizontal movement in m/s */ - static constexpr uint32_t MC_LAND_DETECTOR_TRIGGER_TIME = - 2000000; /**< usec that landing conditions have to hold before triggering a land */ + +private: + /** + * @brief download and update local parameter cache + **/ + void updateParameterCache(const bool force); + + /** + * @brief Handles for interesting parameters + **/ + struct { + param_t maxClimbRate; + param_t maxVelocity; + param_t maxRotation; + param_t maxThrottle; + } _paramHandle; + + struct { + float maxClimbRate; + float maxVelocity; + float maxRotation; + float maxThrottle; + } _params; private: int _vehicleGlobalPositionSub; /**< notification of global position */ @@ -84,6 +102,7 @@ private: int _waypointSub; int _actuatorsSub; int _armingSub; + int _parameterSub; struct vehicle_global_position_s _vehicleGlobalPosition; /**< the result from global position subscription */ struct sensor_combined_s _sensors; /**< subscribe to sensor readings */ diff --git a/src/modules/land_detector/land_detector_params.c b/src/modules/land_detector/land_detector_params.c new file mode 100644 index 0000000000..4aecae5372 --- /dev/null +++ b/src/modules/land_detector/land_detector_params.c @@ -0,0 +1,105 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Author: @author Anton Babushkin + * + * 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 land_detector.c + * Land detector algorithm parameters. + * + * @author Johan Jansen + */ + +#include + +/** + * Multicopter max climb rate + * + * Maximum vertical velocity allowed to trigger a land (m/s up and down) + * + * @group Land Detector + */ +PARAM_DEFINE_FLOAT(LAND_MC_MAX_CLIMB_RATE, 0.30f); + +/** + * Multicopter max horizontal velocity + * + * Maximum horizontal velocity allowed to trigger a land (m/s) + * + * @group Land Detector + */ +PARAM_DEFINE_FLOAT(LAND_MC_VELOCITY_MAX, 1.00f); + +/** + * Multicopter max rotation + * + * Maximum allowed around each axis to trigger a land (radians per second) + * + * @group Land Detector + */ +PARAM_DEFINE_FLOAT(LAND_MC_MAX_ROTATION, 0.20f); + +/** + * Multicopter max throttle + * + * Maximum actuator output on throttle before triggering a land + * + * @group Land Detector + */ +PARAM_DEFINE_FLOAT(LAND_MC_MAX_THROTTLE, 0.20f); + +/** + * Fixedwing max horizontal velocity + * + * Maximum horizontal velocity allowed to trigger a land (m/s) + * + * @group Land Detector + */ +PARAM_DEFINE_FLOAT(LAND_FW_VELOCITY_MAX, 0.20f); + +/** + * Fixedwing max climb rate + * + * Maximum vertical velocity allowed to trigger a land (m/s up and down) + * + * @group Land Detector + */ +PARAM_DEFINE_FLOAT(LAND_FW_MAX_CLIMB_RATE, 10.00f); + +/** + * Airspeed max + * + * Maximum airspeed allowed to trigger a land (m/s) + * + * @group Land Detector + */ +PARAM_DEFINE_FLOAT(LAND_FW_AIRSPEED_MAX, 10.00f); diff --git a/src/modules/land_detector/module.mk b/src/modules/land_detector/module.mk index db0e184472..e08a4b7a81 100644 --- a/src/modules/land_detector/module.mk +++ b/src/modules/land_detector/module.mk @@ -5,6 +5,7 @@ MODULE_COMMAND = land_detector SRCS = land_detector_main.cpp \ + land_detector_params.c \ LandDetector.cpp \ MulticopterLandDetector.cpp \ FixedwingLandDetector.cpp From 510a314386529f95978078d27da25368435d8d90 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Thu, 15 Jan 2015 14:58:06 +0100 Subject: [PATCH 504/652] LandDetector: Shorter and less ambiguous param names --- src/modules/land_detector/FixedwingLandDetector.cpp | 6 +++--- .../land_detector/MulticopterLandDetector.cpp | 8 ++++---- src/modules/land_detector/land_detector_params.c | 12 ++++++------ 3 files changed, 13 insertions(+), 13 deletions(-) diff --git a/src/modules/land_detector/FixedwingLandDetector.cpp b/src/modules/land_detector/FixedwingLandDetector.cpp index 0d6f373100..52084e4c05 100644 --- a/src/modules/land_detector/FixedwingLandDetector.cpp +++ b/src/modules/land_detector/FixedwingLandDetector.cpp @@ -57,9 +57,9 @@ FixedwingLandDetector::FixedwingLandDetector() : LandDetector(), _airspeed_filtered(0.0f), _landDetectTrigger(0) { - _paramHandle.maxVelocity = param_find("LAND_FW_VELOCITY_MAX"); - _paramHandle.maxClimbRate = param_find("LAND_FW_MAX_CLIMB_RATE"); - _paramHandle.maxAirSpeed = param_find("LAND_FW_AIR_SPEED_MAX"); + _paramHandle.maxVelocity = param_find("LAND_FW_VEL_XY_MAX"); + _paramHandle.maxClimbRate = param_find("LAND_FW_VEL_Z_MAX"); + _paramHandle.maxAirSpeed = param_find("LAND_FW_AIRSPEED_MAX"); } void FixedwingLandDetector::initialize() diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp index 278b0cd51e..5371346181 100644 --- a/src/modules/land_detector/MulticopterLandDetector.cpp +++ b/src/modules/land_detector/MulticopterLandDetector.cpp @@ -62,10 +62,10 @@ MulticopterLandDetector::MulticopterLandDetector() : LandDetector(), _landTimer(0) { - _paramHandle.maxRotation = param_find("LAND_MC_MAX_CLIMB_RATE"); - _paramHandle.maxVelocity = param_find("LAND_MC_VELOCITY_MAX"); - _paramHandle.maxClimbRate = param_find("LAND_MC_ROTATION_MAX"); - _paramHandle.maxThrottle = param_find("LAND_MC_THRUST_MAX"); + _paramHandle.maxRotation = param_find("LAND_MC_Z_VEL_MAX"); + _paramHandle.maxVelocity = param_find("LAND_MC_XY_VEL_MAX"); + _paramHandle.maxClimbRate = param_find("LAND_MC_ROT_MAX"); + _paramHandle.maxThrottle = param_find("LAND_MC_THR_MAX"); } void MulticopterLandDetector::initialize() diff --git a/src/modules/land_detector/land_detector_params.c b/src/modules/land_detector/land_detector_params.c index 4aecae5372..e2acf42b3d 100644 --- a/src/modules/land_detector/land_detector_params.c +++ b/src/modules/land_detector/land_detector_params.c @@ -48,7 +48,7 @@ * * @group Land Detector */ -PARAM_DEFINE_FLOAT(LAND_MC_MAX_CLIMB_RATE, 0.30f); +PARAM_DEFINE_FLOAT(LAND_MC_Z_VEL_MAX, 0.30f); /** * Multicopter max horizontal velocity @@ -57,7 +57,7 @@ PARAM_DEFINE_FLOAT(LAND_MC_MAX_CLIMB_RATE, 0.30f); * * @group Land Detector */ -PARAM_DEFINE_FLOAT(LAND_MC_VELOCITY_MAX, 1.00f); +PARAM_DEFINE_FLOAT(LAND_MC_XY_VEL_MAX, 1.00f); /** * Multicopter max rotation @@ -66,7 +66,7 @@ PARAM_DEFINE_FLOAT(LAND_MC_VELOCITY_MAX, 1.00f); * * @group Land Detector */ -PARAM_DEFINE_FLOAT(LAND_MC_MAX_ROTATION, 0.20f); +PARAM_DEFINE_FLOAT(LAND_MC_ROT_MAX, 0.20f); /** * Multicopter max throttle @@ -75,7 +75,7 @@ PARAM_DEFINE_FLOAT(LAND_MC_MAX_ROTATION, 0.20f); * * @group Land Detector */ -PARAM_DEFINE_FLOAT(LAND_MC_MAX_THROTTLE, 0.20f); +PARAM_DEFINE_FLOAT(LAND_MC_THR_MAX, 0.20f); /** * Fixedwing max horizontal velocity @@ -84,7 +84,7 @@ PARAM_DEFINE_FLOAT(LAND_MC_MAX_THROTTLE, 0.20f); * * @group Land Detector */ -PARAM_DEFINE_FLOAT(LAND_FW_VELOCITY_MAX, 0.20f); +PARAM_DEFINE_FLOAT(LAND_FW_VEL_XY_MAX, 0.20f); /** * Fixedwing max climb rate @@ -93,7 +93,7 @@ PARAM_DEFINE_FLOAT(LAND_FW_VELOCITY_MAX, 0.20f); * * @group Land Detector */ -PARAM_DEFINE_FLOAT(LAND_FW_MAX_CLIMB_RATE, 10.00f); +PARAM_DEFINE_FLOAT(LAND_FW_VEL_Z_MAX, 10.00f); /** * Airspeed max From 818ccf7a04fd1aacef4d9e1c6d96493a37302006 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Fri, 16 Jan 2015 15:51:57 +0100 Subject: [PATCH 505/652] Commander: Fix for resetting home timestamp on arm --- src/modules/commander/commander.cpp | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index f511f38762..bae1ed2e43 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1860,7 +1860,8 @@ int commander_thread_main(int argc, char *argv[]) } - hrt_abstime t1 = hrt_absolute_time(); + //Get current timestamp + const hrt_abstime now = hrt_absolute_time(); /* print new state */ if (arming_state_changed) { @@ -1871,7 +1872,8 @@ int commander_thread_main(int argc, char *argv[]) if (armed.armed && !was_armed && hrt_absolute_time() > start_time + 2000000 && status.condition_global_position_valid && (global_position.eph < eph_threshold) && (global_position.epv < epv_threshold)) { - // TODO remove code duplication + // TODO remove code duplication (setting home is also done somewhere else in this file) + home.timestamp = now; home.lat = global_position.lat; home.lon = global_position.lon; home.alt = global_position.alt; @@ -1932,13 +1934,13 @@ int commander_thread_main(int argc, char *argv[]) /* publish states (armed, control mode, vehicle status) at least with 5 Hz */ if (counter % (200000 / COMMANDER_MONITORING_INTERVAL) == 0 || status_changed) { set_control_mode(); - control_mode.timestamp = t1; + control_mode.timestamp = now; orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, &control_mode); - status.timestamp = t1; + status.timestamp = now; orb_publish(ORB_ID(vehicle_status), status_pub, &status); - armed.timestamp = t1; + armed.timestamp = now; orb_publish(ORB_ID(actuator_armed), armed_pub, &armed); } From a581ae8ed60f0f08d4bf407aeeb0f3979b6ba38a Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Fri, 16 Jan 2015 16:43:11 +0100 Subject: [PATCH 506/652] Commander: Remove duplicate code for setting home position --- src/modules/commander/commander.cpp | 126 ++++++++++++++-------------- 1 file changed, 64 insertions(+), 62 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index bae1ed2e43..57f94259ca 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -240,6 +240,12 @@ transition_result_t check_navigation_state_machine(struct vehicle_status_s *stat transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char *armedBy); +/** +* @brief This function initializes the home position of the vehicle. This happens first time we get a good GPS fix and each +* time the vehicle is armed with a good GPS fix. +**/ +static void commander_set_home_position(orb_advert_t &homePub, home_position_s &home, const vehicle_local_position_s &localPosition, const vehicle_global_position_s &globalPosition); + /** * Loop that runs at a lower rate and priority for calibration and parameter tasks. */ @@ -716,6 +722,52 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s return true; } +/** +* @brief This function initializes the home position of the vehicle. This happens first time we get a good GPS fix and each +* time the vehicle is armed with a good GPS fix. +**/ +static void commander_set_home_position(orb_advert_t &homePub, home_position_s &home, const vehicle_local_position_s &localPosition, const vehicle_global_position_s &globalPosition) +{ + //Need global position fix to be able to set home + if(!status.condition_global_position_valid) { + return; + } + + //Ensure that the GPS accuracy is good enough for intializing home + if(globalPosition.eph > eph_threshold || globalPosition.epv > epv_threshold) { + return; + } + + //Set home position + home.timestamp = hrt_absolute_time(); + home.lat = globalPosition.lat; + home.lon = globalPosition.lon; + home.alt = globalPosition.alt; + + home.x = localPosition.x; + home.y = localPosition.y; + home.z = localPosition.z; + + warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", home.lat, home.lon, (double)home.alt); + mavlink_log_info(mavlink_fd, "home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt); + + /* announce new home position */ + if (homePub > 0) { + orb_publish(ORB_ID(home_position), homePub, &home); + + } else { + homePub = orb_advertise(ORB_ID(home_position), &home); + } + + //Play tune first time we initialize HOME + if(!status.condition_home_position_valid) { + tune_positive(true); + } + + /* mark home position as set */ + status.condition_home_position_valid = true; +} + int commander_thread_main(int argc, char *argv[]) { /* not yet initialized */ @@ -904,7 +956,6 @@ int commander_thread_main(int argc, char *argv[]) bool critical_battery_voltage_actions_done = false; hrt_abstime last_idle_time = 0; - hrt_abstime start_time = 0; bool status_changed = true; bool param_init_forced = true; @@ -1035,7 +1086,7 @@ int commander_thread_main(int argc, char *argv[]) commander_initialized = true; thread_running = true; - start_time = hrt_absolute_time(); + const hrt_abstime commander_boot_timestamp = hrt_absolute_time(); transition_result_t arming_ret; @@ -1325,34 +1376,6 @@ int commander_thread_main(int argc, char *argv[]) check_valid(global_position.timestamp, POSITION_TIMEOUT, eph_good, &(status.condition_global_position_valid), &status_changed); - /* update home position */ - if (!status.condition_home_position_valid && status.condition_global_position_valid && !armed.armed && - (global_position.eph < eph_threshold) && (global_position.epv < epv_threshold)) { - - home.lat = global_position.lat; - home.lon = global_position.lon; - home.alt = global_position.alt; - - home.x = local_position.x; - home.y = local_position.y; - home.z = local_position.z; - - warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", home.lat, home.lon, (double)home.alt); - mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt); - - /* announce new home position */ - if (home_pub > 0) { - orb_publish(ORB_ID(home_position), home_pub, &home); - - } else { - home_pub = orb_advertise(ORB_ID(home_position), &home); - } - - /* mark home position as set */ - status.condition_home_position_valid = true; - tune_positive(true); - } - /* update condition_local_position_valid and condition_local_altitude_valid */ /* hysteresis for EPH */ bool local_eph_good; @@ -1401,7 +1424,7 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_controls_sub, &actuator_controls); /* only consider battery voltage if system has been running 2s and battery voltage is valid */ - if (hrt_absolute_time() > start_time + 2000000 && battery.voltage_filtered_v > 0.0f) { + if (hrt_absolute_time() > commander_boot_timestamp + 2000000 && battery.voltage_filtered_v > 0.0f) { status.battery_voltage = battery.voltage_filtered_v; status.battery_current = battery.current_a; status.condition_battery_voltage_valid = true; @@ -1859,44 +1882,23 @@ int commander_thread_main(int argc, char *argv[]) } } - //Get current timestamp const hrt_abstime now = hrt_absolute_time(); + //First time home position update + if(!status.condition_home_position_valid) { + commander_set_home_position(home_pub, home, local_position, global_position); + } + + /* update home position on arming if at least 2s from commander start spent to avoid setting home on in-air restart */ + else if(arming_state_changed && armed.armed && !was_armed && now > commander_boot_timestamp + 2000000) { + commander_set_home_position(home_pub, home, local_position, global_position); + } + /* print new state */ if (arming_state_changed) { status_changed = true; mavlink_log_info(mavlink_fd, "[cmd] arming state: %s", arming_states_str[status.arming_state]); - - /* update home position on arming if at least 2s from commander start spent to avoid setting home on in-air restart */ - if (armed.armed && !was_armed && hrt_absolute_time() > start_time + 2000000 && status.condition_global_position_valid && - (global_position.eph < eph_threshold) && (global_position.epv < epv_threshold)) { - - // TODO remove code duplication (setting home is also done somewhere else in this file) - home.timestamp = now; - home.lat = global_position.lat; - home.lon = global_position.lon; - home.alt = global_position.alt; - - home.x = local_position.x; - home.y = local_position.y; - home.z = local_position.z; - - warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", home.lat, home.lon, (double)home.alt); - mavlink_log_info(mavlink_fd, "home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt); - - /* announce new home position */ - if (home_pub > 0) { - orb_publish(ORB_ID(home_position), home_pub, &home); - - } else { - home_pub = orb_advertise(ORB_ID(home_position), &home); - } - - /* mark home position as set */ - status.condition_home_position_valid = true; - } - arming_state_changed = false; } From 9ecadcd9b46ccf9bc58ea008b77f8408155c3665 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Fri, 16 Jan 2015 16:46:16 +0100 Subject: [PATCH 507/652] Astyle: Fix format for commander.cpp --- src/modules/commander/commander.cpp | 50 +++++++++++++++++------------ 1 file changed, 29 insertions(+), 21 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 57f94259ca..aac6b20023 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -244,7 +244,8 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char *armed * @brief This function initializes the home position of the vehicle. This happens first time we get a good GPS fix and each * time the vehicle is armed with a good GPS fix. **/ -static void commander_set_home_position(orb_advert_t &homePub, home_position_s &home, const vehicle_local_position_s &localPosition, const vehicle_global_position_s &globalPosition); +static void commander_set_home_position(orb_advert_t &homePub, home_position_s &home, + const vehicle_local_position_s &localPosition, const vehicle_global_position_s &globalPosition); /** * Loop that runs at a lower rate and priority for calibration and parameter tasks. @@ -566,7 +567,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s } break; - /* Flight termination */ + /* Flight termination */ case VEHICLE_CMD_DO_FLIGHTTERMINATION: { if (cmd->param1 > 0.5f) { //XXX update state machine? @@ -726,15 +727,16 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s * @brief This function initializes the home position of the vehicle. This happens first time we get a good GPS fix and each * time the vehicle is armed with a good GPS fix. **/ -static void commander_set_home_position(orb_advert_t &homePub, home_position_s &home, const vehicle_local_position_s &localPosition, const vehicle_global_position_s &globalPosition) +static void commander_set_home_position(orb_advert_t &homePub, home_position_s &home, + const vehicle_local_position_s &localPosition, const vehicle_global_position_s &globalPosition) { //Need global position fix to be able to set home - if(!status.condition_global_position_valid) { + if (!status.condition_global_position_valid) { return; } //Ensure that the GPS accuracy is good enough for intializing home - if(globalPosition.eph > eph_threshold || globalPosition.epv > epv_threshold) { + if (globalPosition.eph > eph_threshold || globalPosition.epv > epv_threshold) { return; } @@ -760,7 +762,7 @@ static void commander_set_home_position(orb_advert_t &homePub, home_position_s & } //Play tune first time we initialize HOME - if(!status.condition_home_position_valid) { + if (!status.condition_home_position_valid) { tune_positive(true); } @@ -1147,8 +1149,8 @@ int commander_thread_main(int argc, char *argv[]) } /* set vehicle_status.is_vtol flag */ - status.is_vtol = (status.system_type == VEHICLE_TYPE_VTOL_DUOROTOR) || - (status.system_type == VEHICLE_TYPE_VTOL_QUADROTOR); + status.is_vtol = (status.system_type == VEHICLE_TYPE_VTOL_DUOROTOR) || + (status.system_type == VEHICLE_TYPE_VTOL_QUADROTOR); /* check and update system / component ID */ param_get(_param_system_id, &(status.system_id)); @@ -1310,12 +1312,12 @@ int commander_thread_main(int argc, char *argv[]) } //Notify the user if the status of the safety switch changes - if(safety.safety_switch_available && previous_safety_off != safety.safety_off) { + if (safety.safety_switch_available && previous_safety_off != safety.safety_off) { - if(safety.safety_off) { + if (safety.safety_off) { set_tune(TONE_NOTIFY_POSITIVE_TUNE); - } - else { + + } else { tune_neutral(true); } @@ -1330,6 +1332,7 @@ int commander_thread_main(int argc, char *argv[]) /* vtol status changed */ orb_copy(ORB_ID(vtol_vehicle_status), vtol_vehicle_status_sub, &vtol_status); status.vtol_fw_permanent_stab = vtol_status.fw_permanent_stab; + /* 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; @@ -1632,7 +1635,8 @@ int commander_thread_main(int argc, char *argv[]) } else { if (status.rc_signal_lost) { - mavlink_log_critical(mavlink_fd, "RC SIGNAL REGAINED after %llums",(hrt_absolute_time()-status.rc_signal_lost_timestamp)/1000); + mavlink_log_critical(mavlink_fd, "RC SIGNAL REGAINED after %llums", + (hrt_absolute_time() - status.rc_signal_lost_timestamp) / 1000); status_changed = true; } } @@ -1733,9 +1737,9 @@ int commander_thread_main(int argc, char *argv[]) } else { if (!status.rc_signal_lost) { - mavlink_log_critical(mavlink_fd, "RC SIGNAL LOST (at t=%llums)",hrt_absolute_time()/1000); + 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.rc_signal_lost_timestamp = sp_man.timestamp; status_changed = true; } } @@ -1886,13 +1890,13 @@ int commander_thread_main(int argc, char *argv[]) const hrt_abstime now = hrt_absolute_time(); //First time home position update - if(!status.condition_home_position_valid) { - commander_set_home_position(home_pub, home, local_position, global_position); + if (!status.condition_home_position_valid) { + commander_set_home_position(home_pub, home, local_position, global_position); } /* update home position on arming if at least 2s from commander start spent to avoid setting home on in-air restart */ - else if(arming_state_changed && armed.armed && !was_armed && now > commander_boot_timestamp + 2000000) { - commander_set_home_position(home_pub, home, local_position, global_position); + else if (arming_state_changed && armed.armed && !was_armed && now > commander_boot_timestamp + 2000000) { + commander_set_home_position(home_pub, home, local_position, global_position); } /* print new state */ @@ -1919,11 +1923,14 @@ int commander_thread_main(int argc, char *argv[]) if (status.failsafe != failsafe_old) { status_changed = true; + if (status.failsafe) { mavlink_log_critical(mavlink_fd, "failsafe mode on"); + } else { mavlink_log_critical(mavlink_fd, "failsafe mode off"); } + failsafe_old = status.failsafe; } @@ -1969,7 +1976,7 @@ int commander_thread_main(int argc, char *argv[]) if (!armed.armed || (safety.safety_switch_available && !safety.safety_off)) { //Notify the user that it is safe to approach the vehicle - if(arm_tune_played) { + if (arm_tune_played) { tune_neutral(true); } @@ -2436,6 +2443,7 @@ set_control_mode() control_mode.flag_control_position_enabled = false; control_mode.flag_control_velocity_enabled = false; } + break; default: @@ -2474,7 +2482,7 @@ void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT resul { switch (result) { case VEHICLE_CMD_RESULT_ACCEPTED: - tune_positive(true); + tune_positive(true); break; case VEHICLE_CMD_RESULT_DENIED: From e18e5ca5b5d82a140d9a525cc3ba9bfddf4dcb7f Mon Sep 17 00:00:00 2001 From: hauptmech Date: Thu, 8 Jan 2015 12:38:33 +1300 Subject: [PATCH 508/652] Add SENS_MAG_ID parameter Record devid to SENS_MAG_ID during mag calibration Verify devid matches SENS_MAG_ID during preflight_check --- src/drivers/drv_mag.h | 2 ++ src/modules/commander/mag_calibration.cpp | 7 +++++++ src/modules/sensors/sensor_params.c | 6 ++++++ src/systemcmds/config/config.c | 6 +++++- src/systemcmds/preflight_check/preflight_check.c | 11 +++++++++++ 5 files changed, 31 insertions(+), 1 deletion(-) diff --git a/src/drivers/drv_mag.h b/src/drivers/drv_mag.h index 5ddf5d08e8..d341e89472 100644 --- a/src/drivers/drv_mag.h +++ b/src/drivers/drv_mag.h @@ -41,9 +41,11 @@ #include #include +#include "drv_device.h" #include "drv_sensor.h" #include "drv_orb_dev.h" + #define MAG_DEVICE_PATH "/dev/mag" /** diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 7be8de9c6b..9d2abd3ceb 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -65,6 +65,7 @@ static const char *sensor_name = "mag"; int do_mag_calibration(int mavlink_fd) { + int32_t device_id; mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name); mavlink_log_info(mavlink_fd, "don't move system"); @@ -88,6 +89,9 @@ int do_mag_calibration(int mavlink_fd) /* erase old calibration */ int fd = open(MAG_DEVICE_PATH, O_RDONLY); + + device_id = ioctl(fd, DEVIOCGDEVICEID, 0); + res = ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null); if (res != OK) { @@ -253,6 +257,9 @@ int do_mag_calibration(int mavlink_fd) if (res == OK) { /* set parameters */ + if (param_set(param_find("SENS_MAG_ID"), &(device_id))) { + res = ERROR; + } if (param_set(param_find("SENS_MAG_XOFF"), &(mscale.x_offset))) { res = ERROR; } diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index a065541b91..bf5708e0b7 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -98,6 +98,12 @@ PARAM_DEFINE_FLOAT(SENS_GYRO_YSCALE, 1.0f); */ PARAM_DEFINE_FLOAT(SENS_GYRO_ZSCALE, 1.0f); +/** + * ID of Magnetometer the calibration is for. + * + * @group Sensor Calibration + */ +PARAM_DEFINE_INT32(SENS_MAG_ID, 0); /** * Magnetometer X-axis offset diff --git a/src/systemcmds/config/config.c b/src/systemcmds/config/config.c index 4a97d328cf..077bc47c90 100644 --- a/src/systemcmds/config/config.c +++ b/src/systemcmds/config/config.c @@ -58,6 +58,7 @@ #include "systemlib/systemlib.h" #include "systemlib/err.h" +#include "systemlib/param/param.h" __EXPORT int config_main(int argc, char *argv[]); @@ -264,8 +265,11 @@ do_mag(int argc, char *argv[]) int srate = ioctl(fd, MAGIOCGSAMPLERATE, 0); int prate = ioctl(fd, SENSORIOCGPOLLRATE, 0); int range = ioctl(fd, MAGIOCGRANGE, 0); + int id = ioctl(fd, DEVIOCGDEVICEID,0); + int32_t calibration_id = 0; + param_get(param_find("SENS_MAG_ID"), &(calibration_id)); - warnx("mag: \n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d Ga", srate, prate, range); + warnx("mag: \n\tdevice id:\t%x\t(calibration is for device id %x)\n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d Ga", id, calibration_id, srate, prate, range); close(fd); } diff --git a/src/systemcmds/preflight_check/preflight_check.c b/src/systemcmds/preflight_check/preflight_check.c index 86e4ff545d..bbd90b961f 100644 --- a/src/systemcmds/preflight_check/preflight_check.c +++ b/src/systemcmds/preflight_check/preflight_check.c @@ -84,6 +84,7 @@ int preflight_check_main(int argc, char *argv[]) /* open text message output path */ int mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); int ret; + int32_t mag_devid,mag_calibration_devid; /* give the system some time to sample the sensors in the background */ usleep(150000); @@ -96,6 +97,16 @@ int preflight_check_main(int argc, char *argv[]) system_ok = false; goto system_eval; } + + mag_devid = ioctl(fd, DEVIOCGDEVICEID,0); + param_get(param_find("SENS_MAG_ID"), &(mag_calibration_devid)); + if (mag_devid != mag_calibration_devid){ + warnx("magnetometer calibration is for a different device - calibrate magnetometer first"); + mavlink_log_critical(mavlink_fd, "SENSOR FAIL: MAG CAL ID"); + system_ok = false; + goto system_eval; + } + ret = ioctl(fd, MAGIOCSELFTEST, 0); if (ret != OK) { From 6324513b91702d8fa877e915c91a6dcef9790faa Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 17 Jan 2015 18:48:41 +0100 Subject: [PATCH 509/652] MS5611: Comment-only fix --- src/drivers/ms5611/ms5611.h | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/drivers/ms5611/ms5611.h b/src/drivers/ms5611/ms5611.h index f0b3fd61d7..3f1f6c4738 100644 --- a/src/drivers/ms5611/ms5611.h +++ b/src/drivers/ms5611/ms5611.h @@ -37,12 +37,12 @@ * Shared defines for the ms5611 driver. */ -#define ADDR_RESET_CMD 0x1E /* write to this address to reset chip */ -#define ADDR_CMD_CONVERT_D1 0x48 /* write to this address to start temperature conversion */ -#define ADDR_CMD_CONVERT_D2 0x58 /* write to this address to start pressure conversion */ -#define ADDR_DATA 0x00 /* address of 3 bytes / 32bit pressure data */ -#define ADDR_PROM_SETUP 0xA0 /* address of 8x 2 bytes factory and calibration data */ -#define ADDR_PROM_C1 0xA2 /* address of 6x 2 bytes calibration data */ +#define ADDR_RESET_CMD 0x1E /* write to this address to reset chip */ +#define ADDR_CMD_CONVERT_D1 0x48 /* write to this address to start pressure conversion */ +#define ADDR_CMD_CONVERT_D2 0x58 /* write to this address to start temperature conversion */ +#define ADDR_DATA 0x00 /* address of 3 bytes / 32bit pressure data */ +#define ADDR_PROM_SETUP 0xA0 /* address of 8x 2 bytes factory and calibration data */ +#define ADDR_PROM_C1 0xA2 /* address of 6x 2 bytes calibration data */ /* interface ioctls */ #define IOCTL_RESET 2 From b1127315b453e129753e1f59aff0b0f6906cbaac Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 17 Jan 2015 23:26:43 +0100 Subject: [PATCH 510/652] Fixed land detector param names --- .../land_detector/FixedwingLandDetector.cpp | 6 +-- src/modules/land_detector/LandDetector.cpp | 41 +++++++++++++++++++ .../land_detector/MulticopterLandDetector.cpp | 10 ++--- .../land_detector/land_detector_main.cpp | 2 +- .../land_detector/land_detector_params.c | 17 ++++---- 5 files changed, 58 insertions(+), 18 deletions(-) diff --git a/src/modules/land_detector/FixedwingLandDetector.cpp b/src/modules/land_detector/FixedwingLandDetector.cpp index 52084e4c05..74a197bd23 100644 --- a/src/modules/land_detector/FixedwingLandDetector.cpp +++ b/src/modules/land_detector/FixedwingLandDetector.cpp @@ -57,9 +57,9 @@ FixedwingLandDetector::FixedwingLandDetector() : LandDetector(), _airspeed_filtered(0.0f), _landDetectTrigger(0) { - _paramHandle.maxVelocity = param_find("LAND_FW_VEL_XY_MAX"); - _paramHandle.maxClimbRate = param_find("LAND_FW_VEL_Z_MAX"); - _paramHandle.maxAirSpeed = param_find("LAND_FW_AIRSPEED_MAX"); + _paramHandle.maxVelocity = param_find("LNDFW_VEL_XY_MAX"); + _paramHandle.maxClimbRate = param_find("LNDFW_VEL_Z_MAX"); + _paramHandle.maxAirSpeed = param_find("LNDFW_AIRSPD_MAX"); } void FixedwingLandDetector::initialize() diff --git a/src/modules/land_detector/LandDetector.cpp b/src/modules/land_detector/LandDetector.cpp index 1dd09b6a67..61e678b41c 100644 --- a/src/modules/land_detector/LandDetector.cpp +++ b/src/modules/land_detector/LandDetector.cpp @@ -1,3 +1,44 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file LandDetector.cpp + * Land detection algorithm + * + * @author Johan Jansen + * @author Morten Lysgaard + */ + #include "LandDetector.h" #include //usleep #include diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp index 5371346181..cc984f11fa 100644 --- a/src/modules/land_detector/MulticopterLandDetector.cpp +++ b/src/modules/land_detector/MulticopterLandDetector.cpp @@ -33,7 +33,7 @@ /** * @file MulticopterLandDetector.cpp - * Land detection algorithm + * Land detection algorithm for multicopters * * @author Johan Jansen * @author Morten Lysgaard @@ -62,10 +62,10 @@ MulticopterLandDetector::MulticopterLandDetector() : LandDetector(), _landTimer(0) { - _paramHandle.maxRotation = param_find("LAND_MC_Z_VEL_MAX"); - _paramHandle.maxVelocity = param_find("LAND_MC_XY_VEL_MAX"); - _paramHandle.maxClimbRate = param_find("LAND_MC_ROT_MAX"); - _paramHandle.maxThrottle = param_find("LAND_MC_THR_MAX"); + _paramHandle.maxRotation = param_find("LNDMC_Z_VEL_MAX"); + _paramHandle.maxVelocity = param_find("LNDMC_XY_VEL_MAX"); + _paramHandle.maxClimbRate = param_find("LNDMC_ROT_MAX"); + _paramHandle.maxThrottle = param_find("LNDMC_THR_MAX"); } void MulticopterLandDetector::initialize() diff --git a/src/modules/land_detector/land_detector_main.cpp b/src/modules/land_detector/land_detector_main.cpp index 9e135b5f17..1e43e7ad5d 100644 --- a/src/modules/land_detector/land_detector_main.cpp +++ b/src/modules/land_detector/land_detector_main.cpp @@ -139,7 +139,7 @@ static int land_detector_start(const char *mode) _landDetectorTaskID = task_spawn_cmd("land_detector", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, - 1024, + 1200, (main_t)&land_detector_deamon_thread, nullptr); diff --git a/src/modules/land_detector/land_detector_params.c b/src/modules/land_detector/land_detector_params.c index e2acf42b3d..0302bc7c14 100644 --- a/src/modules/land_detector/land_detector_params.c +++ b/src/modules/land_detector/land_detector_params.c @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: @author Anton Babushkin + * Copyright (c) 2014, 2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -48,7 +47,7 @@ * * @group Land Detector */ -PARAM_DEFINE_FLOAT(LAND_MC_Z_VEL_MAX, 0.30f); +PARAM_DEFINE_FLOAT(LNDMC_Z_VEL_MAX, 0.30f); /** * Multicopter max horizontal velocity @@ -57,7 +56,7 @@ PARAM_DEFINE_FLOAT(LAND_MC_Z_VEL_MAX, 0.30f); * * @group Land Detector */ -PARAM_DEFINE_FLOAT(LAND_MC_XY_VEL_MAX, 1.00f); +PARAM_DEFINE_FLOAT(LNDMC_XY_VEL_MAX, 1.00f); /** * Multicopter max rotation @@ -66,7 +65,7 @@ PARAM_DEFINE_FLOAT(LAND_MC_XY_VEL_MAX, 1.00f); * * @group Land Detector */ -PARAM_DEFINE_FLOAT(LAND_MC_ROT_MAX, 0.20f); +PARAM_DEFINE_FLOAT(LNDMC_ROT_MAX, 0.20f); /** * Multicopter max throttle @@ -75,7 +74,7 @@ PARAM_DEFINE_FLOAT(LAND_MC_ROT_MAX, 0.20f); * * @group Land Detector */ -PARAM_DEFINE_FLOAT(LAND_MC_THR_MAX, 0.20f); +PARAM_DEFINE_FLOAT(LNDMC_THR_MAX, 0.20f); /** * Fixedwing max horizontal velocity @@ -84,7 +83,7 @@ PARAM_DEFINE_FLOAT(LAND_MC_THR_MAX, 0.20f); * * @group Land Detector */ -PARAM_DEFINE_FLOAT(LAND_FW_VEL_XY_MAX, 0.20f); +PARAM_DEFINE_FLOAT(LNDFW_VEL_XY_MAX, 0.20f); /** * Fixedwing max climb rate @@ -93,7 +92,7 @@ PARAM_DEFINE_FLOAT(LAND_FW_VEL_XY_MAX, 0.20f); * * @group Land Detector */ -PARAM_DEFINE_FLOAT(LAND_FW_VEL_Z_MAX, 10.00f); +PARAM_DEFINE_FLOAT(LNDFW_VEL_Z_MAX, 10.00f); /** * Airspeed max @@ -102,4 +101,4 @@ PARAM_DEFINE_FLOAT(LAND_FW_VEL_Z_MAX, 10.00f); * * @group Land Detector */ -PARAM_DEFINE_FLOAT(LAND_FW_AIRSPEED_MAX, 10.00f); +PARAM_DEFINE_FLOAT(LNDFW_AIRSPD_MAX, 10.00f); From 378dcc53d63a18811f17ab6f60c66b8315d8db25 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 17 Jan 2015 23:32:00 +0100 Subject: [PATCH 511/652] Code style: minor comment styling --- .../land_detector/FixedwingLandDetector.cpp | 13 ++++++------ src/modules/land_detector/LandDetector.cpp | 18 ++++++++--------- .../land_detector/MulticopterLandDetector.cpp | 20 +++++++++---------- 3 files changed, 24 insertions(+), 27 deletions(-) diff --git a/src/modules/land_detector/FixedwingLandDetector.cpp b/src/modules/land_detector/FixedwingLandDetector.cpp index 74a197bd23..8e5bcf7bae 100644 --- a/src/modules/land_detector/FixedwingLandDetector.cpp +++ b/src/modules/land_detector/FixedwingLandDetector.cpp @@ -51,7 +51,6 @@ FixedwingLandDetector::FixedwingLandDetector() : LandDetector(), _airspeedSub(-1), _airspeed({}), _parameterSub(-1), - _velocity_xy_filtered(0.0f), _velocity_z_filtered(0.0f), _airspeed_filtered(0.0f), @@ -64,7 +63,7 @@ FixedwingLandDetector::FixedwingLandDetector() : LandDetector(), void FixedwingLandDetector::initialize() { - //Subscribe to local position and airspeed data + // Subscribe to local position and airspeed data _vehicleLocalPositionSub = orb_subscribe(ORB_ID(vehicle_local_position)); _airspeedSub = orb_subscribe(ORB_ID(airspeed)); } @@ -77,30 +76,30 @@ void FixedwingLandDetector::updateSubscriptions() bool FixedwingLandDetector::update() { - //First poll for new data from our subscriptions + // First poll for new data from our subscriptions updateSubscriptions(); const uint64_t now = hrt_absolute_time(); bool landDetected = false; - //TODO: reset filtered values on arming? + // TODO: reset filtered values on arming? _velocity_xy_filtered = 0.95f * _velocity_xy_filtered + 0.05f * sqrtf(_vehicleLocalPosition.vx * _vehicleLocalPosition.vx + _vehicleLocalPosition.vy * _vehicleLocalPosition.vy); _velocity_z_filtered = 0.95f * _velocity_z_filtered + 0.05f * fabsf(_vehicleLocalPosition.vz); _airspeed_filtered = 0.95f * _airspeed_filtered + 0.05f * _airspeed.true_airspeed_m_s; - /* crude land detector for fixedwing */ + // crude land detector for fixedwing if (_velocity_xy_filtered < _params.maxVelocity && _velocity_z_filtered < _params.maxClimbRate && _airspeed_filtered < _params.maxAirSpeed) { - //These conditions need to be stable for a period of time before we trust them + // these conditions need to be stable for a period of time before we trust them if (now > _landDetectTrigger) { landDetected = true; } } else { - //reset land detect trigger + // reset land detect trigger _landDetectTrigger = now + LAND_DETECTOR_TRIGGER_TIME; } diff --git a/src/modules/land_detector/LandDetector.cpp b/src/modules/land_detector/LandDetector.cpp index 61e678b41c..a4fbb9861f 100644 --- a/src/modules/land_detector/LandDetector.cpp +++ b/src/modules/land_detector/LandDetector.cpp @@ -49,7 +49,7 @@ LandDetector::LandDetector() : _taskShouldExit(false), _taskIsRunning(false) { - //ctor + // ctor } LandDetector::~LandDetector() @@ -65,20 +65,20 @@ void LandDetector::shutdown() void LandDetector::start() { - //Make sure this method has not already been called by another thread + // make sure this method has not already been called by another thread if (isRunning()) { return; } - //Advertise the first land detected uORB + // advertise the first land detected uORB _landDetected.timestamp = hrt_absolute_time(); _landDetected.landed = false; _landDetectedPub = orb_advertise(ORB_ID(vehicle_land_detected), &_landDetected); - //Initialize land detection algorithm + // initialize land detection algorithm initialize(); - //Task is now running, keep doing so until shutdown() has been called + // task is now running, keep doing so until shutdown() has been called _taskIsRunning = true; _taskShouldExit = false; @@ -86,16 +86,16 @@ void LandDetector::start() bool landDetected = update(); - //Publish if land detection state has changed + // publish if land detection state has changed if (_landDetected.landed != landDetected) { _landDetected.timestamp = hrt_absolute_time(); _landDetected.landed = landDetected; - /* publish the land detected broadcast */ + // publish the land detected broadcast orb_publish(ORB_ID(vehicle_land_detected), _landDetectedPub, &_landDetected); } - //Limit loop rate + // limit loop rate usleep(1000000 / LAND_DETECTOR_UPDATE_RATE); } @@ -107,7 +107,7 @@ bool LandDetector::orb_update(const struct orb_metadata *meta, int handle, void { bool newData = false; - //Check if there is new data to grab + // check if there is new data to grab if (orb_check(handle, &newData) != OK) { return false; } diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp index cc984f11fa..277cb9363f 100644 --- a/src/modules/land_detector/MulticopterLandDetector.cpp +++ b/src/modules/land_detector/MulticopterLandDetector.cpp @@ -53,13 +53,11 @@ MulticopterLandDetector::MulticopterLandDetector() : LandDetector(), _actuatorsSub(-1), _armingSub(-1), _parameterSub(-1), - _vehicleGlobalPosition({}), _sensors({}), _waypoint({}), _actuators({}), _arming({}), - _landTimer(0) { _paramHandle.maxRotation = param_find("LNDMC_Z_VEL_MAX"); @@ -70,7 +68,7 @@ MulticopterLandDetector::MulticopterLandDetector() : LandDetector(), void MulticopterLandDetector::initialize() { - //Subscribe to position, attitude, arming and velocity changes + // subscribe to position, attitude, arming and velocity changes _vehicleGlobalPositionSub = orb_subscribe(ORB_ID(vehicle_global_position)); _sensorsCombinedSub = orb_subscribe(ORB_ID(sensor_combined)); _waypointSub = orb_subscribe(ORB_ID(position_setpoint_triplet)); @@ -78,7 +76,7 @@ void MulticopterLandDetector::initialize() _armingSub = orb_subscribe(ORB_ID(actuator_armed)); _parameterSub = orb_subscribe(ORB_ID(parameter_update)); - //Download parameters + // download parameters updateParameterCache(true); } @@ -93,33 +91,33 @@ void MulticopterLandDetector::updateSubscriptions() bool MulticopterLandDetector::update() { - //First poll for new data from our subscriptions + // first poll for new data from our subscriptions updateSubscriptions(); - //Only trigger flight conditions if we are armed + // only trigger flight conditions if we are armed if (!_arming.armed) { return true; } const uint64_t now = hrt_absolute_time(); - //Check if we are moving vertically + // check if we are moving vertically bool verticalMovement = fabsf(_vehicleGlobalPosition.vel_d) > _params.maxClimbRate; - //Check if we are moving horizontally + // check if we are moving horizontally bool horizontalMovement = sqrtf(_vehicleGlobalPosition.vel_n * _vehicleGlobalPosition.vel_n + _vehicleGlobalPosition.vel_e * _vehicleGlobalPosition.vel_e) > _params.maxVelocity; - //Next look if all rotation angles are not moving + // next look if all rotation angles are not moving bool rotating = sqrtf(_sensors.gyro_rad_s[0] * _sensors.gyro_rad_s[0] + _sensors.gyro_rad_s[1] * _sensors.gyro_rad_s[1] + _sensors.gyro_rad_s[2] * _sensors.gyro_rad_s[2]) > _params.maxRotation; - //Check if thrust output is minimal (about half of default) + // check if thrust output is minimal (about half of default) bool minimalThrust = _actuators.control[3] <= _params.maxThrottle; if (verticalMovement || rotating || !minimalThrust || horizontalMovement) { - //Sensed movement, so reset the land detector + // sensed movement, so reset the land detector _landTimer = now; return false; } From beab89367f5cc2765c747fb463a27ce001206dd9 Mon Sep 17 00:00:00 2001 From: tumbili Date: Sun, 18 Jan 2015 17:46:01 +0100 Subject: [PATCH 512/652] calculate total airflow over elevons using physical of flow behind propeller. read local position topic for future use. --- src/modules/uORB/topics/vtol_vehicle_status.h | 1 + .../vtol_att_control_main.cpp | 104 ++++++++++++++++-- .../vtol_att_control_params.c | 35 +++++- 3 files changed, 129 insertions(+), 11 deletions(-) diff --git a/src/modules/uORB/topics/vtol_vehicle_status.h b/src/modules/uORB/topics/vtol_vehicle_status.h index 7b4e22bc88..968c2b6bdf 100644 --- a/src/modules/uORB/topics/vtol_vehicle_status.h +++ b/src/modules/uORB/topics/vtol_vehicle_status.h @@ -55,6 +55,7 @@ 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 */ bool fw_permanent_stab; /**< In fw mode stabilize attitude even if in manual mode*/ + float airspeed_tot; /*< Estimated airspeed over control surfaces */ }; /** 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 c72a85ecd4..8e68730b8b 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -65,6 +65,8 @@ #include #include #include +#include +#include #include #include #include @@ -105,7 +107,9 @@ private: int _params_sub; //parameter updates subscription int _manual_control_sp_sub; //manual control setpoint subscription int _armed_sub; //arming status subscription + int _local_pos_sub; // sensor subscription int _airspeed_sub; // airspeed subscription + int _battery_status_sub; // battery 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 @@ -129,7 +133,9 @@ 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 vehicle_local_position_s _local_pos; struct airspeed_s _airspeed; // airspeed + struct battery_status_s _batt_status; // battery status struct { param_t idle_pwm_mc; //pwm value for idle in mc mode @@ -139,6 +145,9 @@ private: float mc_airspeed_trim; // trim airspeed in multicopter mode float mc_airspeed_max; // max airpseed in multicopter mode float fw_pitch_trim; // trim for neutral elevon position in fw mode + float power_max; // maximum power of one engine + float prop_eff; // factor to calculate prop efficiency + float arsp_lp_gain; // total airspeed estimate low pass gain } _params; struct { @@ -149,6 +158,9 @@ private: param_t mc_airspeed_trim; param_t mc_airspeed_max; param_t fw_pitch_trim; + param_t power_max; + param_t prop_eff; + param_t arsp_lp_gain; } _params_handles; perf_counter_t _loop_perf; /**< loop performance counter */ @@ -159,6 +171,7 @@ private: * 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 + float _airspeed_tot; //*****************Member functions*********************************************************************** @@ -172,7 +185,9 @@ 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_local_pos_poll(); // Check for changes in sensor values void vehicle_airspeed_poll(); // Check for changes in airspeed + void vehicle_battery_poll(); // Check for battery updates 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 @@ -182,6 +197,7 @@ private: void set_idle_fw(); void set_idle_mc(); void scale_mc_output(); + void calc_tot_airspeed(); // estimated airspeed seen by elevons }; namespace VTOL_att_control @@ -205,7 +221,9 @@ VtolAttitudeControl::VtolAttitudeControl() : _params_sub(-1), _manual_control_sp_sub(-1), _armed_sub(-1), + _local_pos_sub(-1), _airspeed_sub(-1), + _battery_status_sub(-1), //init publication handlers _actuators_0_pub(-1), @@ -218,6 +236,7 @@ VtolAttitudeControl::VtolAttitudeControl() : { flag_idle_mc = true; + _airspeed_tot = 0.0f; memset(& _vtol_vehicle_status, 0, sizeof(_vtol_vehicle_status)); _vtol_vehicle_status.vtol_in_rw_mode = true; /* start vtol in rotary wing mode*/ @@ -234,7 +253,9 @@ 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(&_local_pos,0,sizeof(_local_pos)); memset(&_airspeed,0,sizeof(_airspeed)); + memset(&_batt_status,0,sizeof(_batt_status)); _params.idle_pwm_mc = PWM_LOWEST_MIN; _params.vtol_motor_count = 0; @@ -247,6 +268,9 @@ VtolAttitudeControl::VtolAttitudeControl() : _params_handles.mc_airspeed_max = param_find("VT_MC_ARSPD_MAX"); _params_handles.mc_airspeed_trim = param_find("VT_MC_ARSPD_TRIM"); _params_handles.fw_pitch_trim = param_find("VT_FW_PITCH_TRIM"); + _params_handles.power_max = param_find("VT_POWER_MAX"); + _params_handles.prop_eff = param_find("VT_PROP_EFF"); + _params_handles.arsp_lp_gain = param_find("VT_ARSP_LP_GAIN"); /* fetch initial parameter values */ parameters_update(); @@ -387,6 +411,19 @@ VtolAttitudeControl::vehicle_airspeed_poll() { } } +/** +* Check for battery updates. +*/ +void +VtolAttitudeControl::vehicle_battery_poll() { + bool updated; + orb_check(_battery_status_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(battery_status), _battery_status_sub , &_batt_status); + } +} + /** * Check for parameter updates. */ @@ -405,6 +442,22 @@ VtolAttitudeControl::parameters_update_poll() } } +/** +* Check for sensor updates. +*/ +void +VtolAttitudeControl::vehicle_local_pos_poll() +{ + bool updated; + /* Check if parameters have changed */ + orb_check(_local_pos_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(vehicle_local_position), _local_pos_sub , &_local_pos); + } + +} + /** * Update parameters. */ @@ -437,6 +490,18 @@ VtolAttitudeControl::parameters_update() param_get(_params_handles.fw_pitch_trim, &v); _params.fw_pitch_trim = v; + /* vtol maximum power engine can produce */ + param_get(_params_handles.power_max, &v); + _params.power_max = v; + + /* vtol propeller efficiency factor */ + param_get(_params_handles.prop_eff, &v); + _params.prop_eff = v; + + /* vtol total airspeed estimate low pass gain */ + param_get(_params_handles.arsp_lp_gain, &v); + _params.arsp_lp_gain = v; + return OK; } @@ -555,7 +620,7 @@ void VtolAttitudeControl::scale_mc_output() { // scale around tuning airspeed float airspeed; - + calc_tot_airspeed(); // estimate air velocity seen by elevons // 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) { @@ -563,16 +628,12 @@ VtolAttitudeControl::scale_mc_output() { 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); - } - - // 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; + } else { + airspeed = _airspeed_tot; + airspeed = math::constrain(airspeed,_params.mc_airspeed_min, _params.mc_airspeed_max); } + _vtol_vehicle_status.airspeed_tot = airspeed; // save value for logging /* * 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 @@ -584,6 +645,23 @@ VtolAttitudeControl::scale_mc_output() { _actuators_mc_in.control[1] = math::constrain(_actuators_mc_in.control[1]*airspeed_scaling*airspeed_scaling,-1.0f,1.0f); } +void VtolAttitudeControl::calc_tot_airspeed() { + float airspeed = math::max(1.0f, _airspeed.true_airspeed_m_s); // prevent numerical drama + // calculate momentary power of one engine + float P = _batt_status.voltage_filtered_v * _batt_status.current_a / _params.vtol_motor_count; + P = math::constrain(P,1.0f,_params.power_max); + // calculate prop efficiency + float power_factor = 1.0f - P*_params.prop_eff/_params.power_max; + float eta = (1.0f/(1 + expf(-0.4f * power_factor * airspeed)) - 0.5f)*2.0f; + eta = math::constrain(eta,0.001f,1.0f); // live on the safe side + // calculate induced airspeed by propeller + float v_ind = (airspeed/eta - airspeed)*2.0f; + // calculate total airspeed + float airspeed_raw = airspeed + v_ind; + // apply low-pass filter + _airspeed_tot = _params.arsp_lp_gain * (_airspeed_tot - airspeed_raw) + airspeed_raw; +} + void VtolAttitudeControl::task_main_trampoline(int argc, char *argv[]) { @@ -604,7 +682,9 @@ 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)); + _local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); + _battery_status_sub = orb_subscribe(ORB_ID(battery_status)); _actuator_inputs_mc = orb_subscribe(ORB_ID(actuator_controls_virtual_mc)); _actuator_inputs_fw = orb_subscribe(ORB_ID(actuator_controls_virtual_fw)); @@ -674,7 +754,10 @@ void VtolAttitudeControl::task_main() vehicle_rates_sp_mc_poll(); vehicle_rates_sp_fw_poll(); parameters_update_poll(); + vehicle_local_pos_poll(); // Check for new sensor values vehicle_airspeed_poll(); + vehicle_battery_poll(); + if (_manual_control_sp.aux1 <= 0.0f) { /* vehicle is in mc mode */ _vtol_vehicle_status.vtol_in_rw_mode = true; @@ -688,7 +771,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 pitch control with total airspeed scale_mc_output(); fill_mc_att_control_output(); 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 d1d4697f3f..33752b2c43 100644 --- a/src/modules/vtol_att_control/vtol_att_control_params.c +++ b/src/modules/vtol_att_control/vtol_att_control_params.c @@ -64,7 +64,7 @@ PARAM_DEFINE_INT32(VT_IDLE_PWM_MC,900); * @min 0.0 * @group VTOL Attitude Control */ -PARAM_DEFINE_FLOAT(VT_MC_ARSPD_MIN,2.0f); +PARAM_DEFINE_FLOAT(VT_MC_ARSPD_MIN,10.0f); /** * Maximum airspeed in multicopter mode @@ -109,3 +109,36 @@ PARAM_DEFINE_INT32(VT_FW_PERM_STAB,0); */ PARAM_DEFINE_FLOAT(VT_FW_PITCH_TRIM,0.0f); +/** + * Motor max power + * + * Indicates the maximum power the motor is able to produce. Used to calculate + * propeller efficiency map. + * + * @min 1 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VT_POWER_MAX,120.0f); + +/** + * Propeller efficiency parameter + * + * Influences propeller efficiency at different power settings. Should be tuned beforehand. + * + * @min 0.5 + * @max 0.9 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VT_PROP_EFF,0.0f); + +/** + * Total airspeed estimate low-pass filter gain + * + * Gain for tuning the low-pass filter for the total airspeed estimate + * + * @min 0.0 + * @max 0.99 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VT_ARSP_LP_GAIN,0.3f); + From 3a05b571690ce675f56184cd5c5168f7699f9a03 Mon Sep 17 00:00:00 2001 From: tumbili Date: Sun, 18 Jan 2015 17:46:25 +0100 Subject: [PATCH 513/652] log total airspeed for vtol --- src/modules/sdlog2/sdlog2.c | 12 ++++++++++++ src/modules/sdlog2/sdlog2_messages.h | 6 ++++++ 2 files changed, 18 insertions(+) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 7b7949239e..0cc193b6c1 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -94,6 +94,7 @@ #include #include #include +#include #include #include @@ -1000,6 +1001,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct satellite_info_s sat_info; struct wind_estimate_s wind_estimate; struct encoders_s encoders; + struct vtol_vehicle_status_s vtol_status; } buf; memset(&buf, 0, sizeof(buf)); @@ -1019,6 +1021,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_GPS_s log_GPS; struct log_ATTC_s log_ATTC; struct log_STAT_s log_STAT; + struct log_VTOL_s log_VTOL; struct log_RC_s log_RC; struct log_OUT0_s log_OUT0; struct log_AIRS_s log_AIRS; @@ -1053,6 +1056,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct { int cmd_sub; int status_sub; + int vtol_status_sub; int sensor_sub; int att_sub; int att_sp_sub; @@ -1086,6 +1090,7 @@ int sdlog2_thread_main(int argc, char *argv[]) subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); subs.status_sub = orb_subscribe(ORB_ID(vehicle_status)); + subs.vtol_status_sub = orb_subscribe(ORB_ID(vtol_vehicle_status)); subs.gps_pos_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); subs.sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); subs.att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); @@ -1219,6 +1224,13 @@ int sdlog2_thread_main(int argc, char *argv[]) LOGBUFFER_WRITE_AND_COUNT(STAT); } + /* --- VTOL VEHICLE STATUS --- */ + if(copy_if_updated(ORB_ID(vtol_vehicle_status), subs.vtol_status_sub, &buf.vtol_status)) { + log_msg.msg_type = LOG_VTOL_MSG; + log_msg.body.log_VTOL.airspeed_tot = buf.vtol_status.airspeed_tot; + LOGBUFFER_WRITE_AND_COUNT(VTOL); + } + /* --- GPS POSITION - UNIT #1 --- */ if (gps_pos_updated) { diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 99f70a948e..c9221d58a6 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -426,6 +426,11 @@ struct log_ENCD_s { /* --- AIR SPEED SENSORS - DIFF. PRESSURE --- */ #define LOG_AIR1_MSG 40 +/* --- VTOL - VTOL VEHICLE STATUS */ +#define LOG_VTOL_MSG 42 +struct log_VTOL_s { + float airspeed_tot; +}; /********** SYSTEM MESSAGES, ID > 0x80 **********/ @@ -468,6 +473,7 @@ static const struct log_format_s log_formats[] = { 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(VTOL, "f", "Arsp"), LOG_FORMAT(RC, "ffffffffBB", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,Count,SignalLost"), LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"), LOG_FORMAT(AIRS, "fff", "IndSpeed,TrueSpeed,AirTemp"), From b85f0ab3cddeb93e92064de92a2065297c34b28c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Jan 2015 13:16:44 +0100 Subject: [PATCH 514/652] README.md: Add coverity logo --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 80f5a585fe..db125e06d4 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,6 @@ ## PX4 Flight Control Stack and Middleware ## -[![Build Status](https://travis-ci.org/PX4/Firmware.svg?branch=master)](https://travis-ci.org/PX4/Firmware) +[![Build Status](https://travis-ci.org/PX4/Firmware.svg?branch=master)](https://travis-ci.org/PX4/Firmware) [![Coverity Scan](https://scan.coverity.com/projects/3966/badge.svg?flat=1)](https://scan.coverity.com/projects/3966?tab=overview) [![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 7d13318df87a97d2793e7c0bd078660cd58dc7f7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Jan 2015 13:22:44 +0100 Subject: [PATCH 515/652] Fix coverity CID 12451 --- 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 3c71195fbc..adf1b0950a 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1340,7 +1340,7 @@ int commander_thread_main(int argc, char *argv[]) status.vtol_fw_permanent_stab = vtol_status.fw_permanent_stab; /* 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) { + if ((status.system_type == VEHICLE_TYPE_VTOL_DUOROTOR) || (status.system_type == VEHICLE_TYPE_VTOL_QUADROTOR)) { status.is_rotary_wing = vtol_status.vtol_in_rw_mode; } } From a1a5a65dfadaaea721e9c87c57b1b1b6bf1d4184 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Jan 2015 13:38:50 +0100 Subject: [PATCH 516/652] Fixed Coverity CID #12412 --- src/modules/uORB/uORB.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/uORB/uORB.cpp b/src/modules/uORB/uORB.cpp index 149b8f6d24..cc6617aea9 100644 --- a/src/modules/uORB/uORB.cpp +++ b/src/modules/uORB/uORB.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (Cc) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -230,7 +230,7 @@ ORBDevNode::open(struct file *filp) ret = CDev::open(filp); if (ret != OK) - free(sd); + delete sd; return ret; } From 5aa75c8f00eede4f502b4c8e94efc65d3cc7f6dc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Jan 2015 13:52:33 +0100 Subject: [PATCH 517/652] Fixed coverity CID #12543 --- src/drivers/px4fmu/fmu.cpp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 4366720407..d3fee16263 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -1637,12 +1637,15 @@ sensor_reset(int ms) fd = open(PX4FMU_DEVICE_PATH, O_RDWR); - if (fd < 0) + if (fd < 0) { errx(1, "open fail"); + } - if (ioctl(fd, GPIO_SENSOR_RAIL_RESET, ms) < 0) - err(1, "servo arm failed"); + if (ioctl(fd, GPIO_SENSOR_RAIL_RESET, ms) < 0) { + warnx("sensor rail reset failed"); + } + close(fd); } void From 65915e5d01cd0b1b8aed95c827a4886f3a57e545 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Jan 2015 14:42:51 +0100 Subject: [PATCH 518/652] EKF: Fix race condition in accel measurement assignment --- .../ekf_att_pos_estimator/estimator_22states.cpp | 15 +++++++++------ 1 file changed, 9 insertions(+), 6 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp index b062097fb3..6cff4b948a 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp @@ -181,9 +181,12 @@ void AttPosEKF::UpdateStrapdownEquationsNED() correctedDelAng.x = dAngIMU.x - states[10]; correctedDelAng.y = dAngIMU.y - states[11]; correctedDelAng.z = dAngIMU.z - states[12]; - dVelIMU.x = dVelIMU.x; - dVelIMU.y = dVelIMU.y; - dVelIMU.z = dVelIMU.z - states[13]; + + Vector3f dVelIMURel; + + dVelIMURel.x = dVelIMU.x; + dVelIMURel.y = dVelIMU.y; + dVelIMURel.z = dVelIMU.z - states[13]; delAngTotal.x += correctedDelAng.x; delAngTotal.y += correctedDelAng.y; @@ -263,9 +266,9 @@ void AttPosEKF::UpdateStrapdownEquationsNED() // transform body delta velocities to delta velocities in the nav frame // * and + operators have been overloaded //delVelNav = Tbn*dVelIMU + gravityNED*dtIMU; - delVelNav.x = Tbn.x.x*dVelIMU.x + Tbn.x.y*dVelIMU.y + Tbn.x.z*dVelIMU.z + gravityNED.x*dtIMU; - delVelNav.y = Tbn.y.x*dVelIMU.x + Tbn.y.y*dVelIMU.y + Tbn.y.z*dVelIMU.z + gravityNED.y*dtIMU; - delVelNav.z = Tbn.z.x*dVelIMU.x + Tbn.z.y*dVelIMU.y + Tbn.z.z*dVelIMU.z + gravityNED.z*dtIMU; + delVelNav.x = Tbn.x.x*dVelIMURel.x + Tbn.x.y*dVelIMURel.y + Tbn.x.z*dVelIMURel.z + gravityNED.x*dtIMU; + delVelNav.y = Tbn.y.x*dVelIMURel.x + Tbn.y.y*dVelIMURel.y + Tbn.y.z*dVelIMURel.z + gravityNED.y*dtIMU; + delVelNav.z = Tbn.z.x*dVelIMURel.x + Tbn.z.y*dVelIMURel.y + Tbn.z.z*dVelIMURel.z + gravityNED.z*dtIMU; // calculate the magnitude of the nav acceleration (required for GPS // variance estimation) From 0a99a26fcc4bea0f19482f632d5492a905026654 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Mon, 19 Jan 2015 16:51:08 +0100 Subject: [PATCH 519/652] Cleanup uploader output. --- Tools/px_uploader.py | 53 +++++++++++++++++++++++++------------------- 1 file changed, 30 insertions(+), 23 deletions(-) diff --git a/Tools/px_uploader.py b/Tools/px_uploader.py index c46f6bede1..95a3d40469 100755 --- a/Tools/px_uploader.py +++ b/Tools/px_uploader.py @@ -266,18 +266,19 @@ class uploader(object): self.__getSync() return value - def __drawProgressBar(self, progress, maxVal): + def __drawProgressBar(self, label, progress, maxVal): if maxVal < progress: progress = maxVal percent = (float(progress) / float(maxVal)) * 100.0 - sys.stdout.write("\rprogress:[%-20s] %.2f%%" % ('='*int(percent/5.0), percent)) + sys.stdout.write("\r%s: [%-20s] %.1f%%" % (label, '='*int(percent/5.0), percent)) sys.stdout.flush() # send the CHIP_ERASE command and wait for the bootloader to become ready - def __erase(self): + def __erase(self, label): + print("\n", end='') self.__send(uploader.CHIP_ERASE + uploader.EOC) @@ -288,15 +289,14 @@ class uploader(object): #Draw progress bar (erase usually takes about 9 seconds to complete) estimatedTimeRemaining = deadline-time.time() if estimatedTimeRemaining >= 9.0: - self.__drawProgressBar(20.0-estimatedTimeRemaining, 9.0) + self.__drawProgressBar(label, 20.0-estimatedTimeRemaining, 9.0) else: - self.__drawProgressBar(10.0, 10.0) + self.__drawProgressBar(label, 10.0, 10.0) sys.stdout.write(" (timeout: %d seconds) " % int(deadline-time.time()) ) sys.stdout.flush() if self.__trySync(): - self.__drawProgressBar(10.0, 10.0) - sys.stdout.write("\nerase complete!\n") + self.__drawProgressBar(label, 10.0, 10.0) return; raise RuntimeError("timed out waiting for erase") @@ -350,7 +350,8 @@ class uploader(object): return [seq[i:i+length] for i in range(0, len(seq), length)] # upload code - def __program(self, fw): + def __program(self, label, fw): + print("\n", end='') code = fw.image groups = self.__split_len(code, uploader.PROG_MULTI_MAX) @@ -361,31 +362,40 @@ class uploader(object): #Print upload progress (throttled, so it does not delay upload progress) uploadProgress += 1 if uploadProgress % 256 == 0: - self.__drawProgressBar(uploadProgress, len(groups)) - self.__drawProgressBar(100, 100) - print("\nprogram complete!") + self.__drawProgressBar(label, uploadProgress, len(groups)) + self.__drawProgressBar(label, 100, 100) # verify code - def __verify_v2(self, fw): + def __verify_v2(self, label, fw): + print("\n", end='') self.__send(uploader.CHIP_VERIFY + uploader.EOC) self.__getSync() code = fw.image groups = self.__split_len(code, uploader.READ_MULTI_MAX) + verifyProgress = 0 for bytes in groups: + verifyProgress += 1 + if verifyProgress % 256 == 0: + self.__drawProgressBar(label, verifyProgress, len(groups)) if (not self.__verify_multi(bytes)): raise RuntimeError("Verification failed") + self.__drawProgressBar(label, 100, 100) - def __verify_v3(self, fw): - expect_crc = fw.crc(self.fw_maxsize) + def __verify_v3(self, label, fw): + print("\n", end='') + self.__drawProgressBar(label, 1, 100) + expect_crc = fw.crc(self.fw_maxsize) self.__send(uploader.GET_CRC + uploader.EOC) report_crc = self.__recv_int() self.__getSync() + verifyProgress = 0 if report_crc != expect_crc: print("Expected 0x%x" % expect_crc) print("Got 0x%x" % report_crc) raise RuntimeError("Program CRC failed") + self.__drawProgressBar(label, 100, 100) # get basic data about the board def identify(self): @@ -439,19 +449,16 @@ class uploader(object): except Exception: # ignore bad character encodings pass - print("erase...") - self.__erase() + + self.__erase("Erase ") + self.__program("Program", fw) - print("program...") - self.__program(fw) - - print("verify...") if self.bl_rev == 2: - self.__verify_v2(fw) + self.__verify_v2("Verify ", fw) else: - self.__verify_v3(fw) + self.__verify_v3("Verify ", fw) - print("done, rebooting.") + print("\nRebooting.\n") self.__reboot() self.port.close() From 8c8315044c4c085a6b8f45f5375a28d19ea655ec Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Jan 2015 20:15:03 +0100 Subject: [PATCH 520/652] NuttX update to include Coverity fixes --- NuttX | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/NuttX b/NuttX index dbcccb2455..574bac488f 160000 --- a/NuttX +++ b/NuttX @@ -1 +1 @@ -Subproject commit dbcccb2455d759b789d549d25e1fbf489b2d3c83 +Subproject commit 574bac488f384ddaa344378e25653c27124a2b69 From fa4d09018527676d3803582a432df8e25c8e5010 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Jan 2015 20:18:44 +0100 Subject: [PATCH 521/652] Hotfix: Fix EKF estimator and remove debug output which should not have been on master anyway --- .../ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 8 +------- 1 file changed, 1 insertion(+), 7 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 3237a1e200..923aa28614 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -1518,12 +1518,6 @@ FixedwingEstimator::task_main() _global_pos.timestamp = _local_pos.timestamp; - // FIXME: usurp terrain alt field for baro_gps_offset - _global_pos.terrain_alt = _baro_gps_offset; - _global_pos.terrain_alt_valid = true; - _global_pos.eph = _baro_alt_filt; - _global_pos.epv = _gps_alt_filt; - /* lazily publish the global position only once available */ if (_global_pos_pub > 0) { /* publish the global position */ From 0af6303d0b70cbac281450b49a11b649852ccbfc Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 20 Jan 2015 14:29:53 +1100 Subject: [PATCH 522/652] hmc5883: fixed mixup of internal and external hmc5883 I2C bus options --- src/drivers/hmc5883/hmc5883.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 2a10b00632..9d63479f38 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -1305,9 +1305,9 @@ struct hmc5883_bus_option { uint8_t busnum; HMC5883 *dev; } bus_options[] = { - { HMC5883_BUS_I2C_INTERNAL, "/dev/hmc5883_int", &HMC5883_I2C_interface, PX4_I2C_BUS_EXPANSION, NULL }, + { HMC5883_BUS_I2C_EXTERNAL, "/dev/hmc5883_ext", &HMC5883_I2C_interface, PX4_I2C_BUS_EXPANSION, NULL }, #ifdef PX4_I2C_BUS_ONBOARD - { HMC5883_BUS_I2C_EXTERNAL, "/dev/hmc5883_ext", &HMC5883_I2C_interface, PX4_I2C_BUS_ONBOARD, NULL }, + { HMC5883_BUS_I2C_INTERNAL, "/dev/hmc5883_int", &HMC5883_I2C_interface, PX4_I2C_BUS_ONBOARD, NULL }, #endif #ifdef PX4_SPIDEV_HMC { HMC5883_BUS_SPI, "/dev/hmc5883_spi", &HMC5883_SPI_interface, PX4_SPI_BUS_SENSORS, NULL }, From 9b10395e94497ef16c99390460808409e08d468f Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 20 Jan 2015 15:07:05 +1100 Subject: [PATCH 523/652] hmc5883: fixed DEVIOCGDEVICEID ioctl we need to pass the ioctl through to the bus interface thanks to Jon Challinger for noticing this bug --- src/drivers/hmc5883/hmc5883.cpp | 4 ++++ src/drivers/hmc5883/hmc5883_i2c.cpp | 5 +++++ src/drivers/hmc5883/hmc5883_spi.cpp | 5 +++++ 3 files changed, 14 insertions(+) diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 9d63479f38..95fbed0ba3 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -66,6 +66,7 @@ #include #include #include +#include #include #include @@ -725,6 +726,9 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg) debug("MAGIOCGEXTERNAL in main driver"); return _interface->ioctl(cmd, dummy); + case DEVIOCGDEVICEID: + return _interface->ioctl(cmd, dummy); + default: /* give it to the superclass */ return CDev::ioctl(filp, cmd, arg); diff --git a/src/drivers/hmc5883/hmc5883_i2c.cpp b/src/drivers/hmc5883/hmc5883_i2c.cpp index 782ea62fe7..f86c1af6b1 100644 --- a/src/drivers/hmc5883/hmc5883_i2c.cpp +++ b/src/drivers/hmc5883/hmc5883_i2c.cpp @@ -53,6 +53,7 @@ #include #include +#include #include "hmc5883.h" @@ -90,6 +91,7 @@ HMC5883_I2C_interface(int bus) HMC5883_I2C::HMC5883_I2C(int bus) : I2C("HMC5883_I2C", nullptr, bus, HMC5883L_ADDRESS, 400000) { + _device_id.devid_s.devtype = DRV_MAG_DEVTYPE_HMC5883; } HMC5883_I2C::~HMC5883_I2C() @@ -117,6 +119,9 @@ HMC5883_I2C::ioctl(unsigned operation, unsigned &arg) return 0; } + case DEVIOCGDEVICEID: + return CDev::ioctl(nullptr, operation, arg); + default: ret = -EINVAL; } diff --git a/src/drivers/hmc5883/hmc5883_spi.cpp b/src/drivers/hmc5883/hmc5883_spi.cpp index 25a2f2b405..aec990ca88 100644 --- a/src/drivers/hmc5883/hmc5883_spi.cpp +++ b/src/drivers/hmc5883/hmc5883_spi.cpp @@ -53,6 +53,7 @@ #include #include +#include #include "hmc5883.h" #include @@ -92,6 +93,7 @@ HMC5883_SPI_interface(int bus) HMC5883_SPI::HMC5883_SPI(int bus, spi_dev_e device) : SPI("HMC5883_SPI", nullptr, bus, device, SPIDEV_MODE3, 11*1000*1000 /* will be rounded to 10.4 MHz */) { + _device_id.devid_s.devtype = DRV_MAG_DEVTYPE_HMC5883; } HMC5883_SPI::~HMC5883_SPI() @@ -146,6 +148,9 @@ HMC5883_SPI::ioctl(unsigned operation, unsigned &arg) return 0; } + case DEVIOCGDEVICEID: + return CDev::ioctl(nullptr, operation, arg); + default: { ret = -EINVAL; From 275a9df928590fbe590fcfed9eb30d794fb3fa6b Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 20 Jan 2015 01:09:20 -0500 Subject: [PATCH 524/652] fixes #1621 - travis after_script upload all Images to s3 with build status and index --- .travis.yml | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/.travis.yml b/.travis.yml index fd2a6b6d1c..fcc273437e 100644 --- a/.travis.yml +++ b/.travis.yml @@ -52,6 +52,16 @@ script: - echo -en 'travis_fold:end:script.3\\r' - zip Firmware.zip Images/*.px4 +after_script: + - git clone git://github.com/PX4/CI-Tools.git + - ./CI-Tools/s3cmd-configure +# upload newest build for this branch with s3 index + - ./CI-Tools/s3cmd-put Images/px4*.px4 CI-Tools/directory/index.html $TRAVIS_BRANCH/ +# archive newest build by date with s3 index + - ./CI-Tools/s3cmd-put Images/px4*.px4 CI-Tools/directory/index.html $TRAVIS_BRANCH/archives/`date "+%Y-%m-%d"`-$TRAVIS_BUILD_ID/ +# upload top level index.html and timestamp.html + - ./CI-Tools/s3cmd-put CI-Tools/index.html timestamp.html / + # We use an encrypted env variable to ensure this only executes when artifacts are uploaded. #after_script: # - echo "Branch $TRAVIS_BRANCH (pull request: $TRAVIS_PULL_REQUEST) ready for flight testing." >> $PX4_REPORT From 0da66c33ad217ae78a52f134067a1c3bfa400dfb Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 20 Jan 2015 01:10:46 -0500 Subject: [PATCH 525/652] travis delete commented after_script PX4_REPORT template --- .travis.yml | 15 --------------- 1 file changed, 15 deletions(-) diff --git a/.travis.yml b/.travis.yml index fcc273437e..69efadfd2b 100644 --- a/.travis.yml +++ b/.travis.yml @@ -62,21 +62,6 @@ after_script: # upload top level index.html and timestamp.html - ./CI-Tools/s3cmd-put CI-Tools/index.html timestamp.html / -# We use an encrypted env variable to ensure this only executes when artifacts are uploaded. -#after_script: -# - 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 -# - /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: provider: releases api_key: From 12e54d1adb437b71bd63973357a647c65a8d77a7 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 20 Jan 2015 01:11:26 -0500 Subject: [PATCH 526/652] travis delete redundant addons artifacts s3 upload --- .travis.yml | 12 ------------ 1 file changed, 12 deletions(-) diff --git a/.travis.yml b/.travis.yml index 69efadfd2b..9f71b4e3a0 100644 --- a/.travis.yml +++ b/.travis.yml @@ -73,18 +73,6 @@ deploy: all_branches: true repo: PX4/Firmware -addons: - artifacts: - paths: - - "Firmware.zip" - key: - secure: j4y9x9KXUiarGrnpFBLPIkEKIH8X6oSRUO61TwxTOamsE0eEKnIaCz1Xq83q7DoqzomHBD3qXAFPV9dhLr1zdKEPJDIyV45GVD4ClIQIzh/P3Uc7kDNxKzdmxY12SH6D0orMpC4tCf1sNK7ETepltWfcnjaDk1Rjs9+TVY7LuzM= - secret: - secure: CJC7VPGtEhJu8Pix85iPF8xUvMPZvTgnHyd9MrSlPKCFFMrlgz9eMT0WWW/TPQ+s4LPwJIfEQx2Q0BRT5tOXuvsTLuOG68mplVddhTWbHb0m0qTQErXFHEppvW4ayuSdeLJ4TjTWphBVainL0mcLLRwQfuAJJDDs/sGan3WrG+Y= - bucket: px4-travis - region: us-east-1 - endpoint: s3-website-us-east-1.amazonaws.com - notifications: webhooks: urls: From 287e802bb5f4f747477b5b69b006d3e6f5295b4b Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 20 Jan 2015 01:54:13 -0500 Subject: [PATCH 527/652] travis s3 move archive location and store Firmware.zip instead of individual images --- .travis.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.travis.yml b/.travis.yml index 9f71b4e3a0..f0008b3b45 100644 --- a/.travis.yml +++ b/.travis.yml @@ -58,7 +58,7 @@ after_script: # upload newest build for this branch with s3 index - ./CI-Tools/s3cmd-put Images/px4*.px4 CI-Tools/directory/index.html $TRAVIS_BRANCH/ # archive newest build by date with s3 index - - ./CI-Tools/s3cmd-put Images/px4*.px4 CI-Tools/directory/index.html $TRAVIS_BRANCH/archives/`date "+%Y-%m-%d"`-$TRAVIS_BUILD_ID/ + - ./CI-Tools/s3cmd-put Firmware.zip CI-Tools/directory/index.html archives/$TRAVIS_BRANCH/`date "+%Y-%m-%d"`-$TRAVIS_BUILD_ID/ # upload top level index.html and timestamp.html - ./CI-Tools/s3cmd-put CI-Tools/index.html timestamp.html / From 70c7764b9f9e780664ae13d0962a55dd226117e6 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Mon, 19 Jan 2015 09:56:42 +0100 Subject: [PATCH 528/652] removed secondary attitude from attitude topic --- src/modules/uORB/topics/vehicle_attitude.h | 18 ------------------ 1 file changed, 18 deletions(-) diff --git a/src/modules/uORB/topics/vehicle_attitude.h b/src/modules/uORB/topics/vehicle_attitude.h index 77324415a7..019944dc07 100755 --- a/src/modules/uORB/topics/vehicle_attitude.h +++ b/src/modules/uORB/topics/vehicle_attitude.h @@ -79,24 +79,6 @@ struct vehicle_attitude_s { float g_comp[3]; /**< Compensated gravity vector */ 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 */ - }; /** From 59cb0cc3b4d91b4dd56369c47d88b4876e9784a9 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Mon, 19 Jan 2015 10:19:18 +0100 Subject: [PATCH 529/652] removed secondary attitude and log quaternion --- .../attitude_estimator_ekf_main.cpp | 38 ------------------- src/modules/sdlog2/sdlog2.c | 16 ++------ src/modules/sdlog2/sdlog2_messages.h | 7 +++- 3 files changed, 9 insertions(+), 52 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 e61204e6c6..b0086676a1 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -595,44 +595,6 @@ 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); diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 0cc193b6c1..a3407e42c4 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1437,6 +1437,10 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- ATTITUDE --- */ if (copy_if_updated(ORB_ID(vehicle_attitude), subs.att_sub, &buf.att)) { log_msg.msg_type = LOG_ATT_MSG; + log_msg.body.log_ATT.q_w = buf.att.q[0]; + log_msg.body.log_ATT.q_x = buf.att.q[1]; + log_msg.body.log_ATT.q_y = buf.att.q[2]; + log_msg.body.log_ATT.q_z = buf.att.q[3]; log_msg.body.log_ATT.roll = buf.att.roll; log_msg.body.log_ATT.pitch = buf.att.pitch; log_msg.body.log_ATT.yaw = buf.att.yaw; @@ -1447,18 +1451,6 @@ 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 --- */ diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index c9221d58a6..7080d9c316 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -50,8 +50,11 @@ #pragma pack(push, 1) /* --- ATT - ATTITUDE --- */ #define LOG_ATT_MSG 2 -#define LOG_ATT2_MSG 41 struct log_ATT_s { + float q_w; + float q_x; + float q_y; + float q_z; float roll; float pitch; float yaw; @@ -460,7 +463,7 @@ struct log_PARM_s { static const struct log_format_s log_formats[] = { /* business-level messages, ID < 0x80 */ 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(ATT, "fffffffffffff", "qw,qx,qy,qz,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"), From 764f4994052e2e73fe7576c98d222d5d9cd3f352 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 20 Jan 2015 14:06:12 +0100 Subject: [PATCH 530/652] Fix CID 12470 --- src/drivers/px4io/px4io.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 96ebedd83c..ed8f86fae1 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -2278,6 +2278,7 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg) case PWM_SERVO_GET_DISABLE_LOCKDOWN: *(unsigned *)arg = _lockdown_override; + break; case PWM_SERVO_SET_FORCE_SAFETY_OFF: /* force safety swith off */ From fc9c67f5bee43127bba75d426ccb5b5ebd903c73 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 20 Jan 2015 14:07:59 +0100 Subject: [PATCH 531/652] Fix CID 12439 --- src/drivers/px4io/px4io.cpp | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index ed8f86fae1..73204e2207 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -3070,12 +3070,13 @@ lockdown(int argc, char *argv[]) if (ret > 0) { - read(0, &c, 1); + if (read(0, &c, 1) > 0) { - if (c != 'y') { - exit(0); - } else if (c == 'y') { - break; + if (c != 'y') { + exit(0); + } else if (c == 'y') { + break; + } } } From f1a53f5152da68e48f7dea8bb0fc3a03e4effd12 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 20 Jan 2015 14:12:25 +0100 Subject: [PATCH 532/652] Fixing CID 12528 --- src/drivers/px4io/px4io.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 73204e2207..f98010f688 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1773,12 +1773,12 @@ PX4IO::print_debug() #ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 int io_fd = -1; - if (io_fd < 0) { + if (io_fd <= 0) { io_fd = ::open("/dev/ttyS0", O_RDONLY | O_NONBLOCK | O_NOCTTY); } /* read IO's output */ - if (io_fd > 0) { + if (io_fd >= 0) { pollfd fds[1]; fds[0].fd = io_fd; fds[0].events = POLLIN; From c04f8705643962d9099517475c110ebb511b7e8b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 20 Jan 2015 14:14:23 +0100 Subject: [PATCH 533/652] Fix CID 12427 & 12438 --- src/drivers/px4io/px4io.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index f98010f688..82c7bf2c45 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -3007,8 +3007,9 @@ monitor(void) poll(fds, 1, 2000); if (fds[0].revents == POLLIN) { - int c; - read(0, &c, 1); + /* control logic is to cancel with any key */ + char c; + (void)read(0, &c, 1); if (cancels-- == 0) { printf("\033[2J\033[H"); /* move cursor home and clear screen */ From 603b43b6d840bf6a730c56f15a0bb35b2af8ea17 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 20 Jan 2015 14:16:48 +0100 Subject: [PATCH 534/652] Fix CID 12574 & 12553 --- 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 82c7bf2c45..0e4e38c2fe 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1102,7 +1102,7 @@ PX4IO::io_set_control_state(unsigned group) uint16_t regs[_max_actuators]; /* get controls */ - bool changed; + bool changed = false; switch (group) { case 0: From 81082c4119c715d4b8e22040052115ecbff6cc4d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 20 Jan 2015 14:21:03 +0100 Subject: [PATCH 535/652] Fix CID 12453 --- src/drivers/px4io/px4io.cpp | 14 +++++++++++--- 1 file changed, 11 insertions(+), 3 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 0e4e38c2fe..5698dbc578 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1144,11 +1144,13 @@ PX4IO::io_set_control_state(unsigned group) break; } - if (!changed) + if (!changed) { return -1; + } - for (unsigned i = 0; i < _max_controls; i++) + for (unsigned i = 0; i < _max_controls; i++) { regs[i] = FLOAT_TO_REG(controls.control[i]); + } /* copy values to registers in IO */ return io_reg_set(PX4IO_PAGE_CONTROLS, group * PX4IO_PROTOCOL_MAX_CONTROL_COUNT, regs, _max_controls); @@ -3240,7 +3242,13 @@ px4io_main(int argc, char *argv[]) if (!strcmp(argv[1], "limit")) { if ((argc > 2)) { - g_dev->set_update_rate(atoi(argv[2])); + int limitrate = atoi(argv[2]); + + if (limitrate > 0) { + g_dev->set_update_rate(limitrate); + } else { + errx(1, "invalid rate: %d", limitrate); + } } else { errx(1, "missing argument (50 - 500 Hz)"); From b84e50a3c601d8afebacad40d17d2db0e5644ce1 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 20 Jan 2015 09:34:03 -0500 Subject: [PATCH 536/652] travis upload s3 artifacts to Firmware subdirectory --- .travis.yml | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/.travis.yml b/.travis.yml index f0008b3b45..52ced6b4fc 100644 --- a/.travis.yml +++ b/.travis.yml @@ -56,9 +56,10 @@ after_script: - git clone git://github.com/PX4/CI-Tools.git - ./CI-Tools/s3cmd-configure # upload newest build for this branch with s3 index - - ./CI-Tools/s3cmd-put Images/px4*.px4 CI-Tools/directory/index.html $TRAVIS_BRANCH/ + - ./CI-Tools/s3cmd-put Images/px4*.px4 CI-Tools/directory/index.html Firmware/$TRAVIS_BRANCH/ # archive newest build by date with s3 index - - ./CI-Tools/s3cmd-put Firmware.zip CI-Tools/directory/index.html archives/$TRAVIS_BRANCH/`date "+%Y-%m-%d"`-$TRAVIS_BUILD_ID/ + - ./CI-Tools/s3cmd-put Firmware.zip archives/Firmware/$TRAVIS_BRANCH/`date "+%Y-%m-%d"`-$TRAVIS_BUILD_ID/ + - ./CI-Tools/s3cmd-put CI-Tools/directory/index.html archives/Firmware/$TRAVIS_BRANCH/ # upload top level index.html and timestamp.html - ./CI-Tools/s3cmd-put CI-Tools/index.html timestamp.html / From b8b0d1d4ae3c7ced44eb1a6c394ee5877803c309 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Tue, 20 Jan 2015 22:28:58 +0100 Subject: [PATCH 537/652] Remove a noisy debugging statement. --- 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 adf1b0950a..247a2c5b83 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1457,7 +1457,7 @@ int commander_thread_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(subsystem_info), subsys_sub, &info); - warnx("subsystem changed: %d\n", (int)info.subsystem_type); + //warnx("subsystem changed: %d\n", (int)info.subsystem_type); /* mark / unmark as present */ if (info.present) { From 76e84e017f89172d46096101b2cd96620dd432dc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Jan 2015 14:05:09 +0100 Subject: [PATCH 538/652] VA_ARGS fixes from coverity --- src/modules/systemlib/err.c | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/modules/systemlib/err.c b/src/modules/systemlib/err.c index 998b5ac7d0..a1a8e7ea8e 100644 --- a/src/modules/systemlib/err.c +++ b/src/modules/systemlib/err.c @@ -154,6 +154,7 @@ warn(const char *fmt, ...) va_start(args, fmt); vwarn(fmt, args); + va_end(args); } void @@ -169,6 +170,7 @@ warnc(int errcode, const char *fmt, ...) va_start(args, fmt); vwarnc(errcode, fmt, args); + va_end(args); } void @@ -184,6 +186,7 @@ warnx(const char *fmt, ...) va_start(args, fmt); vwarnx(fmt, args); + va_end(args); } void From ebacd9b5ad1ab0590559d8839a55f286febd0dd2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Jan 2015 14:05:38 +0100 Subject: [PATCH 539/652] GPS fixes from coverity --- src/drivers/gps/gps_helper.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/drivers/gps/gps_helper.cpp b/src/drivers/gps/gps_helper.cpp index 05dfc99b76..8157bc7e68 100644 --- a/src/drivers/gps/gps_helper.cpp +++ b/src/drivers/gps/gps_helper.cpp @@ -88,8 +88,6 @@ GPS_Helper::set_baudrate(const int &fd, unsigned baud) case 115200: speed = B115200; break; - warnx("try baudrate: %d\n", speed); - default: warnx("ERR: baudrate: %d\n", baud); return -EINVAL; From 6300338d2a3ebd110c738305a7ea21c1d318dfba Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Jan 2015 14:05:59 +0100 Subject: [PATCH 540/652] FMU driver: Fix resource leak for test routine. --- src/drivers/px4fmu/fmu.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index d3fee16263..112c015136 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012-2013 PX4 Development Team. All rights reserved. + * Copyright (C) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions From 12ce2de7ae0aa62f9501f7f53837772a6c662724 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Jan 2015 14:06:21 +0100 Subject: [PATCH 541/652] IO driver: Fix resource corner case (not a real leak) --- src/drivers/px4io/px4io.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 5698dbc578..3b64809e39 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -3006,7 +3006,9 @@ monitor(void) fds[0].fd = 0; fds[0].events = POLLIN; - poll(fds, 1, 2000); + if (poll(fds, 1, 2000) < 0) { + errx(1, "poll fail"); + } if (fds[0].revents == POLLIN) { /* control logic is to cancel with any key */ From 859b34531473c9c78a4fee04de3cbdd147381913 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Jan 2015 14:07:00 +0100 Subject: [PATCH 542/652] Mag cal: Make error handling more explicit --- src/modules/commander/mag_calibration.cpp | 72 ++++++++++++----------- 1 file changed, 39 insertions(+), 33 deletions(-) diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 9d2abd3ceb..53013fdb97 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -150,54 +150,60 @@ int do_mag_calibration(int mavlink_fd) if (res == OK) { int sub_mag = orb_subscribe(ORB_ID(sensor_mag0)); - struct mag_report mag; - /* limit update rate to get equally spaced measurements over time (in ms) */ - orb_set_interval(sub_mag, (calibration_interval / 1000) / calibration_maxcount); + if (sub_mag < 0) { + mavlink_log_critical(mavlink_fd, "No mag found, abort"); + res = ERROR; + } else { + struct mag_report mag; - /* calibrate offsets */ - uint64_t calibration_deadline = hrt_absolute_time() + calibration_interval; - unsigned poll_errcount = 0; + /* limit update rate to get equally spaced measurements over time (in ms) */ + orb_set_interval(sub_mag, (calibration_interval / 1000) / calibration_maxcount); - mavlink_log_info(mavlink_fd, "Turn on all sides: front/back,left/right,up/down"); + /* calibrate offsets */ + uint64_t calibration_deadline = hrt_absolute_time() + calibration_interval; + unsigned poll_errcount = 0; - calibration_counter = 0; + mavlink_log_info(mavlink_fd, "Turn on all sides: front/back,left/right,up/down"); - while (hrt_absolute_time() < calibration_deadline && - calibration_counter < calibration_maxcount) { + calibration_counter = 0U; - /* wait blocking for new data */ - struct pollfd fds[1]; - fds[0].fd = sub_mag; - fds[0].events = POLLIN; + while (hrt_absolute_time() < calibration_deadline && + calibration_counter < calibration_maxcount) { - int poll_ret = poll(fds, 1, 1000); + /* wait blocking for new data */ + struct pollfd fds[1]; + fds[0].fd = sub_mag; + fds[0].events = POLLIN; - if (poll_ret > 0) { - orb_copy(ORB_ID(sensor_mag0), sub_mag, &mag); + int poll_ret = poll(fds, 1, 1000); - x[calibration_counter] = mag.x; - y[calibration_counter] = mag.y; - z[calibration_counter] = mag.z; + if (poll_ret > 0) { + orb_copy(ORB_ID(sensor_mag0), sub_mag, &mag); - calibration_counter++; + x[calibration_counter] = mag.x; + y[calibration_counter] = mag.y; + z[calibration_counter] = mag.z; - if (calibration_counter % (calibration_maxcount / 20) == 0) { - mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 20 + (calibration_counter * 50) / calibration_maxcount); + calibration_counter++; + + if (calibration_counter % (calibration_maxcount / 20) == 0) { + mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 20 + (calibration_counter * 50) / calibration_maxcount); + } + + } else { + poll_errcount++; } - } else { - poll_errcount++; + if (poll_errcount > 1000) { + mavlink_log_critical(mavlink_fd, CAL_FAILED_SENSOR_MSG); + res = ERROR; + break; + } } - if (poll_errcount > 1000) { - mavlink_log_critical(mavlink_fd, CAL_FAILED_SENSOR_MSG); - res = ERROR; - break; - } + close(sub_mag); } - - close(sub_mag); } float sphere_x; @@ -205,7 +211,7 @@ int do_mag_calibration(int mavlink_fd) float sphere_z; float sphere_radius; - if (res == OK) { + if (res == OK && calibration_counter > (calibration_maxcount / 2)) { /* sphere fit */ mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 70); From a9c8f2331b6cca3ca59debe82df82ace502170d8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Jan 2015 14:07:18 +0100 Subject: [PATCH 543/652] IO firmware: Make error checking more obvious --- src/modules/px4iofirmware/controls.c | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 58c9429b6f..d20f776d62 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -454,6 +454,10 @@ ppm_input(uint16_t *values, uint16_t *num_values, uint16_t *frame_len) { bool result = false; + if (!(num_values) || !(values) || !(frame_len)) { + return result; + } + /* avoid racing with PPM updates */ irqstate_t state = irqsave(); @@ -468,7 +472,7 @@ ppm_input(uint16_t *values, uint16_t *num_values, uint16_t *frame_len) if (*num_values > PX4IO_RC_INPUT_CHANNELS) *num_values = PX4IO_RC_INPUT_CHANNELS; - for (unsigned i = 0; i < *num_values; i++) { + for (unsigned i = 0; ((i < *num_values) && (i < PPM_MAX_CHANNELS)); i++) { values[i] = ppm_buffer[i]; } @@ -476,8 +480,7 @@ ppm_input(uint16_t *values, uint16_t *num_values, uint16_t *frame_len) ppm_last_valid_decode = 0; /* store PPM frame length */ - if (num_values) - *frame_len = ppm_frame_length; + *frame_len = ppm_frame_length; /* good if we got any channels */ result = (*num_values > 0); From 9a40fe4edeb2874ee1ec280b863d43825b92df7a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Jan 2015 14:07:41 +0100 Subject: [PATCH 544/652] Sensors: Fix corner case on FD leak (not a real leak) --- src/modules/sensors/sensors.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 2c798af3bd..793b7c2b65 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -1465,7 +1465,7 @@ Sensors::parameter_update_poll(bool forced) /* this sensor is optional, abort without error */ - if (fd > 0) { + if (fd >= 0) { struct airspeed_scale airscale = { _parameters.diff_pres_offset_pa, 1.0f, From 5f77a57761517b6f7b2113a17ff7cb621376216b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 21 Jan 2015 09:19:15 +0100 Subject: [PATCH 545/652] FMUv1 + FMUv2: Reduce excessively large OS stack sizes. Actual use was 800 bytes hpwork, 400 bytes lpwork, 1700 bytes in running system. This leaves 1K headroom per task --- nuttx-configs/px4fmu-v1/nsh/defconfig | 6 +++--- nuttx-configs/px4fmu-v2/nsh/defconfig | 6 +++--- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig index fade1f0c69..a467fa605e 100644 --- a/nuttx-configs/px4fmu-v1/nsh/defconfig +++ b/nuttx-configs/px4fmu-v1/nsh/defconfig @@ -417,7 +417,7 @@ CONFIG_PREALLOC_TIMERS=50 # Stack and heap information # CONFIG_IDLETHREAD_STACKSIZE=3500 -CONFIG_USERMAIN_STACKSIZE=3000 +CONFIG_USERMAIN_STACKSIZE=2600 CONFIG_PTHREAD_STACK_MIN=512 CONFIG_PTHREAD_STACK_DEFAULT=2048 @@ -719,11 +719,11 @@ CONFIG_SCHED_WORKQUEUE=y CONFIG_SCHED_HPWORK=y CONFIG_SCHED_WORKPRIORITY=192 CONFIG_SCHED_WORKPERIOD=5000 -CONFIG_SCHED_WORKSTACKSIZE=2048 +CONFIG_SCHED_WORKSTACKSIZE=1800 CONFIG_SCHED_LPWORK=y CONFIG_SCHED_LPWORKPRIORITY=50 CONFIG_SCHED_LPWORKPERIOD=50000 -CONFIG_SCHED_LPWORKSTACKSIZE=2048 +CONFIG_SCHED_LPWORKSTACKSIZE=1800 # CONFIG_LIB_KBDCODEC is not set # CONFIG_LIB_SLCDCODEC is not set diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index ac3dbe5daf..9030a1f022 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -451,7 +451,7 @@ CONFIG_PREALLOC_TIMERS=50 # Stack and heap information # CONFIG_IDLETHREAD_STACKSIZE=3500 -CONFIG_USERMAIN_STACKSIZE=3000 +CONFIG_USERMAIN_STACKSIZE=2600 CONFIG_PTHREAD_STACK_MIN=512 CONFIG_PTHREAD_STACK_DEFAULT=2048 @@ -797,11 +797,11 @@ CONFIG_SCHED_WORKQUEUE=y CONFIG_SCHED_HPWORK=y CONFIG_SCHED_WORKPRIORITY=192 CONFIG_SCHED_WORKPERIOD=5000 -CONFIG_SCHED_WORKSTACKSIZE=2000 +CONFIG_SCHED_WORKSTACKSIZE=1800 CONFIG_SCHED_LPWORK=y CONFIG_SCHED_LPWORKPRIORITY=50 CONFIG_SCHED_LPWORKPERIOD=50000 -CONFIG_SCHED_LPWORKSTACKSIZE=2000 +CONFIG_SCHED_LPWORKSTACKSIZE=1800 # CONFIG_LIB_KBDCODEC is not set # CONFIG_LIB_SLCDCODEC is not set From 9aa7daa86c7f18dcbba4f20cc1a2b3189117f242 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 21 Jan 2015 09:22:46 +0100 Subject: [PATCH 546/652] PX4IO: Adjust stack size based on real usage of 900 bytes --- 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 3b64809e39..1c9a5adc9a 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -834,7 +834,7 @@ PX4IO::init() _task = task_spawn_cmd("px4io", SCHED_DEFAULT, SCHED_PRIORITY_ACTUATOR_OUTPUTS, - 2000, + 1800, (main_t)&PX4IO::task_main_trampoline, nullptr); From 21b88cd8742959585dbe479907ef82e4e0136bbb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 21 Jan 2015 09:24:31 +0100 Subject: [PATCH 547/652] dataman: Reduce stack to 1800, based on real usage of 800 bytes --- src/modules/dataman/dataman.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/dataman/dataman.c b/src/modules/dataman/dataman.c index 705e54be32..68bf120240 100644 --- a/src/modules/dataman/dataman.c +++ b/src/modules/dataman/dataman.c @@ -793,7 +793,7 @@ start(void) sem_init(&g_init_sema, 1, 0); /* start the worker thread */ - if ((task = task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, 2000, task_main, NULL)) <= 0) { + if ((task = task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, 1800, task_main, NULL)) <= 0) { warn("task start failed"); return -1; } From dad2f754e968493d56a23c29625c2c0c264df4c1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 21 Jan 2015 09:54:31 +0100 Subject: [PATCH 548/652] Logging: Reduce logging on FMUv1 to free system resources --- ROMFS/px4fmu_common/init.d/rc.logging | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.logging b/ROMFS/px4fmu_common/init.d/rc.logging index 25f31a7e04..a14eb5247a 100644 --- a/ROMFS/px4fmu_common/init.d/rc.logging +++ b/ROMFS/px4fmu_common/init.d/rc.logging @@ -8,7 +8,7 @@ then if ver hwcmp PX4FMU_V1 then echo "Start sdlog2 at 50Hz" - sdlog2 start -r 50 -a -b 4 -t + sdlog2 start -r 40 -a -b 3 -t else echo "Start sdlog2 at 200Hz" sdlog2 start -r 200 -a -b 22 -t From 0632d882bb734ac52ff5f7f5d1dc7930046e4600 Mon Sep 17 00:00:00 2001 From: Denis Yeldandi Date: Wed, 21 Jan 2015 12:22:07 +0300 Subject: [PATCH 549/652] INAV_W_Z_GPS_V defaults to 0 --- .../position_estimator_inav/position_estimator_inav_params.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.c b/src/modules/position_estimator_inav/position_estimator_inav_params.c index 1a1b3d3102..b2771f1a62 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -72,7 +72,7 @@ PARAM_DEFINE_FLOAT(INAV_W_Z_GPS_P, 0.005f); * @max 10.0 * @group Position Estimator INAV */ -PARAM_DEFINE_FLOAT(INAV_W_Z_GPS_V, 1.0f); +PARAM_DEFINE_FLOAT(INAV_W_Z_GPS_V, 0.0f); /** * Z axis weight for vision From e092705b0d518197987a7a74a2a8cdd4eb740997 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 21 Jan 2015 10:57:41 +0100 Subject: [PATCH 550/652] EKF bugfix: Replace all off-by-one (<=22 and <23) instances by correct check. Replace all badly-written <=21 (correct, but unreadable) code by < n_states --- .../estimator_22states.cpp | 40 +++++++++---------- 1 file changed, 20 insertions(+), 20 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp index 6cff4b948a..15d018ab62 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp @@ -343,9 +343,9 @@ void AttPosEKF::CovariancePrediction(float dt) } if (!inhibitMagStates) { for (uint8_t i=16; i<=18; i++) processNoise[i] = dt * magEarthSigma; - for (uint8_t i=19; i<=21; i++) processNoise[i] = dt * magBodySigma; + for (uint8_t i=19; i < n_states; i++) processNoise[i] = dt * magBodySigma; } else { - for (uint8_t i=16; i<=21; i++) processNoise[i] = 0; + for (uint8_t i=16; i < n_states; i++) processNoise[i] = 0; } // square all sigmas @@ -1179,7 +1179,7 @@ void AttPosEKF::FuseVelposNED() } // Don't update magnetic field states if inhibited if (inhibitMagStates) { - for (uint8_t i = 16; i<=21; i++) + for (uint8_t i = 16; i < n_states; i++) { Kfusion[i] = 0; } @@ -1360,7 +1360,7 @@ void AttPosEKF::FuseMagnetometer() Kfusion[20] = SK_MX[0]*(P[20][19] + P[20][1]*SH_MAG[0] + P[20][3]*SH_MAG[2] + P[20][0]*SK_MX[3] - P[20][2]*SK_MX[2] - P[20][16]*SK_MX[1] + P[20][17]*SK_MX[5] - P[20][18]*SK_MX[4]); Kfusion[21] = SK_MX[0]*(P[21][19] + P[21][1]*SH_MAG[0] + P[21][3]*SH_MAG[2] + P[21][0]*SK_MX[3] - P[21][2]*SK_MX[2] - P[21][16]*SK_MX[1] + P[21][17]*SK_MX[5] - P[21][18]*SK_MX[4]); } else { - for (uint8_t i=16; i <= 21; i++) { + for (uint8_t i=16; i < n_states; i++) { Kfusion[i] = 0; } } @@ -1538,14 +1538,14 @@ void AttPosEKF::FuseMagnetometer() for (uint8_t j = 4; j <= 15; j++) KH[i][j] = 0.0f; if (!onGround) { - for (uint8_t j = 16; j <= 21; j++) + for (uint8_t j = 16; j < n_states; j++) { KH[i][j] = Kfusion[i] * H_MAG[j]; } } else { - for (uint8_t j = 16; j <= 21; j++) + for (uint8_t j = 16; j < n_states; j++) { KH[i][j] = 0.0f; } @@ -1562,7 +1562,7 @@ void AttPosEKF::FuseMagnetometer() } if (!onGround) { - for (uint8_t k = 16; k<=21; k++) + for (uint8_t k = 16; k < n_states; k++) { KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j]; } @@ -1664,7 +1664,7 @@ void AttPosEKF::FuseAirspeed() Kfusion[20] = SK_TAS*(P[20][4]*SH_TAS[2] - P[20][14]*SH_TAS[2] + P[20][5]*SH_TAS[1] - P[20][15]*SH_TAS[1] + P[20][6]*vd*SH_TAS[0]); Kfusion[21] = SK_TAS*(P[21][4]*SH_TAS[2] - P[21][14]*SH_TAS[2] + P[21][5]*SH_TAS[1] - P[21][15]*SH_TAS[1] + P[21][6]*vd*SH_TAS[0]); } else { - for (uint8_t i=16; i <= 21; i++) { + for (uint8_t i=16; i < n_states; i++) { Kfusion[i] = 0; } } @@ -1676,7 +1676,7 @@ void AttPosEKF::FuseAirspeed() if ((innovVtas*innovVtas*SK_TAS) < 25.0f) { // correct the state vector - for (uint8_t j=0; j <= 22; j++) + for (uint8_t j=0; j < n_states; j++) { states[j] = states[j] - Kfusion[j] * innovVtas; } @@ -1693,7 +1693,7 @@ void AttPosEKF::FuseAirspeed() // correct the covariance P = (I - K*H)*P // take advantage of the empty columns in H to reduce the // number of operations - for (uint8_t i = 0; i <= 22; i++) + for (uint8_t i = 0; i < n_states; i++) { for (uint8_t j = 0; j <= 3; j++) KH[i][j] = 0.0; for (uint8_t j = 4; j <= 6; j++) @@ -1705,11 +1705,11 @@ void AttPosEKF::FuseAirspeed() { KH[i][j] = Kfusion[i] * H_TAS[j]; } - for (uint8_t j = 16; j <= 22; j++) KH[i][j] = 0.0; + for (uint8_t j = 16; j < n_states; j++) KH[i][j] = 0.0; } - for (uint8_t i = 0; i <= 22; i++) + for (uint8_t i = 0; i < n_states; i++) { - for (uint8_t j = 0; j <= 22; j++) + for (uint8_t j = 0; j < n_states; j++) { KHP[i][j] = 0.0; for (uint8_t k = 4; k <= 6; k++) @@ -1722,9 +1722,9 @@ void AttPosEKF::FuseAirspeed() } } } - for (uint8_t i = 0; i <= 22; i++) + for (uint8_t i = 0; i < n_states; i++) { - for (uint8_t j = 0; j <= 22; j++) + for (uint8_t j = 0; j < n_states; j++) { P[i][j] = P[i][j] - KHP[i][j]; } @@ -1836,7 +1836,7 @@ void AttPosEKF::FuseOptFlow() tempVar[8] = (SK_LOS[4] + q0*tempVar[2]); // calculate observation jacobians for X LOS rate - for (uint8_t i = 0; i < 23; i++) H_LOS[0][i] = 0; + for (uint8_t i = 0; i < n_states; i++) H_LOS[0][i] = 0; H_LOS[0][0] = - SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) - 2*q0*SH_LOS[2]*SH_LOS[3]; H_LOS[0][1] = 2*q1*SH_LOS[2]*SH_LOS[3] - SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn); H_LOS[0][2] = 2*q2*SH_LOS[2]*SH_LOS[3] - SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn); @@ -1877,7 +1877,7 @@ void AttPosEKF::FuseOptFlow() K_LOS[0][20] = -SK_LOS[1]*(P[20][0]*tempVar[8] + P[20][1]*tempVar[7] - P[20][3]*tempVar[6] + P[20][2]*tempVar[5] - P[20][4]*tempVar[4] + P[20][6]*tempVar[3] - P[20][9]*tempVar[1] + P[20][5]*tempVar[0]); K_LOS[0][21] = -SK_LOS[1]*(P[21][0]*tempVar[8] + P[21][1]*tempVar[7] - P[21][3]*tempVar[6] + P[21][2]*tempVar[5] - P[21][4]*tempVar[4] + P[21][6]*tempVar[3] - P[21][9]*tempVar[1] + P[21][5]*tempVar[0]); } else { - for (uint8_t i = 16; i <= 21; i++) { + for (uint8_t i = 16; i < n_states; i++) { K_LOS[0][i] = 0.0f; } } @@ -1898,7 +1898,7 @@ void AttPosEKF::FuseOptFlow() tempVar[8] = SH_LOS[0]*SK_LOS[7]*SK_LOS[8]; // Calculate observation jacobians for Y LOS rate - for (uint8_t i = 0; i < 23; i++) H_LOS[1][i] = 0; + for (uint8_t i = 0; i < n_states; i++) H_LOS[1][i] = 0; H_LOS[1][0] = SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) + 2*q0*SH_LOS[1]*SH_LOS[3]; H_LOS[1][1] = SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q1*SH_LOS[1]*SH_LOS[3]; H_LOS[1][2] = - SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) - 2*q2*SH_LOS[1]*SH_LOS[3]; @@ -1939,7 +1939,7 @@ void AttPosEKF::FuseOptFlow() K_LOS[1][20] = SK_LOS[0]*(P[20][0]*tempVar[1] + P[20][1]*tempVar[2] - P[20][2]*tempVar[3] + P[20][3]*tempVar[4] + P[20][5]*tempVar[5] - P[20][6]*tempVar[6] - P[20][9]*tempVar[7] + P[20][4]*tempVar[8]); K_LOS[1][21] = SK_LOS[0]*(P[21][0]*tempVar[1] + P[21][1]*tempVar[2] - P[21][2]*tempVar[3] + P[21][3]*tempVar[4] + P[21][5]*tempVar[5] - P[21][6]*tempVar[6] - P[21][9]*tempVar[7] + P[21][4]*tempVar[8]); } else { - for (uint8_t i = 16; i <= 21; i++) { + for (uint8_t i = 16; i < n_states; i++) { K_LOS[1][i] = 0.0f; } } @@ -1983,7 +1983,7 @@ void AttPosEKF::FuseOptFlow() KH[i][j] = 0.0f; } KH[i][9] = K_LOS[obsIndex][i] * H_LOS[obsIndex][9]; - for (uint8_t j = 10; j <= 21; j++) + for (uint8_t j = 10; j < n_states; j++) { KH[i][j] = 0.0f; } From 87aaf2a959433d2c3df3848ffdcb173d113e807e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 21 Jan 2015 11:01:25 +0100 Subject: [PATCH 551/652] uORB: Remove unused function --- src/modules/uORB/uORB.cpp | 12 ------------ 1 file changed, 12 deletions(-) diff --git a/src/modules/uORB/uORB.cpp b/src/modules/uORB/uORB.cpp index cc6617aea9..3373aac837 100644 --- a/src/modules/uORB/uORB.cpp +++ b/src/modules/uORB/uORB.cpp @@ -817,18 +817,6 @@ uorb_main(int argc, char *argv[]) namespace { -void debug(const char *fmt, ...) -{ - va_list ap; - - va_start(ap, fmt); - vfprintf(stderr, fmt, ap); - va_end(ap); - fprintf(stderr, "\n"); - fflush(stderr); - usleep(100000); -} - /** * Advertise a node; don't consider it an error if the node has * already been advertised. From e8e4a3b5da058bd2ab8575c095dd74a5484333be Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 21 Jan 2015 11:04:56 +0100 Subject: [PATCH 552/652] Allow GCC 4.9.3 --- makefiles/toolchain_gnu-arm-eabi.mk | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/makefiles/toolchain_gnu-arm-eabi.mk b/makefiles/toolchain_gnu-arm-eabi.mk index d7e48f03be..fbe378f509 100644 --- a/makefiles/toolchain_gnu-arm-eabi.mk +++ b/makefiles/toolchain_gnu-arm-eabi.mk @@ -50,7 +50,7 @@ OBJDUMP = $(CROSSDEV)objdump # Check if the right version of the toolchain is available # -CROSSDEV_VER_SUPPORTED = 4.7.4 4.7.5 4.7.6 4.8.4 +CROSSDEV_VER_SUPPORTED = 4.7.4 4.7.5 4.7.6 4.8.4 4.9.3 CROSSDEV_VER_FOUND = $(shell $(CC) -dumpversion) ifeq (,$(findstring $(CROSSDEV_VER_FOUND), $(CROSSDEV_VER_SUPPORTED))) From eea3c801f4be7efa306af01a8153e8bbee4d43ce Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 10 Jan 2015 22:26:49 +0300 Subject: [PATCH 553/652] UAVCAN perf counters --- src/modules/uavcan/uavcan_main.cpp | 30 ++++++++++++++++++++++++++++++ src/modules/uavcan/uavcan_main.hpp | 9 +++++++-- 2 files changed, 37 insertions(+), 2 deletions(-) diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 60901e0c75..af5f2ec96f 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -81,6 +81,22 @@ UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &sys if (res < 0) { std::abort(); } + + if (_perfcnt_node_spin_elapsed == nullptr) { + errx(1, "uavcan: couldn't allocate _perfcnt_node_spin_elapsed"); + } + + if (_perfcnt_esc_mixer_output_elapsed == nullptr) { + errx(1, "uavcan: couldn't allocate _perfcnt_esc_mixer_output_elapsed"); + } + + if (_perfcnt_esc_mixer_total_elapsed == nullptr) { + errx(1, "uavcan: couldn't allocate _perfcnt_esc_mixer_total_elapsed"); + } + + if (_perfcnt_esc_mixer_subscriptions == nullptr) { + errx(1, "uavcan: couldn't allocate _perfcnt_esc_mixer_subscriptions"); + } } UavcanNode::~UavcanNode() @@ -118,6 +134,11 @@ UavcanNode::~UavcanNode() } _instance = nullptr; + + perf_free(_perfcnt_node_spin_elapsed); + perf_free(_perfcnt_esc_mixer_output_elapsed); + perf_free(_perfcnt_esc_mixer_total_elapsed); + perf_free(_perfcnt_esc_mixer_subscriptions); } int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate) @@ -265,10 +286,12 @@ int UavcanNode::init(uavcan::NodeID node_id) void UavcanNode::node_spin_once() { + perf_begin(_perfcnt_node_spin_elapsed); const int spin_res = _node.spin(uavcan::MonotonicTime()); if (spin_res < 0) { warnx("node spin error %i", spin_res); } + perf_end(_perfcnt_node_spin_elapsed); } /* @@ -344,8 +367,12 @@ int UavcanNode::run() // Mutex is unlocked while the thread is blocked on IO multiplexing (void)pthread_mutex_unlock(&_node_mutex); + perf_end(_perfcnt_esc_mixer_total_elapsed); // end goes first, it's not a mistake + const int poll_ret = ::poll(_poll_fds, _poll_fds_num, PollTimeoutMs); + perf_begin(_perfcnt_esc_mixer_total_elapsed); + (void)pthread_mutex_lock(&_node_mutex); node_spin_once(); // Non-blocking @@ -430,7 +457,9 @@ int UavcanNode::run() } // Output to the bus _outputs.timestamp = hrt_absolute_time(); + perf_begin(_perfcnt_esc_mixer_output_elapsed); _esc_controller.update_outputs(_outputs.output, _outputs.noutputs); + perf_end(_perfcnt_esc_mixer_output_elapsed); } @@ -498,6 +527,7 @@ UavcanNode::arm_actuators(bool arm) void UavcanNode::subscribe() { + perf_count(_perfcnt_esc_mixer_subscriptions); // Subscribe/unsubscribe to required actuator control groups uint32_t sub_groups = _groups_required & ~_groups_subscribed; uint32_t unsub_groups = _groups_subscribed & ~_groups_required; diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index 98f2e5ad4b..8a0993f15d 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -34,9 +34,9 @@ #pragma once #include - #include #include +#include #include #include @@ -66,7 +66,7 @@ */ class UavcanNode : public device::CDev { - static constexpr unsigned MemPoolSize = 10752; + static constexpr unsigned MemPoolSize = 10752; ///< Refer to the libuavcan manual to learn why static constexpr unsigned RxQueueLenPerIface = 64; static constexpr unsigned StackSize = 3000; @@ -142,4 +142,9 @@ private: // index into _poll_fds for each _control_subs handle uint8_t _poll_ids[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN]; + + perf_counter_t _perfcnt_node_spin_elapsed = perf_alloc(PC_ELAPSED, "uavcan_node_spin_elapsed"); + perf_counter_t _perfcnt_esc_mixer_output_elapsed = perf_alloc(PC_ELAPSED, "uavcan_esc_mixer_output_elapsed"); + perf_counter_t _perfcnt_esc_mixer_total_elapsed = perf_alloc(PC_ELAPSED, "uavcan_esc_mixer_total_elapsed"); + perf_counter_t _perfcnt_esc_mixer_subscriptions = perf_alloc(PC_COUNT, "uavcan_esc_mixer_subscriptions"); }; From 7f0f4b3072008239f4481c79769ab1b00f92e707 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 11 Jan 2015 00:11:48 +0300 Subject: [PATCH 554/652] UavcanEscController broadcasting percounter --- src/modules/uavcan/actuators/esc.cpp | 14 ++++++++++++++ src/modules/uavcan/actuators/esc.hpp | 1 + 2 files changed, 15 insertions(+) diff --git a/src/modules/uavcan/actuators/esc.cpp b/src/modules/uavcan/actuators/esc.cpp index 9f682c7e16..995c8987c0 100644 --- a/src/modules/uavcan/actuators/esc.cpp +++ b/src/modules/uavcan/actuators/esc.cpp @@ -49,12 +49,24 @@ UavcanEscController::UavcanEscController(uavcan::INode &node) : _uavcan_sub_status(node), _orb_timer(node) { + if (_perfcnt_invalid_input == nullptr) { + errx(1, "uavcan: couldn't allocate _perfcnt_invalid_input"); + } + + if (_perfcnt_scaling_error == nullptr) { + errx(1, "uavcan: couldn't allocate _perfcnt_scaling_error"); + } + + if (_perfcnt_broadcast_elapsed == nullptr) { + errx(1, "uavcan: couldn't allocate _perfcnt_broadcast_elapsed"); + } } UavcanEscController::~UavcanEscController() { perf_free(_perfcnt_invalid_input); perf_free(_perfcnt_scaling_error); + perf_free(_perfcnt_broadcast_elapsed); } int UavcanEscController::init() @@ -129,7 +141,9 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs) * Publish the command message to the bus * Note that for a quadrotor it takes one CAN frame */ + perf_begin(_perfcnt_broadcast_elapsed); (void)_uavcan_pub_raw_cmd.broadcast(msg); + perf_end(_perfcnt_broadcast_elapsed); } void UavcanEscController::arm_all_escs(bool arm) diff --git a/src/modules/uavcan/actuators/esc.hpp b/src/modules/uavcan/actuators/esc.hpp index 12c0355422..498fb9dd8b 100644 --- a/src/modules/uavcan/actuators/esc.hpp +++ b/src/modules/uavcan/actuators/esc.hpp @@ -109,4 +109,5 @@ private: */ perf_counter_t _perfcnt_invalid_input = perf_alloc(PC_COUNT, "uavcan_esc_invalid_input"); perf_counter_t _perfcnt_scaling_error = perf_alloc(PC_COUNT, "uavcan_esc_scaling_error"); + perf_counter_t _perfcnt_broadcast_elapsed = perf_alloc(PC_ELAPSED, "uavcan_esc_broadcast_elapsed"); }; From 0901d999924012b482a5379854c24eb9f2c6f345 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 11 Jan 2015 04:38:20 +0300 Subject: [PATCH 555/652] UAVCAN update (speed optimizations) --- src/lib/uavcan | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/uavcan b/src/lib/uavcan index c4c45b995f..20439bb397 160000 --- a/src/lib/uavcan +++ b/src/lib/uavcan @@ -1 +1 @@ -Subproject commit c4c45b995f5c8192c7a36c4293c201711ceac74d +Subproject commit 20439bb3975c34a24c3c337a1f231fbf973a41e8 From 9215a8d8dafb3807852e10a3dab150c1fc6fc6f4 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Thu, 15 Jan 2015 00:11:55 +0300 Subject: [PATCH 556/652] Nuttx.py fixes: Python 2.7 support (required for gcc-arm-toolchain from Terry Guo), fixed int parsing, show mybt by name --- Debug/Nuttx.py | 51 ++++++++++++++++++++++++++++++-------------------- 1 file changed, 31 insertions(+), 20 deletions(-) diff --git a/Debug/Nuttx.py b/Debug/Nuttx.py index cf86dd668f..f915d3bcbb 100644 --- a/Debug/Nuttx.py +++ b/Debug/Nuttx.py @@ -1,7 +1,10 @@ # GDB/Python functions for dealing with NuttX +from __future__ import print_function import gdb, gdb.types +parse_int = lambda x: int(str(x), 0) + class NX_register_set(object): """Copy of the registers for a given context""" @@ -155,7 +158,7 @@ class NX_task(object): result = [] for i in range(pidhash_type.range()[0],pidhash_type.range()[1]): entry = pidhash_value[i] - pid = int(entry['pid']) + pid = parse_int(entry['pid']) if pid is not -1: result.append(pid) return result @@ -184,7 +187,7 @@ class NX_task(object): self.__dict__['stack_used'] = 0 else: stack_limit = self._tcb['adj_stack_size'] - for offset in range(0, int(stack_limit)): + for offset in range(0, parse_int(stack_limit)): if stack_base[offset] != 0xff: break self.__dict__['stack_used'] = stack_limit - offset @@ -244,7 +247,7 @@ class NX_task(object): filearray = filelist['fl_files'] result = dict() for i in range(filearray.type.range()[0],filearray.type.range()[1]): - inode = int(filearray[i]['f_inode']) + inode = parse_int(filearray[i]['f_inode']) if inode != 0: result[i] = inode return result @@ -299,7 +302,7 @@ class NX_show_task (gdb.Command): super(NX_show_task, self).__init__("show task", gdb.COMMAND_USER) def invoke(self, arg, from_tty): - t = NX_task.for_pid(int(arg)) + t = NX_task.for_pid(parse_int(arg)) if t is not None: my_fmt = 'PID:{pid} name:{name} state:{state}\n' my_fmt += ' stack used {stack_used} of {stack_limit}\n' @@ -435,12 +438,12 @@ class NX_tcb(object): first_tcb = tcb_ptr.dereference() tcb_list.append(first_tcb); next_tcb = first_tcb['flink'].dereference() - while not self.is_in(int(next_tcb['pid']),[int(t['pid']) for t in tcb_list]): + while not self.is_in(parse_int(next_tcb['pid']),[parse_int(t['pid']) for t in tcb_list]): tcb_list.append(next_tcb); old_tcb = next_tcb; next_tcb = old_tcb['flink'].dereference() - return [t for t in tcb_list if int(t['pid'])<2000] + return [t for t in tcb_list if parse_int(t['pid'])<2000] def getTCB(self): list_of_listsnames = ['g_pendingtasks','g_readytorun','g_waitingforsemaphore','g_waitingforsignal','g_inactivetasks'] @@ -469,12 +472,12 @@ class NX_check_stack_order(gdb.Command): first_tcb = tcb_ptr.dereference() tcb_list.append(first_tcb); next_tcb = first_tcb['flink'].dereference() - while not self.is_in(int(next_tcb['pid']),[int(t['pid']) for t in tcb_list]): + while not self.is_in(parse_int(next_tcb['pid']),[parse_int(t['pid']) for t in tcb_list]): tcb_list.append(next_tcb); old_tcb = next_tcb; next_tcb = old_tcb['flink'].dereference() - return [t for t in tcb_list if int(t['pid'])<2000] + return [t for t in tcb_list if parse_int(t['pid'])<2000] def getTCB(self): list_of_listsnames = ['g_pendingtasks','g_readytorun','g_waitingforsemaphore','g_waitingforsignal','g_inactivetasks'] @@ -488,7 +491,7 @@ class NX_check_stack_order(gdb.Command): def getSPfromTask(self,tcb): regmap = NX_register_set.v7em_regmap a =tcb['xcp']['regs'] - return int(a[regmap['SP']]) + return parse_int(a[regmap['SP']]) def find_closest(self,list,val): tmp_list = [abs(i-val) for i in list] @@ -525,8 +528,8 @@ class NX_check_stack_order(gdb.Command): for t in tcb: p = []; #print(t.name,t._tcb['stack_alloc_ptr']) - p.append(int(t['stack_alloc_ptr'])) - p.append(int(t['adj_stack_ptr'])) + p.append(parse_int(t['stack_alloc_ptr'])) + p.append(parse_int(t['adj_stack_ptr'])) p.append(self.getSPfromTask(t)) stackadresses[str(t['name'])] = p; address = int("0x30000000",0) @@ -594,12 +597,12 @@ class NX_search_tcb(gdb.Command): first_tcb = tcb_ptr.dereference() tcb_list.append(first_tcb); next_tcb = first_tcb['flink'].dereference() - while not self.is_in(int(next_tcb['pid']),[int(t['pid']) for t in tcb_list]): + while not self.is_in(parse_int(next_tcb['pid']),[parse_int(t['pid']) for t in tcb_list]): tcb_list.append(next_tcb); old_tcb = next_tcb; next_tcb = old_tcb['flink'].dereference() - return [t for t in tcb_list if int(t['pid'])<2000] + return [t for t in tcb_list if parse_int(t['pid'])<2000] def invoke(self,args,sth): list_of_listsnames = ['g_pendingtasks','g_readytorun','g_waitingforsemaphore','g_waitingforsignal','g_inactivetasks'] @@ -612,7 +615,7 @@ class NX_search_tcb(gdb.Command): # filter for tasks that are listed twice tasks_filt = {} for t in tasks: - pid = int(t['pid']); + pid = parse_int(t['pid']); if not pid in tasks_filt.keys(): tasks_filt[pid] = t['name']; print('{num_t} Tasks found:'.format(num_t = len(tasks_filt))) @@ -687,13 +690,21 @@ class NX_my_bt(gdb.Command): def invoke(self,args,sth): - addr_dec = int(args[2:],16) - _tcb = self.get_tcb_from_address(addr_dec) + try: + addr_dec = parse_int(args) # Trying to interpret the input as TCB address + except ValueError: + for task in NX_task.tasks(): # Interpreting as a task name + if task.name == args: + _tcb = task._tcb + break + else: + _tcb = self.get_tcb_from_address(addr_dec) + print("found task with PID: ",_tcb["pid"]) - up_stack = int(_tcb['adj_stack_ptr']) - curr_sp = int(_tcb['xcp']['regs'][0]) #curr stack pointer - other_sp = int(_tcb['xcp']['regs'][8]) # other stack pointer - stacksize = int(_tcb['adj_stack_size']) # other stack pointer + up_stack = parse_int(_tcb['adj_stack_ptr']) + curr_sp = parse_int(_tcb['xcp']['regs'][0]) #curr stack pointer + other_sp = parse_int(_tcb['xcp']['regs'][8]) # other stack pointer + stacksize = parse_int(_tcb['adj_stack_size']) # other stack pointer print("tasks current SP = ",hex(curr_sp),"stack max ptr is at ",hex(up_stack)) From e41bf13ac57081342a94f48a468e3f6f24404f8b Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Thu, 15 Jan 2015 00:49:32 +0300 Subject: [PATCH 557/652] Nuttx.py: 'show mybt' outputs in GDB-like format for ease of parsing --- Debug/Nuttx.py | 37 ++++++++----------------------------- 1 file changed, 8 insertions(+), 29 deletions(-) diff --git a/Debug/Nuttx.py b/Debug/Nuttx.py index f915d3bcbb..826e12c97c 100644 --- a/Debug/Nuttx.py +++ b/Debug/Nuttx.py @@ -656,38 +656,20 @@ class NX_my_bt(gdb.Command): tcb_ptr = addr_value.cast(gdb.lookup_type('struct tcb_s').pointer()) return tcb_ptr.dereference() - def print_instruction_at(self,addr,stack_percentage): + def resolve_file_line_func(self,addr,stack_percentage): gdb.write(str(round(stack_percentage,2))+":") str_to_eval = "info line *"+hex(addr) #gdb.execute(str_to_eval) res = gdb.execute(str_to_eval,to_string = True) # get information from results string: words = res.split() - valid = False - if words[0] == 'No': - #no line info... - pass - else: - valid = True + if words[0] != 'No': line = int(words[1]) - idx = words[3].rfind("/"); #find first backslash - if idx>0: - name = words[3][idx+1:]; - path = words[3][:idx]; - else: - name = words[3]; - path = ""; block = gdb.block_for_pc(addr) func = block.function if str(func) == "None": func = block.superblock.function - - if valid: - print("Line: ",line," in ",path,"/",name,"in ",func) - return name,path,line,func - - - + return words[3].strip('"'), line, func def invoke(self,args,sth): try: @@ -708,18 +690,15 @@ class NX_my_bt(gdb.Command): print("tasks current SP = ",hex(curr_sp),"stack max ptr is at ",hex(up_stack)) - if curr_sp == up_stack: - sp = other_sp - else: - sp = curr_sp; - - while(sp < up_stack): + item = 0 + for sp in range(other_sp if curr_sp == up_stack else curr_sp, up_stack, 4): mem = self.readmem(sp) #print(hex(sp)," : ",hex(mem)) if self.is_in_bounds(mem): # this is a potential instruction ptr stack_percentage = (up_stack-sp)/stacksize - name,path,line,func = self.print_instruction_at(mem,stack_percentage) - sp = sp + 4; # jump up one word + filename,line,func = self.resolve_file_line_func(mem, stack_percentage) + print('#%-2d ' % item, '0x%08x in ' % mem, func, ' at ', filename, ':', line, sep='') + item += 1 NX_my_bt() From dd3fa2532e63118bfd076e1027d74af2d06b9e05 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Thu, 15 Jan 2015 02:44:54 +0300 Subject: [PATCH 558/652] Poor man's sampling profiler for NuttX --- Debug/poor-mans-profiler.sh | 66 +++++++++++++++++++++++++++++++++++++ 1 file changed, 66 insertions(+) create mode 100755 Debug/poor-mans-profiler.sh diff --git a/Debug/poor-mans-profiler.sh b/Debug/poor-mans-profiler.sh new file mode 100755 index 0000000000..f706fad853 --- /dev/null +++ b/Debug/poor-mans-profiler.sh @@ -0,0 +1,66 @@ +#!/bin/bash +# +# Poor man's sampling profiler for NuttX. +# +# The stack folding script was inspired by stackcollapse-gdb.pl from the FlameGraph project. +# +# Usage: Install flamegraph.pl in your PATH, define the variables below, configure your .gdbinit, run the script and go +# have a coffee. When you're back, you'll see the flamegraph. Note that frequent calls to GDB significantly +# interfere with normal operation of the target, which means that you can't profile real-time tasks with it. +# +# Requirements: ARM GDB with Python support +# + +root=$(dirname $0)/.. + +nsamples=100 +sleeptime=0.01 # Doctors recommend 7-8 hours a day +taskname=uavcan +exe=$root/Build/px4fmu-v2_default.build/firmware.elf + +set -e + +stacksfile=/tmp/$taskname-stacks.log + +cd $root +rm -f $stacksfile +echo "Sampling..." + +for x in $(seq 1 $nsamples) +do + arm-none-eabi-gdb $exe --batch -ex "set print asm-demangle on" \ + -ex "source $root/Debug/Nuttx.py" \ + -ex "show mybt $taskname" \ + 2> /dev/null \ + | sed -n 's/0\.0:\(#.*\)/\1/p' \ + >> $stacksfile + echo -e '\n\n' >> $stacksfile + echo -ne "\r$x/$nsamples" + sleep $sleeptime +done + +echo +echo "Stacks saved to $stacksfile" + +cat $stacksfile | perl -e 'my $current = ""; +my %stacks; +while(<>) { + if(m/^#[0-9]*\s*0x[a-zA-Z0-9]*\s*in (.*) at/) { + if ($1 ne "None") { + if ($current eq "") { $current = $1; } + else { $current = $1 . ";" . $current; } + } + } elsif(!($current eq "")) { + $stacks{$current} += 1; + $current = ""; + } +} +foreach my $k (sort { $a cmp $b } keys %stacks) { + print "$k $stacks{$k}\n"; +}' > /tmp/$taskname-folded.txt + +echo "Folded stacks saved to /tmp/$taskname-folded.txt" + +cat /tmp/$taskname-folded.txt | flamegraph.pl --fontsize=8 --width=1800 > /tmp/$taskname-flamegraph.svg + +xdg-open /tmp/$taskname-flamegraph.svg From 9293950cdb17af28e344f710f9121e606710a006 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 16 Jan 2015 20:47:29 +0300 Subject: [PATCH 559/652] NuttX profiler improvements --- Debug/poor-mans-profiler.sh | 136 +++++++++++++++++++++++++++--------- 1 file changed, 102 insertions(+), 34 deletions(-) diff --git a/Debug/poor-mans-profiler.sh b/Debug/poor-mans-profiler.sh index f706fad853..1d76eaa445 100755 --- a/Debug/poor-mans-profiler.sh +++ b/Debug/poor-mans-profiler.sh @@ -11,45 +11,111 @@ # Requirements: ARM GDB with Python support # +set -e root=$(dirname $0)/.. -nsamples=100 -sleeptime=0.01 # Doctors recommend 7-8 hours a day -taskname=uavcan -exe=$root/Build/px4fmu-v2_default.build/firmware.elf +# +# Parsing the arguments. Read this section for usage info. +# +nsamples=0 +sleeptime=0.1 # Doctors recommend 7-8 hours a day +taskname= +elf=$root/Build/px4fmu-v2_default.build/firmware.elf +append=0 +fgfontsize=5 +fgwidth=1900 -set -e +function usage() +{ + echo "Invalid usage. Supported options:" + cat $0 | sed -n 's/^\s*--\([^)\*]*\).*/\1/p' # Don't try this at home. + exit 1 +} -stacksfile=/tmp/$taskname-stacks.log - -cd $root -rm -f $stacksfile -echo "Sampling..." - -for x in $(seq 1 $nsamples) +for i in "$@" do - arm-none-eabi-gdb $exe --batch -ex "set print asm-demangle on" \ - -ex "source $root/Debug/Nuttx.py" \ - -ex "show mybt $taskname" \ - 2> /dev/null \ - | sed -n 's/0\.0:\(#.*\)/\1/p' \ - >> $stacksfile - echo -e '\n\n' >> $stacksfile - echo -ne "\r$x/$nsamples" - sleep $sleeptime + case $i in + --nsamples=*) + nsamples="${i#*=}" + ;; + --sleeptime=*) + sleeptime="${i#*=}" + ;; + --taskname=*) + taskname="${i#*=}" + ;; + --elf=*) + elf="${i#*=}" + ;; + --append) + append=1 + ;; + --fgfontsize=*) + fgfontsize="${i#*=}" + ;; + --fgwidth=*) + fgwidth="${i#*=}" + ;; + *) + usage + ;; + esac + shift done -echo -echo "Stacks saved to $stacksfile" +# +# Temporary files +# +stacksfile=/tmp/pmpn-stacks.log +foldfile=/tmp/pmpn-folded.txt +graphfile=/tmp/pmpn-flamegraph.svg -cat $stacksfile | perl -e 'my $current = ""; +# +# Sampling if requested. Note that if $append is true, the stack file will not be rewritten. +# +cd $root + +if [[ $nsamples > 0 && "$taskname" != "" ]] +then + [[ $append = 0 ]] && (rm -f $stacksfile; echo "Old stacks removed") + + echo "Sampling..." + + for x in $(seq 1 $nsamples) + do + arm-none-eabi-gdb $exe --batch -ex "set print asm-demangle on" \ + -ex "source $root/Debug/Nuttx.py" \ + -ex "show mybt $taskname" \ + 2> /dev/null \ + | sed -n 's/0\.0:\(#.*\)/\1/p' \ + >> $stacksfile + echo -e '\n\n' >> $stacksfile + echo -ne "\r$x/$nsamples" + sleep $sleeptime + done + + echo + echo "Stacks saved to $stacksfile" +else + echo "Sampling skipped - set 'nsamples' and 'taskname' to re-sample." +fi + +# +# Folding the stacks. +# +if [ ! -f $stacksfile ]; then + echo "Where are the stack samples?" + exit 1 +fi + +cat $stacksfile | perl -e 'use File::Basename; +my $current = ""; my %stacks; while(<>) { - if(m/^#[0-9]*\s*0x[a-zA-Z0-9]*\s*in (.*) at/) { - if ($1 ne "None") { - if ($current eq "") { $current = $1; } - else { $current = $1 . ";" . $current; } - } + if(m/^#[0-9]*\s*0x[a-zA-Z0-9]*\s*in (.*) at (.*)/) { + my $x = $1 eq "None" ? basename($2) : $1; + if ($current eq "") { $current = $x; } + else { $current = $x . ";" . $current; } } elsif(!($current eq "")) { $stacks{$current} += 1; $current = ""; @@ -57,10 +123,12 @@ while(<>) { } foreach my $k (sort { $a cmp $b } keys %stacks) { print "$k $stacks{$k}\n"; -}' > /tmp/$taskname-folded.txt +}' > $foldfile -echo "Folded stacks saved to /tmp/$taskname-folded.txt" +echo "Folded stacks saved to $foldfile" -cat /tmp/$taskname-folded.txt | flamegraph.pl --fontsize=8 --width=1800 > /tmp/$taskname-flamegraph.svg - -xdg-open /tmp/$taskname-flamegraph.svg +# +# Graphing. +# +cat $foldfile | flamegraph.pl --fontsize=$fgfontsize --width=$fgwidth > $graphfile +xdg-open $graphfile From c0937ec8cac523d4c8fad028584e2b87956f3019 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 16 Jan 2015 21:00:48 +0300 Subject: [PATCH 560/652] Profiler fixes --- Debug/poor-mans-profiler.sh | 27 ++++++++++++++++----------- 1 file changed, 16 insertions(+), 11 deletions(-) diff --git a/Debug/poor-mans-profiler.sh b/Debug/poor-mans-profiler.sh index 1d76eaa445..d2393b296e 100755 --- a/Debug/poor-mans-profiler.sh +++ b/Debug/poor-mans-profiler.sh @@ -14,6 +14,21 @@ set -e root=$(dirname $0)/.. +function die() +{ + echo "$@" + exit 1 +} + +function usage() +{ + echo "Invalid usage. Supported options:" + cat $0 | sed -n 's/^\s*--\([^)\*]*\).*/\1/p' # Don't try this at home. + exit 1 +} + +which flamegraph.pl > /dev/null || die "Install flamegraph.pl first" + # # Parsing the arguments. Read this section for usage info. # @@ -25,13 +40,6 @@ append=0 fgfontsize=5 fgwidth=1900 -function usage() -{ - echo "Invalid usage. Supported options:" - cat $0 | sed -n 's/^\s*--\([^)\*]*\).*/\1/p' # Don't try this at home. - exit 1 -} - for i in "$@" do case $i in @@ -103,10 +111,7 @@ fi # # Folding the stacks. # -if [ ! -f $stacksfile ]; then - echo "Where are the stack samples?" - exit 1 -fi +[ -f $stacksfile ] || die "Where are the stack samples?" cat $stacksfile | perl -e 'use File::Basename; my $current = ""; From c0d71529bce816596dc3574e876e0f4c69bc9b9f Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 16 Jan 2015 21:13:10 +0300 Subject: [PATCH 561/652] Profiler fixes --- Debug/poor-mans-profiler.sh | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/Debug/poor-mans-profiler.sh b/Debug/poor-mans-profiler.sh index d2393b296e..eede0072d7 100755 --- a/Debug/poor-mans-profiler.sh +++ b/Debug/poor-mans-profiler.sh @@ -77,6 +77,7 @@ done stacksfile=/tmp/pmpn-stacks.log foldfile=/tmp/pmpn-folded.txt graphfile=/tmp/pmpn-flamegraph.svg +gdberrfile=/tmp/pmpn-gdberr.log # # Sampling if requested. Note that if $append is true, the stack file will not be rewritten. @@ -87,14 +88,14 @@ if [[ $nsamples > 0 && "$taskname" != "" ]] then [[ $append = 0 ]] && (rm -f $stacksfile; echo "Old stacks removed") - echo "Sampling..." + echo "Sampling the task '$taskname'..." for x in $(seq 1 $nsamples) do - arm-none-eabi-gdb $exe --batch -ex "set print asm-demangle on" \ - -ex "source $root/Debug/Nuttx.py" \ - -ex "show mybt $taskname" \ - 2> /dev/null \ + arm-none-eabi-gdb $elf --batch -ex "set print asm-demangle on" \ + -ex "source $root/Debug/Nuttx.py" \ + -ex "show mybt $taskname" \ + 2> $gdberrfile \ | sed -n 's/0\.0:\(#.*\)/\1/p' \ >> $stacksfile echo -e '\n\n' >> $stacksfile From ff7c33a4b04e0545dc78a324cbc8f9a945f97519 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 16 Jan 2015 21:40:14 +0300 Subject: [PATCH 562/652] Profiler: xdg-open work-around --- Debug/poor-mans-profiler.sh | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/Debug/poor-mans-profiler.sh b/Debug/poor-mans-profiler.sh index eede0072d7..2b25337565 100755 --- a/Debug/poor-mans-profiler.sh +++ b/Debug/poor-mans-profiler.sh @@ -37,7 +37,7 @@ sleeptime=0.1 # Doctors recommend 7-8 hours a day taskname= elf=$root/Build/px4fmu-v2_default.build/firmware.elf append=0 -fgfontsize=5 +fgfontsize=10 fgwidth=1900 for i in "$@" @@ -137,4 +137,12 @@ echo "Folded stacks saved to $foldfile" # Graphing. # cat $foldfile | flamegraph.pl --fontsize=$fgfontsize --width=$fgwidth > $graphfile -xdg-open $graphfile +echo "FlameGraph saved to $graphfile" + +# On KDE, xdg-open prefers Gwenview by default, which doesn't handle interactive SVGs, so we need a browser. +# The current implementation is hackish and stupid. Somebody, please do something about it. +opener=xdg-open +which firefox > /dev/null && opener=firefox +which google-chrome > /dev/null && opener=google-chrome + +$opener $graphfile From 1898b51c740dba2b3dd979980775f25c982dea1e Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 16 Jan 2015 22:08:46 +0300 Subject: [PATCH 563/652] Profiler: reporting function position in flame graphs --- Debug/poor-mans-profiler.sh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Debug/poor-mans-profiler.sh b/Debug/poor-mans-profiler.sh index 2b25337565..460b28d0ad 100755 --- a/Debug/poor-mans-profiler.sh +++ b/Debug/poor-mans-profiler.sh @@ -119,7 +119,7 @@ my $current = ""; my %stacks; while(<>) { if(m/^#[0-9]*\s*0x[a-zA-Z0-9]*\s*in (.*) at (.*)/) { - my $x = $1 eq "None" ? basename($2) : $1; + my $x = $1 eq "None" ? basename($2) : ("$1 at " . basename($2)); if ($current eq "") { $current = $x; } else { $current = $x . ";" . $current; } } elsif(!($current eq "")) { From d1abf9c133bfe865f9648a88610d92a78f787f21 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 17 Jan 2015 02:25:26 +0300 Subject: [PATCH 564/652] Poor man's profiler - proper stack folder that handles generic C++ types --- Debug/poor-mans-profiler.sh | 137 +++++++++++++++++++++++++++++------- 1 file changed, 113 insertions(+), 24 deletions(-) diff --git a/Debug/poor-mans-profiler.sh b/Debug/poor-mans-profiler.sh index 460b28d0ad..7657e4dc7d 100755 --- a/Debug/poor-mans-profiler.sh +++ b/Debug/poor-mans-profiler.sh @@ -84,7 +84,7 @@ gdberrfile=/tmp/pmpn-gdberr.log # cd $root -if [[ $nsamples > 0 && "$taskname" != "" ]] +if [[ $nsamples > 0 ]] then [[ $append = 0 ]] && (rm -f $stacksfile; echo "Old stacks removed") @@ -92,12 +92,20 @@ then for x in $(seq 1 $nsamples) do - arm-none-eabi-gdb $elf --batch -ex "set print asm-demangle on" \ - -ex "source $root/Debug/Nuttx.py" \ - -ex "show mybt $taskname" \ - 2> $gdberrfile \ - | sed -n 's/0\.0:\(#.*\)/\1/p' \ - >> $stacksfile + if [[ "$taskname" = "" ]] + then + arm-none-eabi-gdb $elf --batch -ex "set print asm-demangle on" -ex bt \ + 2> $gdberrfile \ + | sed -n 's/\(#.*\)/\1/p' \ + >> $stacksfile + else + arm-none-eabi-gdb $elf --batch -ex "set print asm-demangle on" \ + -ex "source $root/Debug/Nuttx.py" \ + -ex "show mybt $taskname" \ + 2> $gdberrfile \ + | sed -n 's/0\.0:\(#.*\)/\1/p' \ + >> $stacksfile + fi echo -e '\n\n' >> $stacksfile echo -ne "\r$x/$nsamples" sleep $sleeptime @@ -106,7 +114,7 @@ then echo echo "Stacks saved to $stacksfile" else - echo "Sampling skipped - set 'nsamples' and 'taskname' to re-sample." + echo "Sampling skipped - set 'nsamples' to re-sample." fi # @@ -114,22 +122,103 @@ fi # [ -f $stacksfile ] || die "Where are the stack samples?" -cat $stacksfile | perl -e 'use File::Basename; -my $current = ""; -my %stacks; -while(<>) { - if(m/^#[0-9]*\s*0x[a-zA-Z0-9]*\s*in (.*) at (.*)/) { - my $x = $1 eq "None" ? basename($2) : ("$1 at " . basename($2)); - if ($current eq "") { $current = $x; } - else { $current = $x . ";" . $current; } - } elsif(!($current eq "")) { - $stacks{$current} += 1; - $current = ""; - } -} -foreach my $k (sort { $a cmp $b } keys %stacks) { - print "$k $stacks{$k}\n"; -}' > $foldfile +cat $stacksfile | python -c " +# +# This stack folder correctly handles C++ types. +# + +from __future__ import print_function, division +import fileinput, collections, os + +def enforce(x, msg='Invalid input'): + if not x: + raise Exception(msg) + +def split_first_part_with_parens(line): + LBRACES = {'(':'()', '<':'<>', '[':'[]', '{':'{}'} + RBRACES = {')':'()', '>':'<>', ']':'[]', '}':'{}'} + braces = collections.defaultdict(int) + out = '' + for ch in line: + out += ch + # special cases + if out.endswith('operator>') or out.endswith('operator->'): # gotta love c++ + braces['<>'] += 1 + if out.endswith('operator<'): + braces['<>'] -= 1 + # counting parens + if ch in LBRACES.keys(): + braces[LBRACES[ch]] += 1 + if ch in RBRACES.keys(): + braces[RBRACES[ch]] -= 1 + # sanity check + for v in braces.values(): + enforce(v >= 0, 'Unaligned braces: ' + str(dict(braces))) + # termination condition + if ch == ' ' and sum(braces.values()) == 0: + break + out = out.strip() + return out, line[len(out):] + +def parse(line): + def take_path(line, output): + line = line.strip() + if line.startswith('at '): + line = line[3:].strip() + if line: + output['file_full_path'] = line.rsplit(':', 1)[0].strip() + output['file_base_name'] = os.path.basename(output['file_full_path']) + output['line'] = int(line.rsplit(':', 1)[1]) + return output + + def take_args(line, output): + line = line.lstrip() + if line[0] == '(': + output['args'], line = split_first_part_with_parens(line) + return take_path(line.lstrip(), output) + + def take_function(line, output): + output['function'], line = split_first_part_with_parens(line.lstrip()) + return take_args(line.lstrip(), output) + + def take_mem_loc(line, output): + line = line.lstrip() + if line.startswith('0x'): + end = line.find(' ') + num = line[:end] + output['memloc'] = int(num, 16) + line = line[end:].lstrip() + end = line.find(' ') + enforce(line[:end] == 'in') + line = line[end:].lstrip() + return take_function(line, output) + + def take_frame_num(line, output): + line = line.lstrip() + enforce(line[0] == '#') + end = line.find(' ') + num = line[1:end] + output['frame_num'] = int(num) + return take_mem_loc(line[end:], output) + + return take_frame_num(line, {}) + +stacks = collections.defaultdict(int) +current = '' + +for line in fileinput.input(): + line = line.strip() + if line: + inf = parse(line) + fun = inf['function'] + current = (fun + ';' + current) if current else fun + elif current: + stacks[current] += 1 + current = '' + +for s, f in sorted(stacks.items(), key=lambda (s, f): s): + print(s, f) +" > $foldfile echo "Folded stacks saved to $foldfile" From f158c8270b7572b3f3384a52cb6f6c51ecd7ec57 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 17 Jan 2015 02:58:07 +0300 Subject: [PATCH 565/652] Rich man's profiler - handling quotes --- Debug/poor-mans-profiler.sh | 49 +++++++++++++++++++++++-------------- 1 file changed, 31 insertions(+), 18 deletions(-) diff --git a/Debug/poor-mans-profiler.sh b/Debug/poor-mans-profiler.sh index 7657e4dc7d..89ff4f424b 100755 --- a/Debug/poor-mans-profiler.sh +++ b/Debug/poor-mans-profiler.sh @@ -122,13 +122,12 @@ fi # [ -f $stacksfile ] || die "Where are the stack samples?" -cat $stacksfile | python -c " +cat << 'EOF' > /tmp/pmpn-folder.py # # This stack folder correctly handles C++ types. # - from __future__ import print_function, division -import fileinput, collections, os +import fileinput, collections, os, sys def enforce(x, msg='Invalid input'): if not x: @@ -137,20 +136,29 @@ def enforce(x, msg='Invalid input'): def split_first_part_with_parens(line): LBRACES = {'(':'()', '<':'<>', '[':'[]', '{':'{}'} RBRACES = {')':'()', '>':'<>', ']':'[]', '}':'{}'} + QUOTES = set(['"', "'"]) + quotes = collections.defaultdict(bool) braces = collections.defaultdict(int) out = '' for ch in line: out += ch + # escape character cancels further processing + if ch == '\\': + continue # special cases if out.endswith('operator>') or out.endswith('operator->'): # gotta love c++ braces['<>'] += 1 if out.endswith('operator<'): braces['<>'] -= 1 - # counting parens - if ch in LBRACES.keys(): - braces[LBRACES[ch]] += 1 - if ch in RBRACES.keys(): - braces[RBRACES[ch]] -= 1 + # switching quotes + if ch in QUOTES: + quotes[ch] = not quotes[ch] + # counting parens only when outside quotes + if sum(quotes.values()) == 0: + if ch in LBRACES.keys(): + braces[LBRACES[ch]] += 1 + if ch in RBRACES.keys(): + braces[RBRACES[ch]] -= 1 # sanity check for v in braces.values(): enforce(v >= 0, 'Unaligned braces: ' + str(dict(braces))) @@ -206,19 +214,24 @@ def parse(line): stacks = collections.defaultdict(int) current = '' -for line in fileinput.input(): - line = line.strip() - if line: - inf = parse(line) - fun = inf['function'] - current = (fun + ';' + current) if current else fun - elif current: - stacks[current] += 1 - current = '' +for idx,line in enumerate(fileinput.input()): + try: + line = line.strip() + if line: + inf = parse(line) + fun = inf['function'] + current = (fun + ';' + current) if current else fun + elif current: + stacks[current] += 1 + current = '' + except Exception, ex: + print('ERROR (line %d):' % (idx + 1), ex, file=sys.stderr) for s, f in sorted(stacks.items(), key=lambda (s, f): s): print(s, f) -" > $foldfile +EOF + +cat $stacksfile | python /tmp/pmpn-folder.py > $foldfile echo "Folded stacks saved to $foldfile" From 647163d6fa4761b0799e073bae8ea368a986b4fe Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 17 Jan 2015 03:06:41 +0300 Subject: [PATCH 566/652] Profiler - header comment fix --- Debug/poor-mans-profiler.sh | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/Debug/poor-mans-profiler.sh b/Debug/poor-mans-profiler.sh index 89ff4f424b..63dba562a8 100755 --- a/Debug/poor-mans-profiler.sh +++ b/Debug/poor-mans-profiler.sh @@ -2,9 +2,7 @@ # # Poor man's sampling profiler for NuttX. # -# The stack folding script was inspired by stackcollapse-gdb.pl from the FlameGraph project. -# -# Usage: Install flamegraph.pl in your PATH, define the variables below, configure your .gdbinit, run the script and go +# Usage: Install flamegraph.pl in your PATH, configure your .gdbinit, run the script with proper arguments and go # have a coffee. When you're back, you'll see the flamegraph. Note that frequent calls to GDB significantly # interfere with normal operation of the target, which means that you can't profile real-time tasks with it. # From 543cb231282fdb899557f42003f53ac2f7a7dcc7 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 17 Jan 2015 03:57:46 +0300 Subject: [PATCH 567/652] Profiler: computing stack top distribution --- Debug/poor-mans-profiler.sh | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/Debug/poor-mans-profiler.sh b/Debug/poor-mans-profiler.sh index 63dba562a8..d907d8749b 100755 --- a/Debug/poor-mans-profiler.sh +++ b/Debug/poor-mans-profiler.sh @@ -212,6 +212,9 @@ def parse(line): stacks = collections.defaultdict(int) current = '' +stack_tops = collections.defaultdict(int) +num_stack_frames = 0 + for idx,line in enumerate(fileinput.input()): try: line = line.strip() @@ -219,6 +222,10 @@ for idx,line in enumerate(fileinput.input()): inf = parse(line) fun = inf['function'] current = (fun + ';' + current) if current else fun + + if inf['frame_num'] == 0: + num_stack_frames += 1 + stack_tops[fun] += 1 elif current: stacks[current] += 1 current = '' @@ -227,6 +234,11 @@ for idx,line in enumerate(fileinput.input()): for s, f in sorted(stacks.items(), key=lambda (s, f): s): print(s, f) + +print('Total stack frames:', num_stack_frames, file=sys.stderr) +print('Top consumers (distribution of the stack tops):', file=sys.stderr) +for name,num in sorted(stack_tops.items(), key=lambda (name, num): num, reverse=True)[:10]: + print('% 5.1f%% ' % (100 * num / num_stack_frames), name, file=sys.stderr) EOF cat $stacksfile | python /tmp/pmpn-folder.py > $foldfile From 6bbacc4271680cc262094af7f9a34807ff5c34ed Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 17 Jan 2015 04:04:31 +0300 Subject: [PATCH 568/652] Intrusive changes made for UAVCAN profiling. Will be reverted in the next commit (this one is needed to keep the changes in history) --- ROMFS/px4fmu_common/init.d/rcS | 5 ++++- makefiles/toolchain_gnu-arm-eabi.mk | 2 +- src/modules/uavcan/actuators/esc.cpp | 10 +++++----- src/modules/uavcan/uavcan_main.cpp | 10 ++++++++++ 4 files changed, 20 insertions(+), 7 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 2c9387ff06..02f4649d5f 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -8,7 +8,7 @@ # # Default to auto-start mode. # -set MODE autostart +set MODE none set FRC /fs/microsd/etc/rc.txt set FCONFIG /fs/microsd/etc/config.txt @@ -656,6 +656,9 @@ then # End of autostart fi +param set UAVCAN_ENABLE 1 +sh /etc/init.d/rc.uavcan + # There is no further processing, so we can free some RAM # XXX potentially unset all script variables. unset TUNE_ERR diff --git a/makefiles/toolchain_gnu-arm-eabi.mk b/makefiles/toolchain_gnu-arm-eabi.mk index fbe378f509..1bbc5437d5 100644 --- a/makefiles/toolchain_gnu-arm-eabi.mk +++ b/makefiles/toolchain_gnu-arm-eabi.mk @@ -105,7 +105,7 @@ ARCHDEFINES += -DCONFIG_ARCH_BOARD_$(CONFIG_BOARD) # optimisation flags # ARCHOPTIMIZATION = $(MAXOPTIMIZATION) \ - -g \ + -g3 \ -fno-strict-aliasing \ -fno-strength-reduce \ -fomit-frame-pointer \ diff --git a/src/modules/uavcan/actuators/esc.cpp b/src/modules/uavcan/actuators/esc.cpp index 995c8987c0..f7adbed760 100644 --- a/src/modules/uavcan/actuators/esc.cpp +++ b/src/modules/uavcan/actuators/esc.cpp @@ -98,11 +98,11 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs) /* * Rate limiting - we don't want to congest the bus */ - const auto timestamp = _node.getMonotonicTime(); - if ((timestamp - _prev_cmd_pub).toUSec() < (1000000 / MAX_RATE_HZ)) { - return; - } - _prev_cmd_pub = timestamp; +// const auto timestamp = _node.getMonotonicTime(); +// if ((timestamp - _prev_cmd_pub).toUSec() < (1000000 / MAX_RATE_HZ)) { +// return; +// } +// _prev_cmd_pub = timestamp; /* * Fill the command message diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index af5f2ec96f..c29cd83230 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -357,6 +357,16 @@ int UavcanNode::run() _actuator_direct_poll_fd_num = add_poll_fd(_actuator_direct_sub); } + _esc_controller.arm_all_escs(true); + while (true) { + for (int i = 0; i < 1000; i++) { + node_spin_once(); + _outputs.noutputs = 8; + _esc_controller.update_outputs(_outputs.output, _outputs.noutputs); + } + ::usleep(1000); + } + while (!_task_should_exit) { // update actuator controls subscriptions if needed if (_groups_subscribed != _groups_required) { From d87bb4dfcb59601bc51d46332bbe0db78d11294a Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 17 Jan 2015 04:04:48 +0300 Subject: [PATCH 569/652] Revert "Intrusive changes made for UAVCAN profiling. Will be reverted in the next commit (this one is needed to keep the changes in history)" This reverts commit 4c301d9dcf180e39186fa6753c7a3d3215b3cfa7. --- ROMFS/px4fmu_common/init.d/rcS | 5 +---- makefiles/toolchain_gnu-arm-eabi.mk | 2 +- src/modules/uavcan/actuators/esc.cpp | 10 +++++----- src/modules/uavcan/uavcan_main.cpp | 10 ---------- 4 files changed, 7 insertions(+), 20 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 02f4649d5f..2c9387ff06 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -8,7 +8,7 @@ # # Default to auto-start mode. # -set MODE none +set MODE autostart set FRC /fs/microsd/etc/rc.txt set FCONFIG /fs/microsd/etc/config.txt @@ -656,9 +656,6 @@ then # End of autostart fi -param set UAVCAN_ENABLE 1 -sh /etc/init.d/rc.uavcan - # There is no further processing, so we can free some RAM # XXX potentially unset all script variables. unset TUNE_ERR diff --git a/makefiles/toolchain_gnu-arm-eabi.mk b/makefiles/toolchain_gnu-arm-eabi.mk index 1bbc5437d5..fbe378f509 100644 --- a/makefiles/toolchain_gnu-arm-eabi.mk +++ b/makefiles/toolchain_gnu-arm-eabi.mk @@ -105,7 +105,7 @@ ARCHDEFINES += -DCONFIG_ARCH_BOARD_$(CONFIG_BOARD) # optimisation flags # ARCHOPTIMIZATION = $(MAXOPTIMIZATION) \ - -g3 \ + -g \ -fno-strict-aliasing \ -fno-strength-reduce \ -fomit-frame-pointer \ diff --git a/src/modules/uavcan/actuators/esc.cpp b/src/modules/uavcan/actuators/esc.cpp index f7adbed760..995c8987c0 100644 --- a/src/modules/uavcan/actuators/esc.cpp +++ b/src/modules/uavcan/actuators/esc.cpp @@ -98,11 +98,11 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs) /* * Rate limiting - we don't want to congest the bus */ -// const auto timestamp = _node.getMonotonicTime(); -// if ((timestamp - _prev_cmd_pub).toUSec() < (1000000 / MAX_RATE_HZ)) { -// return; -// } -// _prev_cmd_pub = timestamp; + const auto timestamp = _node.getMonotonicTime(); + if ((timestamp - _prev_cmd_pub).toUSec() < (1000000 / MAX_RATE_HZ)) { + return; + } + _prev_cmd_pub = timestamp; /* * Fill the command message diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index c29cd83230..af5f2ec96f 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -357,16 +357,6 @@ int UavcanNode::run() _actuator_direct_poll_fd_num = add_poll_fd(_actuator_direct_sub); } - _esc_controller.arm_all_escs(true); - while (true) { - for (int i = 0; i < 1000; i++) { - node_spin_once(); - _outputs.noutputs = 8; - _esc_controller.update_outputs(_outputs.output, _outputs.noutputs); - } - ::usleep(1000); - } - while (!_task_should_exit) { // update actuator controls subscriptions if needed if (_groups_subscribed != _groups_required) { From 569c3b7d37f4be5c9a635fe3e4633ecebbe8f9b5 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 17 Jan 2015 05:10:43 +0300 Subject: [PATCH 570/652] Using -g3 flag instead of -g --- makefiles/toolchain_gnu-arm-eabi.mk | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/makefiles/toolchain_gnu-arm-eabi.mk b/makefiles/toolchain_gnu-arm-eabi.mk index fbe378f509..1bbc5437d5 100644 --- a/makefiles/toolchain_gnu-arm-eabi.mk +++ b/makefiles/toolchain_gnu-arm-eabi.mk @@ -105,7 +105,7 @@ ARCHDEFINES += -DCONFIG_ARCH_BOARD_$(CONFIG_BOARD) # optimisation flags # ARCHOPTIMIZATION = $(MAXOPTIMIZATION) \ - -g \ + -g3 \ -fno-strict-aliasing \ -fno-strength-reduce \ -fomit-frame-pointer \ From 885077a1c3cfa9980001e8dce76615e1f7552788 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 17 Jan 2015 18:32:29 +0300 Subject: [PATCH 571/652] Profiler: folder fix - more special cases for operator<< and operator>> --- Debug/poor-mans-profiler.sh | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Debug/poor-mans-profiler.sh b/Debug/poor-mans-profiler.sh index d907d8749b..ab06a1b66a 100755 --- a/Debug/poor-mans-profiler.sh +++ b/Debug/poor-mans-profiler.sh @@ -144,9 +144,9 @@ def split_first_part_with_parens(line): if ch == '\\': continue # special cases - if out.endswith('operator>') or out.endswith('operator->'): # gotta love c++ + if out.endswith('operator>') or out.endswith('operator>>') or out.endswith('operator->'): # gotta love c++ braces['<>'] += 1 - if out.endswith('operator<'): + if out.endswith('operator<') or out.endswith('operator<<'): braces['<>'] -= 1 # switching quotes if ch in QUOTES: From c2bc298409585aadce4b60dff5e6fada87c6c436 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 17 Jan 2015 20:40:09 +0300 Subject: [PATCH 572/652] Disable instrumentation for the uavcan module --- makefiles/toolchain_gnu-arm-eabi.mk | 2 +- src/modules/uavcan/module.mk | 2 ++ 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/makefiles/toolchain_gnu-arm-eabi.mk b/makefiles/toolchain_gnu-arm-eabi.mk index 1bbc5437d5..5a7f8390ff 100644 --- a/makefiles/toolchain_gnu-arm-eabi.mk +++ b/makefiles/toolchain_gnu-arm-eabi.mk @@ -116,7 +116,7 @@ ARCHOPTIMIZATION = $(MAXOPTIMIZATION) \ # enable precise stack overflow tracking # note - requires corresponding support in NuttX -INSTRUMENTATIONDEFINES = $(ARCHINSTRUMENTATIONDEFINES_$(CONFIG_ARCH)) +INSTRUMENTATIONDEFINES ?= $(ARCHINSTRUMENTATIONDEFINES_$(CONFIG_ARCH)) # Language-specific flags # diff --git a/src/modules/uavcan/module.mk b/src/modules/uavcan/module.mk index e5d30f6c47..64e60bd186 100644 --- a/src/modules/uavcan/module.mk +++ b/src/modules/uavcan/module.mk @@ -40,6 +40,8 @@ MODULE_COMMAND = uavcan MAXOPTIMIZATION = -Os +INSTRUMENTATIONDEFINES = -fno-instrument-functions + # Main SRCS += uavcan_main.cpp \ uavcan_clock.cpp \ From f49f183f74be7d602fee6a1192538273d3d11cac Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 17 Jan 2015 22:25:33 +0300 Subject: [PATCH 573/652] UAVCAN module: -O3 instead of -Os; fixed instrumentation defines --- src/modules/uavcan/module.mk | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/uavcan/module.mk b/src/modules/uavcan/module.mk index 64e60bd186..4d1b7156b8 100644 --- a/src/modules/uavcan/module.mk +++ b/src/modules/uavcan/module.mk @@ -38,9 +38,9 @@ MODULE_COMMAND = uavcan -MAXOPTIMIZATION = -Os +MAXOPTIMIZATION = -O3 -INSTRUMENTATIONDEFINES = -fno-instrument-functions +INSTRUMENTATIONDEFINES = -fno-instrument-functions -ffixed-r10 # Main SRCS += uavcan_main.cpp \ From 1d13edcf9221cfeabfe23eeff046dd7324f532b8 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 17 Jan 2015 22:35:59 +0300 Subject: [PATCH 574/652] Stack checks made optional: ENABLE_STACK_CHECKS --- makefiles/toolchain_gnu-arm-eabi.mk | 16 ++++++++++++---- src/modules/uavcan/module.mk | 3 ++- 2 files changed, 14 insertions(+), 5 deletions(-) diff --git a/makefiles/toolchain_gnu-arm-eabi.mk b/makefiles/toolchain_gnu-arm-eabi.mk index 5a7f8390ff..b38f40eb4c 100644 --- a/makefiles/toolchain_gnu-arm-eabi.mk +++ b/makefiles/toolchain_gnu-arm-eabi.mk @@ -80,14 +80,22 @@ ARCHCPUFLAGS_CORTEXM3 = -mcpu=cortex-m3 \ -march=armv7-m \ -mfloat-abi=soft -ARCHINSTRUMENTATIONDEFINES_CORTEXM4F = -finstrument-functions \ - -ffixed-r10 +ARCHINSTRUMENTATIONDEFINES_CORTEXM4F = -ffixed-r10 -ARCHINSTRUMENTATIONDEFINES_CORTEXM4 = -finstrument-functions \ - -ffixed-r10 +ARCHINSTRUMENTATIONDEFINES_CORTEXM4 = -ffixed-r10 ARCHINSTRUMENTATIONDEFINES_CORTEXM3 = +# Enabling stack checks if requested +# +ENABLE_STACK_CHECKS ?= 0 +ifneq ($(ENABLE_STACK_CHECKS),0) +$(info Stack checks enabled) +ARCHINSTRUMENTATIONDEFINES_CORTEXM4F += -finstrument-functions +ARCHINSTRUMENTATIONDEFINES_CORTEXM4 += -finstrument-functions +ARCHINSTRUMENTATIONDEFINES_CORTEXM3 += +endif + # Pick the right set of flags for the architecture. # ARCHCPUFLAGS = $(ARCHCPUFLAGS_$(CONFIG_ARCH)) diff --git a/src/modules/uavcan/module.mk b/src/modules/uavcan/module.mk index 4d1b7156b8..a3c6e46a0a 100644 --- a/src/modules/uavcan/module.mk +++ b/src/modules/uavcan/module.mk @@ -40,7 +40,8 @@ MODULE_COMMAND = uavcan MAXOPTIMIZATION = -O3 -INSTRUMENTATIONDEFINES = -fno-instrument-functions -ffixed-r10 +# Instrumentation makes the CPU load about 3 times higher, see https://github.com/PX4/Firmware/issues/1417 +INSTRUMENTATIONDEFINES += -fno-instrument-functions # Main SRCS += uavcan_main.cpp \ From 4b1782174c334a9846096145a7156a015df3edae Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 17 Jan 2015 22:40:03 +0300 Subject: [PATCH 575/652] INSTRUMENTATIONDEFINES assignment made non-optional --- makefiles/toolchain_gnu-arm-eabi.mk | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/makefiles/toolchain_gnu-arm-eabi.mk b/makefiles/toolchain_gnu-arm-eabi.mk index b38f40eb4c..7e1f510e6d 100644 --- a/makefiles/toolchain_gnu-arm-eabi.mk +++ b/makefiles/toolchain_gnu-arm-eabi.mk @@ -124,7 +124,7 @@ ARCHOPTIMIZATION = $(MAXOPTIMIZATION) \ # enable precise stack overflow tracking # note - requires corresponding support in NuttX -INSTRUMENTATIONDEFINES ?= $(ARCHINSTRUMENTATIONDEFINES_$(CONFIG_ARCH)) +INSTRUMENTATIONDEFINES = $(ARCHINSTRUMENTATIONDEFINES_$(CONFIG_ARCH)) # Language-specific flags # From ed27e583e01bee74a5bf46561b9aea7bd630846b Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 18 Jan 2015 01:30:46 +0300 Subject: [PATCH 576/652] UAVCAN update --- src/lib/uavcan | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/uavcan b/src/lib/uavcan index 20439bb397..c9227cf6d2 160000 --- a/src/lib/uavcan +++ b/src/lib/uavcan @@ -1 +1 @@ -Subproject commit 20439bb3975c34a24c3c337a1f231fbf973a41e8 +Subproject commit c9227cf6d24e0bafee3ada499dea183446aa35b1 From c9eae96cf67ebdf6d202dc7ecac55a5b4a670a50 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 18 Jan 2015 04:05:50 +0300 Subject: [PATCH 577/652] Frame size fix fix per Lorenz's suggestion --- src/modules/attitude_estimator_ekf/module.mk | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/attitude_estimator_ekf/module.mk b/src/modules/attitude_estimator_ekf/module.mk index 1158536e1d..d158d7a49c 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 -Wframe-larger-than=3600 +EXTRACFLAGS = -Wno-float-equal -Wframe-larger-than=3700 EXTRACXXFLAGS = -Wframe-larger-than=2400 From 2ebd7099de83c603b01bedd278c38a4eb6b77b2b Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 18 Jan 2015 16:09:46 +0300 Subject: [PATCH 578/652] Globally configurable stack checks, R10 is always fixed --- makefiles/toolchain_gnu-arm-eabi.mk | 16 +++++++--------- nuttx-configs/px4fmu-v2/nsh/Make.defs | 6 ++++++ src/modules/uavcan/module.mk | 3 --- 3 files changed, 13 insertions(+), 12 deletions(-) diff --git a/makefiles/toolchain_gnu-arm-eabi.mk b/makefiles/toolchain_gnu-arm-eabi.mk index 7e1f510e6d..7055137ca5 100644 --- a/makefiles/toolchain_gnu-arm-eabi.mk +++ b/makefiles/toolchain_gnu-arm-eabi.mk @@ -80,20 +80,18 @@ ARCHCPUFLAGS_CORTEXM3 = -mcpu=cortex-m3 \ -march=armv7-m \ -mfloat-abi=soft -ARCHINSTRUMENTATIONDEFINES_CORTEXM4F = -ffixed-r10 - -ARCHINSTRUMENTATIONDEFINES_CORTEXM4 = -ffixed-r10 - -ARCHINSTRUMENTATIONDEFINES_CORTEXM3 = - # Enabling stack checks if requested # ENABLE_STACK_CHECKS ?= 0 ifneq ($(ENABLE_STACK_CHECKS),0) $(info Stack checks enabled) -ARCHINSTRUMENTATIONDEFINES_CORTEXM4F += -finstrument-functions -ARCHINSTRUMENTATIONDEFINES_CORTEXM4 += -finstrument-functions -ARCHINSTRUMENTATIONDEFINES_CORTEXM3 += +ARCHINSTRUMENTATIONDEFINES_CORTEXM4F = -finstrument-functions -ffixed-r10 +ARCHINSTRUMENTATIONDEFINES_CORTEXM4 = -finstrument-functions -ffixed-r10 +ARCHINSTRUMENTATIONDEFINES_CORTEXM3 = +else +ARCHINSTRUMENTATIONDEFINES_CORTEXM4F = -ffixed-r10 +ARCHINSTRUMENTATIONDEFINES_CORTEXM4 = -ffixed-r10 +ARCHINSTRUMENTATIONDEFINES_CORTEXM3 = endif # Pick the right set of flags for the architecture. diff --git a/nuttx-configs/px4fmu-v2/nsh/Make.defs b/nuttx-configs/px4fmu-v2/nsh/Make.defs index 99f3b3140d..798d58572d 100644 --- a/nuttx-configs/px4fmu-v2/nsh/Make.defs +++ b/nuttx-configs/px4fmu-v2/nsh/Make.defs @@ -62,8 +62,14 @@ ARCHCPUFLAGS = -mcpu=cortex-m4 \ # enable precise stack overflow tracking +ENABLE_STACK_CHECKS ?= 0 +ifneq ($(ENABLE_STACK_CHECKS),0) +$(info Stack checks enabled) INSTRUMENTATIONDEFINES = -finstrument-functions \ -ffixed-r10 +else +INSTRUMENTATIONDEFINES = -ffixed-r10 +endif # pull in *just* libm from the toolchain ... this is grody LIBM = "${shell $(CC) $(ARCHCPUFLAGS) -print-file-name=libm.a}" diff --git a/src/modules/uavcan/module.mk b/src/modules/uavcan/module.mk index a3c6e46a0a..924b730a23 100644 --- a/src/modules/uavcan/module.mk +++ b/src/modules/uavcan/module.mk @@ -40,9 +40,6 @@ MODULE_COMMAND = uavcan MAXOPTIMIZATION = -O3 -# Instrumentation makes the CPU load about 3 times higher, see https://github.com/PX4/Firmware/issues/1417 -INSTRUMENTATIONDEFINES += -fno-instrument-functions - # Main SRCS += uavcan_main.cpp \ uavcan_clock.cpp \ From f6786d0be91659bfe6cca393d512edd5aa7a407e Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 18 Jan 2015 22:40:22 +0300 Subject: [PATCH 579/652] Removing -ffixed-r10 when stack checks aren't enabled --- makefiles/toolchain_gnu-arm-eabi.mk | 4 ++-- nuttx-configs/px4fmu-v2/nsh/Make.defs | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/makefiles/toolchain_gnu-arm-eabi.mk b/makefiles/toolchain_gnu-arm-eabi.mk index 7055137ca5..0d681cacc8 100644 --- a/makefiles/toolchain_gnu-arm-eabi.mk +++ b/makefiles/toolchain_gnu-arm-eabi.mk @@ -89,8 +89,8 @@ ARCHINSTRUMENTATIONDEFINES_CORTEXM4F = -finstrument-functions -ffixed-r10 ARCHINSTRUMENTATIONDEFINES_CORTEXM4 = -finstrument-functions -ffixed-r10 ARCHINSTRUMENTATIONDEFINES_CORTEXM3 = else -ARCHINSTRUMENTATIONDEFINES_CORTEXM4F = -ffixed-r10 -ARCHINSTRUMENTATIONDEFINES_CORTEXM4 = -ffixed-r10 +ARCHINSTRUMENTATIONDEFINES_CORTEXM4F = +ARCHINSTRUMENTATIONDEFINES_CORTEXM4 = ARCHINSTRUMENTATIONDEFINES_CORTEXM3 = endif diff --git a/nuttx-configs/px4fmu-v2/nsh/Make.defs b/nuttx-configs/px4fmu-v2/nsh/Make.defs index 798d58572d..b2f05293d1 100644 --- a/nuttx-configs/px4fmu-v2/nsh/Make.defs +++ b/nuttx-configs/px4fmu-v2/nsh/Make.defs @@ -68,7 +68,7 @@ $(info Stack checks enabled) INSTRUMENTATIONDEFINES = -finstrument-functions \ -ffixed-r10 else -INSTRUMENTATIONDEFINES = -ffixed-r10 +INSTRUMENTATIONDEFINES = endif # pull in *just* libm from the toolchain ... this is grody From 91362223ea5dc5badc34870210c356eb47ebd59d Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Mon, 19 Jan 2015 15:49:18 +0300 Subject: [PATCH 580/652] Fixed uninitialized fields of UavcanNode --- src/modules/uavcan/uavcan_main.hpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index 8a0993f15d..583c621eb2 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -107,11 +107,11 @@ private: int _task = -1; ///< handle to the OS task bool _task_should_exit = false; ///< flag to indicate to tear down the CAN driver int _armed_sub = -1; ///< uORB subscription of the arming status - actuator_armed_s _armed; ///< the arming request of the system + actuator_armed_s _armed = {}; ///< the arming request of the system bool _is_armed = false; ///< the arming status of the actuators on the bus int _test_motor_sub = -1; ///< uORB subscription of the test_motor status - test_motor_s _test_motor; + test_motor_s _test_motor = {}; bool _test_in_progress = false; unsigned _output_count = 0; ///< number of actuators currently available @@ -135,10 +135,10 @@ private: 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; + uint8_t _actuator_direct_poll_fd_num = 0; + actuator_direct_s _actuator_direct = {}; - actuator_outputs_s _outputs; + actuator_outputs_s _outputs = {}; // index into _poll_fds for each _control_subs handle uint8_t _poll_ids[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN]; From 4baf4a032fa87a01863ae475d4ad27496c90db86 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Mon, 19 Jan 2015 15:54:14 +0300 Subject: [PATCH 581/652] Fixed: Passing this->_armed_sub to close, which cannot accept a negative number. --- src/modules/uavcan/uavcan_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index af5f2ec96f..5b961e2efc 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -513,7 +513,7 @@ UavcanNode::teardown() _control_subs[i] = -1; } } - return ::close(_armed_sub); + return (_armed_sub >= 0) ? ::close(_armed_sub) : 0; } int From f6dc2af3986ba823822525d9865c101d91aa67c3 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Mon, 19 Jan 2015 16:24:07 +0300 Subject: [PATCH 582/652] UAVCAN update --- src/lib/uavcan | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/uavcan b/src/lib/uavcan index c9227cf6d2..8966f97013 160000 --- a/src/lib/uavcan +++ b/src/lib/uavcan @@ -1 +1 @@ -Subproject commit c9227cf6d24e0bafee3ada499dea183446aa35b1 +Subproject commit 8966f970135fb421db31886d6f99ac918594a780 From 4b8feb03cfca89b18ca88a19079e796b44f6d216 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Tue, 20 Jan 2015 17:36:55 -1000 Subject: [PATCH 583/652] Match the OS build's CONFIG_ARMV7M_STACKCHECK setting by using actual setting in the exported nuttx config.h file to control each board build setting of ENABLE_STACK_CHECKS in toolchain_gnu-arm-eabi.mk --- makefiles/setup.mk | 1 + makefiles/toolchain_gnu-arm-eabi.mk | 9 +++++---- nuttx-configs/aerocore/nsh/Make.defs | 5 +++-- nuttx-configs/px4fmu-v1/nsh/Make.defs | 5 +++-- nuttx-configs/px4fmu-v2/nsh/Make.defs | 9 ++------- nuttx-configs/px4fmu-v2/nsh/defconfig | 2 +- nuttx-configs/px4io-v1/nsh/Make.defs | 5 +++++ nuttx-configs/px4io-v2/nsh/Make.defs | 5 +++++ 8 files changed, 25 insertions(+), 16 deletions(-) mode change 100644 => 100755 makefiles/setup.mk mode change 100644 => 100755 makefiles/toolchain_gnu-arm-eabi.mk mode change 100644 => 100755 nuttx-configs/aerocore/nsh/Make.defs mode change 100644 => 100755 nuttx-configs/px4fmu-v1/nsh/Make.defs mode change 100644 => 100755 nuttx-configs/px4fmu-v2/nsh/Make.defs mode change 100644 => 100755 nuttx-configs/px4fmu-v2/nsh/defconfig mode change 100644 => 100755 nuttx-configs/px4io-v1/nsh/Make.defs mode change 100644 => 100755 nuttx-configs/px4io-v2/nsh/Make.defs diff --git a/makefiles/setup.mk b/makefiles/setup.mk old mode 100644 new mode 100755 index 4bfa7a0876..c932a67586 --- a/makefiles/setup.mk +++ b/makefiles/setup.mk @@ -80,6 +80,7 @@ export ECHO = echo export UNZIP_CMD = unzip export PYTHON = python export OPENOCD = openocd +export GREP = grep # # Host-specific paths, hacks and fixups diff --git a/makefiles/toolchain_gnu-arm-eabi.mk b/makefiles/toolchain_gnu-arm-eabi.mk old mode 100644 new mode 100755 index 0d681cacc8..d4d73fb843 --- a/makefiles/toolchain_gnu-arm-eabi.mk +++ b/makefiles/toolchain_gnu-arm-eabi.mk @@ -80,11 +80,12 @@ ARCHCPUFLAGS_CORTEXM3 = -mcpu=cortex-m3 \ -march=armv7-m \ -mfloat-abi=soft -# Enabling stack checks if requested +# Enabling stack checks if OS was build with them # -ENABLE_STACK_CHECKS ?= 0 -ifneq ($(ENABLE_STACK_CHECKS),0) -$(info Stack checks enabled) +TEST_FILE_STACKCHECK=$(WORK_DIR)nuttx-export/include/nuttx/config.h +TEST_VALUE_STACKCHECK=CONFIG_ARMV7M_STACKCHECK\ 1 +ENABLE_STACK_CHECKS=$(shell $(GREP) -q "$(TEST_VALUE_STACKCHECK)" $(TEST_FILE_STACKCHECK); echo $$?;) +ifeq ("$(ENABLE_STACK_CHECKS)","0") ARCHINSTRUMENTATIONDEFINES_CORTEXM4F = -finstrument-functions -ffixed-r10 ARCHINSTRUMENTATIONDEFINES_CORTEXM4 = -finstrument-functions -ffixed-r10 ARCHINSTRUMENTATIONDEFINES_CORTEXM3 = diff --git a/nuttx-configs/aerocore/nsh/Make.defs b/nuttx-configs/aerocore/nsh/Make.defs old mode 100644 new mode 100755 index c1f5a8ac42..3808fc1cfd --- a/nuttx-configs/aerocore/nsh/Make.defs +++ b/nuttx-configs/aerocore/nsh/Make.defs @@ -62,8 +62,9 @@ ARCHCPUFLAGS = -mcpu=cortex-m4 \ # enable precise stack overflow tracking -INSTRUMENTATIONDEFINES = -finstrument-functions \ - -ffixed-r10 +ifeq ($(CONFIG_ARMV7M_STACKCHECK),y) +INSTRUMENTATIONDEFINES = -finstrument-functions -ffixed-r10 +endif # pull in *just* libm from the toolchain ... this is grody LIBM = "${shell $(CC) $(ARCHCPUFLAGS) -print-file-name=libm.a}" diff --git a/nuttx-configs/px4fmu-v1/nsh/Make.defs b/nuttx-configs/px4fmu-v1/nsh/Make.defs old mode 100644 new mode 100755 index 5e28f2473d..4e08d28a29 --- a/nuttx-configs/px4fmu-v1/nsh/Make.defs +++ b/nuttx-configs/px4fmu-v1/nsh/Make.defs @@ -62,8 +62,9 @@ ARCHCPUFLAGS = -mcpu=cortex-m4 \ # enable precise stack overflow tracking -INSTRUMENTATIONDEFINES = -finstrument-functions \ - -ffixed-r10 +ifeq ($(CONFIG_ARMV7M_STACKCHECK),y) +INSTRUMENTATIONDEFINES = -finstrument-functions -ffixed-r10 +endif # pull in *just* libm from the toolchain ... this is grody LIBM = "${shell $(CC) $(ARCHCPUFLAGS) -print-file-name=libm.a}" diff --git a/nuttx-configs/px4fmu-v2/nsh/Make.defs b/nuttx-configs/px4fmu-v2/nsh/Make.defs old mode 100644 new mode 100755 index b2f05293d1..5a1d5af2c4 --- a/nuttx-configs/px4fmu-v2/nsh/Make.defs +++ b/nuttx-configs/px4fmu-v2/nsh/Make.defs @@ -62,13 +62,8 @@ ARCHCPUFLAGS = -mcpu=cortex-m4 \ # enable precise stack overflow tracking -ENABLE_STACK_CHECKS ?= 0 -ifneq ($(ENABLE_STACK_CHECKS),0) -$(info Stack checks enabled) -INSTRUMENTATIONDEFINES = -finstrument-functions \ - -ffixed-r10 -else -INSTRUMENTATIONDEFINES = +ifeq ($(CONFIG_ARMV7M_STACKCHECK),y) +INSTRUMENTATIONDEFINES = -finstrument-functions -ffixed-r10 endif # pull in *just* libm from the toolchain ... this is grody diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig old mode 100644 new mode 100755 index 9030a1f022..dedebdfa03 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -117,7 +117,7 @@ CONFIG_ARCH_HAVE_MPU=y # # CONFIG_ARMV7M_TOOLCHAIN_BUILDROOT is not set CONFIG_ARMV7M_TOOLCHAIN_GNU_EABI=y -CONFIG_ARMV7M_STACKCHECK=y +CONFIG_ARMV7M_STACKCHECK=n CONFIG_SERIAL_TERMIOS=y CONFIG_SDIO_DMA=y CONFIG_SDIO_DMAPRIO=0x00010000 diff --git a/nuttx-configs/px4io-v1/nsh/Make.defs b/nuttx-configs/px4io-v1/nsh/Make.defs old mode 100644 new mode 100755 index b4f5577aed..74f38c0cb4 --- a/nuttx-configs/px4io-v1/nsh/Make.defs +++ b/nuttx-configs/px4io-v1/nsh/Make.defs @@ -58,6 +58,11 @@ ARCHCPUFLAGS = -mcpu=cortex-m3 \ -mthumb \ -march=armv7-m +# enable precise stack overflow tracking +ifeq ($(CONFIG_ARMV7M_STACKCHECK),y) +INSTRUMENTATIONDEFINES = -finstrument-functions -ffixed-r10 +endif + # use our linker script LDSCRIPT = ld.script diff --git a/nuttx-configs/px4io-v2/nsh/Make.defs b/nuttx-configs/px4io-v2/nsh/Make.defs old mode 100644 new mode 100755 index 51420eb23d..287466db60 --- a/nuttx-configs/px4io-v2/nsh/Make.defs +++ b/nuttx-configs/px4io-v2/nsh/Make.defs @@ -58,6 +58,11 @@ ARCHCPUFLAGS = -mcpu=cortex-m3 \ -mthumb \ -march=armv7-m +# enable precise stack overflow tracking +ifeq ($(CONFIG_ARMV7M_STACKCHECK),y) +INSTRUMENTATIONDEFINES = -finstrument-functions -ffixed-r10 +endif + # use our linker script LDSCRIPT = ld.script From ae0e2d720926702f5dc1f97aeaa893d30c61fe29 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Wed, 21 Jan 2015 10:39:14 +0300 Subject: [PATCH 584/652] Removing extra UAVCAN perfcounters --- src/modules/uavcan/actuators/esc.cpp | 7 ------- src/modules/uavcan/actuators/esc.hpp | 1 - src/modules/uavcan/uavcan_main.cpp | 6 ------ src/modules/uavcan/uavcan_main.hpp | 1 - 4 files changed, 15 deletions(-) diff --git a/src/modules/uavcan/actuators/esc.cpp b/src/modules/uavcan/actuators/esc.cpp index 995c8987c0..51589e43eb 100644 --- a/src/modules/uavcan/actuators/esc.cpp +++ b/src/modules/uavcan/actuators/esc.cpp @@ -56,17 +56,12 @@ UavcanEscController::UavcanEscController(uavcan::INode &node) : if (_perfcnt_scaling_error == nullptr) { errx(1, "uavcan: couldn't allocate _perfcnt_scaling_error"); } - - if (_perfcnt_broadcast_elapsed == nullptr) { - errx(1, "uavcan: couldn't allocate _perfcnt_broadcast_elapsed"); - } } UavcanEscController::~UavcanEscController() { perf_free(_perfcnt_invalid_input); perf_free(_perfcnt_scaling_error); - perf_free(_perfcnt_broadcast_elapsed); } int UavcanEscController::init() @@ -141,9 +136,7 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs) * Publish the command message to the bus * Note that for a quadrotor it takes one CAN frame */ - perf_begin(_perfcnt_broadcast_elapsed); (void)_uavcan_pub_raw_cmd.broadcast(msg); - perf_end(_perfcnt_broadcast_elapsed); } void UavcanEscController::arm_all_escs(bool arm) diff --git a/src/modules/uavcan/actuators/esc.hpp b/src/modules/uavcan/actuators/esc.hpp index 498fb9dd8b..12c0355422 100644 --- a/src/modules/uavcan/actuators/esc.hpp +++ b/src/modules/uavcan/actuators/esc.hpp @@ -109,5 +109,4 @@ private: */ perf_counter_t _perfcnt_invalid_input = perf_alloc(PC_COUNT, "uavcan_esc_invalid_input"); perf_counter_t _perfcnt_scaling_error = perf_alloc(PC_COUNT, "uavcan_esc_scaling_error"); - perf_counter_t _perfcnt_broadcast_elapsed = perf_alloc(PC_ELAPSED, "uavcan_esc_broadcast_elapsed"); }; diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 5b961e2efc..4dc03b61b7 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -93,10 +93,6 @@ UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &sys if (_perfcnt_esc_mixer_total_elapsed == nullptr) { errx(1, "uavcan: couldn't allocate _perfcnt_esc_mixer_total_elapsed"); } - - if (_perfcnt_esc_mixer_subscriptions == nullptr) { - errx(1, "uavcan: couldn't allocate _perfcnt_esc_mixer_subscriptions"); - } } UavcanNode::~UavcanNode() @@ -138,7 +134,6 @@ UavcanNode::~UavcanNode() perf_free(_perfcnt_node_spin_elapsed); perf_free(_perfcnt_esc_mixer_output_elapsed); perf_free(_perfcnt_esc_mixer_total_elapsed); - perf_free(_perfcnt_esc_mixer_subscriptions); } int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate) @@ -527,7 +522,6 @@ UavcanNode::arm_actuators(bool arm) void UavcanNode::subscribe() { - perf_count(_perfcnt_esc_mixer_subscriptions); // Subscribe/unsubscribe to required actuator control groups uint32_t sub_groups = _groups_required & ~_groups_subscribed; uint32_t unsub_groups = _groups_subscribed & ~_groups_required; diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index 583c621eb2..19b9b4b484 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -146,5 +146,4 @@ private: perf_counter_t _perfcnt_node_spin_elapsed = perf_alloc(PC_ELAPSED, "uavcan_node_spin_elapsed"); perf_counter_t _perfcnt_esc_mixer_output_elapsed = perf_alloc(PC_ELAPSED, "uavcan_esc_mixer_output_elapsed"); perf_counter_t _perfcnt_esc_mixer_total_elapsed = perf_alloc(PC_ELAPSED, "uavcan_esc_mixer_total_elapsed"); - perf_counter_t _perfcnt_esc_mixer_subscriptions = perf_alloc(PC_COUNT, "uavcan_esc_mixer_subscriptions"); }; From e62c8d73678f87b9f6cab1ad3a33c8911277a8a8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 21 Jan 2015 09:29:47 +0100 Subject: [PATCH 585/652] FMUv1: Disable stack checking --- nuttx-configs/px4fmu-v1/nsh/defconfig | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig index a467fa605e..539634e3da 100644 --- a/nuttx-configs/px4fmu-v1/nsh/defconfig +++ b/nuttx-configs/px4fmu-v1/nsh/defconfig @@ -92,7 +92,7 @@ CONFIG_ARCH_HAVE_MPU=y # # CONFIG_ARMV7M_TOOLCHAIN_BUILDROOT is not set CONFIG_ARMV7M_TOOLCHAIN_GNU_EABI=y -CONFIG_ARMV7M_STACKCHECK=y +CONFIG_ARMV7M_STACKCHECK=n CONFIG_SERIAL_TERMIOS=y # From 517e1e8d4879c7a2d359b92e2eff89eddb944a16 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Wed, 21 Jan 2015 03:07:15 -1000 Subject: [PATCH 586/652] Fixed permissions --- makefiles/setup.mk | 0 makefiles/toolchain_gnu-arm-eabi.mk | 0 nuttx-configs/aerocore/nsh/Make.defs | 0 nuttx-configs/px4fmu-v1/nsh/Make.defs | 0 nuttx-configs/px4fmu-v2/nsh/Make.defs | 0 nuttx-configs/px4fmu-v2/nsh/defconfig | 0 nuttx-configs/px4io-v1/nsh/Make.defs | 0 nuttx-configs/px4io-v2/nsh/Make.defs | 0 8 files changed, 0 insertions(+), 0 deletions(-) mode change 100755 => 100644 makefiles/setup.mk mode change 100755 => 100644 makefiles/toolchain_gnu-arm-eabi.mk mode change 100755 => 100644 nuttx-configs/aerocore/nsh/Make.defs mode change 100755 => 100644 nuttx-configs/px4fmu-v1/nsh/Make.defs mode change 100755 => 100644 nuttx-configs/px4fmu-v2/nsh/Make.defs mode change 100755 => 100644 nuttx-configs/px4fmu-v2/nsh/defconfig mode change 100755 => 100644 nuttx-configs/px4io-v1/nsh/Make.defs mode change 100755 => 100644 nuttx-configs/px4io-v2/nsh/Make.defs diff --git a/makefiles/setup.mk b/makefiles/setup.mk old mode 100755 new mode 100644 diff --git a/makefiles/toolchain_gnu-arm-eabi.mk b/makefiles/toolchain_gnu-arm-eabi.mk old mode 100755 new mode 100644 diff --git a/nuttx-configs/aerocore/nsh/Make.defs b/nuttx-configs/aerocore/nsh/Make.defs old mode 100755 new mode 100644 diff --git a/nuttx-configs/px4fmu-v1/nsh/Make.defs b/nuttx-configs/px4fmu-v1/nsh/Make.defs old mode 100755 new mode 100644 diff --git a/nuttx-configs/px4fmu-v2/nsh/Make.defs b/nuttx-configs/px4fmu-v2/nsh/Make.defs old mode 100755 new mode 100644 diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig old mode 100755 new mode 100644 diff --git a/nuttx-configs/px4io-v1/nsh/Make.defs b/nuttx-configs/px4io-v1/nsh/Make.defs old mode 100755 new mode 100644 diff --git a/nuttx-configs/px4io-v2/nsh/Make.defs b/nuttx-configs/px4io-v2/nsh/Make.defs old mode 100755 new mode 100644 From 4b27e4029d15e5ae4936252f224ee4275f83cab0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 21 Jan 2015 15:32:38 +0100 Subject: [PATCH 587/652] Disabled stack checking on aerocore --- nuttx-configs/aerocore/nsh/defconfig | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nuttx-configs/aerocore/nsh/defconfig b/nuttx-configs/aerocore/nsh/defconfig index 317194f685..c44b074f31 100644 --- a/nuttx-configs/aerocore/nsh/defconfig +++ b/nuttx-configs/aerocore/nsh/defconfig @@ -94,7 +94,7 @@ CONFIG_ARCH_HAVE_MPU=y # CONFIG_ARMV7M_TOOLCHAIN_CODEREDL is not set # CONFIG_ARMV7M_TOOLCHAIN_CODESOURCERYL is not set CONFIG_ARMV7M_TOOLCHAIN_GNU_EABI=y -CONFIG_ARMV7M_STACKCHECK=y +CONFIG_ARMV7M_STACKCHECK=n CONFIG_SERIAL_TERMIOS=y # From b3b5a2de0329aa42ab842bd568244aaa9fd8d40d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 21 Jan 2015 15:59:15 +0100 Subject: [PATCH 588/652] Set NuttX version to one which always colors the stack --- NuttX | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/NuttX b/NuttX index 574bac488f..e4c914e261 160000 --- a/NuttX +++ b/NuttX @@ -1 +1 @@ -Subproject commit 574bac488f384ddaa344378e25653c27124a2b69 +Subproject commit e4c914e261d2647e44d05222afa7aa3cc90d3c67 From 0943bd9a31896996db7a030715eb9d0e8626b0b4 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Wed, 21 Jan 2015 09:54:49 -1000 Subject: [PATCH 589/652] Added Prefix to message to identify it as PX4IO related --- 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 1c9a5adc9a..8aecbb4693 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -2856,7 +2856,7 @@ checkcrc(int argc, char *argv[]) } if (ret != OK) { - printf("check CRC failed - %d\n", ret); + printf("[PX4IO::checkcrc] check CRC failed - %d\n", ret); exit(1); } printf("CRCs match\n"); From e037b7ae9b361812e99c5985a9c543c2ba503de0 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Wed, 21 Jan 2015 10:02:22 -1000 Subject: [PATCH 590/652] Added delay to ensure the the px4io is in loop waiting for a characterit loop. --- src/drivers/px4io/px4io_uploader.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/drivers/px4io/px4io_uploader.cpp b/src/drivers/px4io/px4io_uploader.cpp index fb16f891f6..02e527695c 100644 --- a/src/drivers/px4io/px4io_uploader.cpp +++ b/src/drivers/px4io/px4io_uploader.cpp @@ -621,6 +621,7 @@ int PX4IO_Uploader::reboot() { send(PROTO_REBOOT); + up_udelay(100*1000); // Ensure the farend is in wait for char. send(PROTO_EOC); return OK; From 2a00948c7ad71f849223337d566c41b56f7e1e08 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Wed, 21 Jan 2015 10:19:24 -1000 Subject: [PATCH 591/652] Added prefix to Match Message also --- 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 8aecbb4693..556eebab6d 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -2859,7 +2859,7 @@ checkcrc(int argc, char *argv[]) printf("[PX4IO::checkcrc] check CRC failed - %d\n", ret); exit(1); } - printf("CRCs match\n"); + printf("[PX4IO::checkcrc] CRCs match\n"); exit(0); } From d80a00fad105a2dc98ab6ec415e28fa86f1ceed6 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 21 Jan 2015 16:46:04 +0900 Subject: [PATCH 592/652] batt_smbus: disable if no batt 10seconds after startup --- src/drivers/batt_smbus/batt_smbus.cpp | 24 ++++++++++++++++++++++-- 1 file changed, 22 insertions(+), 2 deletions(-) diff --git a/src/drivers/batt_smbus/batt_smbus.cpp b/src/drivers/batt_smbus/batt_smbus.cpp index 2b5fef4d77..92b752a282 100644 --- a/src/drivers/batt_smbus/batt_smbus.cpp +++ b/src/drivers/batt_smbus/batt_smbus.cpp @@ -91,6 +91,7 @@ #define BATT_SMBUS_MANUFACTURE_INFO 0x25 ///< cell voltage register #define BATT_SMBUS_CURRENT 0x2a ///< current register #define BATT_SMBUS_MEASUREMENT_INTERVAL_MS (1000000 / 10) ///< time in microseconds, measure at 10hz +#define BATT_SMBUS_TIMEOUT_MS 10000000 ///< timeout looking for battery 10seconds after startup #define BATT_SMBUS_PEC_POLYNOMIAL 0x07 ///< Polynomial for calculating PEC @@ -171,11 +172,13 @@ private: uint8_t get_PEC(uint8_t cmd, bool reading, const uint8_t buff[], uint8_t len) const; // internal variables + bool _enabled; ///< true if we have successfully connected to battery work_s _work; ///< work queue for scheduling reads RingBuffer *_reports; ///< buffer of recorded voltages, currents struct battery_status_s _last_report; ///< last published report, used for test() orb_advert_t _batt_topic; ///< uORB battery topic orb_id_t _batt_orb_id; ///< uORB battery topic ID + uint64_t _start_time; ///< system time we first attempt to communicate with battery }; namespace @@ -189,13 +192,18 @@ extern "C" __EXPORT int batt_smbus_main(int argc, char *argv[]); BATT_SMBUS::BATT_SMBUS(int bus, uint16_t batt_smbus_addr) : I2C("batt_smbus", BATT_SMBUS_DEVICE_PATH, bus, batt_smbus_addr, 400000), + _enabled(false), _work{}, _reports(nullptr), _batt_topic(-1), - _batt_orb_id(nullptr) + _batt_orb_id(nullptr), + _start_time(0) { // work_cancel in the dtor will explode if we don't do this... memset(&_work, 0, sizeof(_work)); + + // capture startup time + _start_time = hrt_absolute_time(); } BATT_SMBUS::~BATT_SMBUS() @@ -330,11 +338,20 @@ BATT_SMBUS::cycle_trampoline(void *arg) void BATT_SMBUS::cycle() { + // get current time + uint64_t now = hrt_absolute_time(); + + // exit without rescheduling if we have failed to find a battery after 10 seconds + if (!_enabled && (now - _start_time > BATT_SMBUS_TIMEOUT_MS)) { + warnx("did not find smart battery"); + return; + } + // read data from sensor struct battery_status_s new_report; // set time of reading - new_report.timestamp = hrt_absolute_time(); + new_report.timestamp = now; // read voltage uint16_t tmp; @@ -375,6 +392,9 @@ BATT_SMBUS::cycle() // notify anyone waiting for data poll_notify(POLLIN); + + // record we are working + _enabled = true; } // schedule a fresh cycle call when the measurement is done From 30e32b004d6e9d0a4b13abf6ef5f93bfe7bb4754 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 21 Jan 2015 17:18:55 -0500 Subject: [PATCH 593/652] travis fix s3 index.html and timestamp.html upload these files were being uploaded as "/index.html" and "/timestamp.html" --- .travis.yml | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/.travis.yml b/.travis.yml index 52ced6b4fc..8677bf91bf 100644 --- a/.travis.yml +++ b/.travis.yml @@ -61,7 +61,8 @@ after_script: - ./CI-Tools/s3cmd-put Firmware.zip archives/Firmware/$TRAVIS_BRANCH/`date "+%Y-%m-%d"`-$TRAVIS_BUILD_ID/ - ./CI-Tools/s3cmd-put CI-Tools/directory/index.html archives/Firmware/$TRAVIS_BRANCH/ # upload top level index.html and timestamp.html - - ./CI-Tools/s3cmd-put CI-Tools/index.html timestamp.html / + - ./CI-Tools/s3cmd-put CI-Tools/index.html index.html + - ./CI-Tools/s3cmd-put CI-Tools/timestamp.html timestamp.html deploy: provider: releases From 38ae6bcc5fd5e9ab1a7304140ec6b36ff027625f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 22 Jan 2015 07:24:22 +0100 Subject: [PATCH 594/652] Added block diagram --- Documentation/px4_block_diagram.odg | Bin 0 -> 42200 bytes 1 file changed, 0 insertions(+), 0 deletions(-) create mode 100644 Documentation/px4_block_diagram.odg diff --git a/Documentation/px4_block_diagram.odg b/Documentation/px4_block_diagram.odg new file mode 100644 index 0000000000000000000000000000000000000000..95b6f600b87aedcb06535d5b330afbdb76d91c41 GIT binary patch literal 42200 zcmb5T1#nzTleQ^lW@ct)W+sc7nVDrVGc#L^mMmswXraZD#TGMf*w_B%&iuQ7%=Qx< z(Wfda^R3K072T(mWI@5ufPkQYfUqi`fg^-0aNkT^TH#jT|klOk9-yo5Pdpr7lA-AfR9G z-ww@OjTk)bZ21H|?QDtM&757V9PIf?m>HNzh|KIw989h3E%-^)R3+%SNCX97_#Dj5 ztxU{#{zjqywVue+&eqXPn=ik;mzXSa<;%RGT zZ^KVw>FVmp!^r62;lbd+%HZH^!N|zr+ef)dPzkNA4IQ&QSuYCNIuHVvGn3&iY z|GECF+QrVw_J14kmo2@CgPo(1tCg|sf6J+ny_Mbn*3B>5-#h)cY71vmQ``UO{#Q|0 zn3z}@Ef`IWT#e}6t;{?~|0@4yAOH8=a{rIrnmQYK{9jivGco?MAtCz5JI}uwm4!s$ zZ&&^*p8((QBAPgx{oV_`sgbLh01FcnH$4*pI66C6o0+&Wnz3*ju`sjIvlttjnR2qwGjXsmo0@UZ8*y{8vv8TvbD1-7aG3rH z{a;c3iTa=W+vQjDySQ4J5dBVVi~@Xriuot~f3N=I;&)a4 zIfbC0pne}~K>vEd0RaKIn7RJSrOU6AXkUBFWtRiV|H2^g>bB62DD2g`AIu&q1lA>F z7HC<2E2Qf)j@Gr5Gt`G4xrxg3%JyhEAai1X+(LG%MZ3mBu*UD@n4XBTt{2?AX=g#9 zY8qN8{wZS`ciMElMwd4~OK_uc70yTLArli^$V~imigg5+GN7Wo&Ve&00>C2*63rtH z9MOJzLI>8KslG*^7eBV7u?h8s5#EB>Ow(gy-`T63PteTwe97M4e*N*>+;UWyii*r6 zI2zf0X7$S*R_>0Ma?0J&*;7n|NAX?AUBB_bOywf1$J@u0C9Bcr4Vz&b}&a1D`*#~yJlXrD6X-G#I zJkkuO?Cf~rngK)gzVRHCHk%JF-NuFZOS_+k>0CKD_i0o)nNt7hgkDA_KHK^9uDnnprag6NsRmVbG~I^t(8 zqQYaHN`hqcQbqBLIJk^>#QS6vFZQuEg62#exBaE;cx!u%K?AeqD_o{`#7{bNDUz$t z<%&X0qbN0WVRZ<4caq`7Pg4BWgX8z>sCb4^WN_G8L;j~#u#KfpjVPba8nw9?OVhtj zn3;%=h274{q;|6jpg6iYmBnn+mK|ffx7lZ`vZ!*a6n#Xq`yF(B4p>2ZU|RUxTtWl% z2^W{k&`x5dHgf-F!$s^fVKvvmBac>qawwM`4Q5Q0ULg)=uYAj zhVC3HJ_a>q3-rDI@Fm5CUye@b#Oxs08F-P4r}=4dXlMo=^3Px_kO1?EpYp9tlRe|~ z-jEV1=;(uhLexXufwv7I#Ef-n{E^;8vjp+X>qRA`cc5^H0VZl+ODf`_H8>~Wwx#9D zFR32}m%eKgg&wJ%QyxBAKYXiXg>8m#aNiDzU_O(MwTj0fC}C99*`}d5-ArGd32KgT z@}3?=YF=4q`BZ}IoF~@GAuAtYgxnd#fGe_z5}-A3@9nzV5M8*>+l>+s31K+U)i8?j z)vgmBGd1@oR8xR>LQ1>(nmL7+Y9DEi^#5qf2fRK$rAuL7+K6mNl|yw+0^Po5cNQ9wu`=#SMd5Oa*02zRZ>6ACJ_>a(2&B(}{)Qsi>y#HWySG>DToA z?AWd-vffM_Uj{4kmPJT%v?GRe>Sdh)g}^_kJEM zUjv>t4#QuLyx6ki%aSi@J^O2m=1jS|2o&vmI-ZaB2u(chI+j1ydgL7JhX>|bmH|6} z0n5gSaKIRdzog@X>ARixeMeQ<>W{T!LxH>iUTnUYgDr-zEd@f!vLYvT=V!`3>Z3T0J_f?dAYFtJ70_db>@ijJ+O6C zk=9(ef*k%+wJ!&+i?0W{^!s|u=s9rf z@9URJA6*COBJ`1ej`_83%9G_e-0y$K6UUNxZRFp z+t*XH4mrM7!Yz@B_Yr_EbN7=<=OuP1>$>heQ<$=#wtnat{?_!D{N0YZ$9n{xg{f_S z1Pg~Rvrx{lAO_#I_A@p^A1|MLHgG;0Qqls*jFHa z<}A4}&aohzy74;=tL|E<^gTZmCq#eB-|y=j8}VtSHx@D{A|j)Jj= z{qS83N8`M0og!+DzvE(JRqK%2s&FDm9Xi=ukvqS4L3=_WOptNX)N1M+V!2Nlz9ojj z2l^+N`+_w1w|H9x!?&F1FVl)vLosmgXLYs;hQrlnCjI%WpoOTKWHy0sbXN`myZ~nU z==pMMk+*?4vJgJpF(X2aTiNnQ$ruAJZoKM!kK0FZlVP}UM!snutRAkLf^@_H{YpTN znCO{N8L^FR2fMjuC^tmc$_4EaX{HGy>tJ_>H!G-?9kw@%i-AVW>d4i#vxKqu%f67! zi>FnTgXDw#wb&W;ZhAfY+B2sM&NMaKU^)}e8qD#*7cJwlxsY3wB?}#5cKmWl)OlIW z97$DN7lH)L(bRsy5{^8!Q4QhXuKAmXv2br_*^dOS4@4#kNLIX51Br(RfZ_)d=f@+q zzMJ#;j+Jy`_uHN#iIKHpL;|Cc76GxGUNjlMNArY#sX7Y>7CZEJJlUE=%CV?wbV)#; zFPl@RBT&3=@L}DLF;p;)D!1fuSPj13e+oXFhs8ub;U2cWg;LN`IkvS+3vVtqwVcV3tWcR5HF5TOjJAcPV654cWfOr0$sI{J}RXt`Uxlu82 zE~dbHqJZySmhSRBl^2we!@Rc5%eaNj{oZy45a`-7ALyNzTKTfiGM@RypRy!oXMIR6|^FQDCF+k!9tXcBDUpoCX4mv)}5=GDq4C@*#EYN4Y+<2 z{Kl@i7}87^_RVg>#M|QshF!I*eN9}Zww>*aS@L8gKVma$X+3I4({L@$dq-7GmvEZ5 zsJHO<-lsc-M%2)j+k$J-E7zUCObtz+7H*x6$MmxEzVWB{W32XhxMf3!ABYpwdN*^9 zQL*>r(|UVvj;;5ZHOb(I(Q~A_*KBv4_x zhx2VY={#rBU+4BLfy9q5uIazd$-hF8XbF+*0E-UXtuM=B5vrULYK;%y(0#i+VQ~nAhf<7EckEOxW zGTiDN?c#QT`WZGR%at%~jBR+2Lb+r%tT3A_n>b7Gsr1^Ed=$;((=ig%+EevIjb`k; zuaz%I3==KL7Ls0!$`q z2SKD_#2*+!6czA%hy=KC@_&fD3{w7sV~Dse-S_Z=78PtdcE-edjF% z#%f4}|2-Nd@q5&c^B+r*e{8E|e~lJ;@?Ak+L0^qcTuWyk!wpjU9VD+ad>*8v{E@5< z`;h}`gl94jiYy@ry(h)muE2>~UR-(+&p+|9fEpdg7W}gba39xLCyb~wLtvGkH?9YO ziC%y3)PeSypK^7k`sM;0r0+oDU|>(UL^01Zv~>I;6`izn;@H_drzQ{2m;sinMHLdm zz|tOP>N;fDnZc7=bZuIx+Dlzn*&Owgnx)g*!c%Kpu0wTxw4zT8M-1mj1!=#fB36pG zM2PSo<|3G=T4JDr|4@u2h7s;Hl)Xvycljl#iFKX!-5A^cs5g zPfZyoDA_V|A4C370xMA1cAh>#*)-}S`1qp{Q?gKevxjHCpyE7T2B)bABN0LiTMTOh zC9JINrx?~>VxfY+G@u$G{eg`HE~`x8--)s$c?L&@m;))5MYch*!OLa0Y<~i8kcgwT zO!U}A-9_C^PTxn(viBGE6{!~fjg#wAf%HD+APyUJ!f$auLVQJ%#~C9OBiR2TZW@Qp z?KMa<*qHdA{-peYnx*z{7q&~=F$ZY|NOovJ#N+dmh$LtvXe23^WN2AF{%s*5vLyGx zN(??U#PpGmcMTBoLOrTW1+jK7{Wzv!3E0<*NHFqBJ*pywWCnj0QlMpZD1?aoVII10 z_J`Z=so#DTph6FtQ45j&;3`7J){qMo`iD|fkwWeZF{;1hBEiUeQ;HD(;W$W%OFsNx z*C9%lJbcJ7e~_UJ5n;8jjthezB?9r)E2Ur~=`=Eg()Y)RS-9goT zgVxt5$v6~Ah!2(M8xGX*Z%vPhIxSz#rk_{H?7o8U4VWNv{okGx25W0s< z>nC{-2R1#VN28g~RSj1!Y6mz(qXpo+tx=&$eq&OlXlL?!n*7$W;>5tVvLk-+gaACr zcd-!0a^cOMH^0{#!^a7`rF^F(2rE?OwgQN;Sfqe)2&=tBKA>ig4A8baikGm<3)x)E z5LPJ09*m)OGFmty^=jin=Ij86irDY|4&I7f={=q0p+a+0XxW0EEv>$~)m4o&agaAx zhAJM;Tz;ONgns&Y_V455J;TxlUTXJNt4{uTn+BYiV&2Oqu6JEt+-mDx-9+nKx(Vcp z{gGhD-&}LYT~0CGecW=A@b&z+=-oJX6f02c?cTbte|+Iz)n8;>&GAt)fWNwct}OM-T}?Q_1ypv|P4GQe&s(W%EwUwY<=Lx!;!I+CE0?*e{mY$i3-is@@% z7cJX-v%ad09!Wl(BO~mVVTA0@7eolV%-TDhWAh8`d)wPEtXb!i4~s5a0FB>)o0S`; z8kVm{Ke=YL#ONtlV#P&V4*FojWt=6wVj2?aeWTNV?L6KI&KJ!w?6wM}K|TjA&4eve zgsonrC~D(Ix9(^K#s9cxP+M7QcRYyxa^r*pYV?6tGlcs*R|(mf8OuDKc5HN<`RLAu z9f-t4_so>>qt+9TF|2?>5OI4mKV6`J2B(XV9;4jLC_Sq8FVi*qhpKvB3^C;(&rZ$(ttW0bd z)_u!slocF!60urVEA()1_b+`A4Lm#ay7RU45#%mPx9fI^Li=dGS#_jRaw5^)R|W9i zIHvw^Yk%gfN0ioc_)=?u3x0ab1%ocD*4!MF<*XrD>8Jh7CYgQ<2FT#Z$y~ml)>`x^ zIWb76ypUQ%_%BD`O?mrF&`AJ3t_Ta4bJhxvl)a2MV3jNw5U zX5FjAxaY#l4&Id@c~*q=g|9FWE9^Jsf?-~nV6~rECt*u1^e_gRq(Vu;xVZ;%D07iK zZqs=gLJ1>)vsi?p=@g7Vo5(|30Sf3FRV#!Q6^_xzdJe~~SILBzSgR6utsvKuR-Yma z9E56AV9=CpWcBF3)SF9sOck7ro6lt@iw;W27{ zV6cMS+ZN!<0@v84X^R?FS+9#l)@V?lE_U;itrFGTS0#^+sC0sg&x#N*ev?=lNy@K{ z&bnAm^3y&!F&Ud$R3Kn%GF}`>iUc%$FVn9+F^S~KUV2@qBJiVgG6rPjdq-BrlXLJj zwJc;yRa~e0Z-w`tv!-$^yaGH0X7uJp-!%MpRgBjvo7%q!4=pW;y4@i-JF1;L>6~_S zS4zin-V4d+KDbM>Uucn^6};pvCT?Z$DAe&NP@no~jEu2c7PS~#+#SoQ4S@TU`;@|s zyKDDr)veq#bY3d+T6NC$^Ab+b!sFL%;@9TiG;|s&0lf3Qk8|SpO9SrFy$JLv3f?nosj{Lbn z0{q({;S?S);r5jR_uYe!D?B|pfsS7TK4w0;(u@v>ai!1U%&N0Memaa4si8qKrvR49E=mAwE9hPMHroWVM_bvJkr4bfF}Y9tp^C zqag9q7s}=CQY|x(|Mi?pc?ANFbA#Xu&@m=~I@HrMo-g@_YdcE5UX~xho zg>l96+hO0&XMiC`)aGQE$KH@-t7lg1(psy(mgGz=;gh#y8+W_-rSQ<`_h(+=SwyPp zRnad=oPZqw-&YsrJYV0=4-|v=gT1w}dBm^N-Y^Cb9=&Hr^4@rlw*3p0bc1)C=|VOyVn!F5?rS4vhvWXI z-!lRkIH~hLpbi5(Ul^WYDK5n>EC4%}0`Z#)gammMdqz_6dfzxVd#qnZN|_{*JNity z+T$D~0pWAT_v8zwUVP8DFVyxjgbh-T18IWk7av@j1vgF{+He;HWqZ;qJ$bAx`$3FP zu=5MWtCm2q4;{GJZ|4$y0q`Ho9eM#?c!j9fR?hf)!?(d>XWn%@mWx+Yo@~L{=&TIN z`p)o#+Yi@KcQ!`y-5uVLh~F-00>VFfI-KSZcUt1#EvOU?E+HQ$H5^_Z_8mHp!tX3F z?YF5?0^;A#tjYU7QYnZn!h5H@eMFCv6dyqGGENaRy+9c^FMGZo^~{VUZ@r#F2)rIR zy+-lgPjRjhl0+c}ewX;rb0#&=BAvR=HoG#Pg#S3)+K8}x@vHRQGJam!=Fsb;H_aXC za9|t41(4lBczhq{1QT}XnNsW8p6>9KXZP3~mGz@cmAjWX+E{*&y_FHP$a!}pUbv+3 zaDRvH$}c`$KOdQJVa3uZesqBwWcT;%w|&pL37$EAawQ|3QZ*Sp5yP-bNAHOh5L^%7 z?H;avS97V3Q%`3bv{H@+_;M;>3TvApX-9-uAZ2v`tP3W@bE@(yz4<}wIvUGEb;AN)%($axw*ak;9{oH+iW`5{NY=n}hSxLP~hULy?Kn`jnyK zNkl0Xr3hLLT8$A>Ej|wkzTDawdJZH;Z#qBj1K4{bjG2edKcW9*TPGLPk?s(+fj$6$(23!h>B_@KpJ=mS05T zIv0fng+(8M<-7Xm%AkjJTDQt|Zqh}HQmWFOjgXo~bfw;t!qmLR_nM1aMYnDM2jbiN zfRFQNl4zPxzR-uRaL+KY4{PvpZGV74^oKX)`^4a7Qcobc#4+6ai`b*>E}f!BzSa`0 z@7WAshIV}uVeaJQu7fk+Wry+OzOGK7^GWpeAhhgMIXRe8??+3A(2T9%^0H3nn!0YcwSQc(S62fZxFW-6V;Ft{hD>JoXV{S z0{j-JWjp6k0#L7)@@wxd<-J)DY+P07hQSdKbt{uXWCHmDnP(D)iFyT)idbn}L#e6i zFtP}I^E^_gM`cS5xpJOw(+uXE72gM4I+E2~96hN@t?Oy?2@ayK4v*@DY5+&^d#hC5 z=v{B89fH38IXmwRf{Kt|PeE5qdQ!Wz=Oo9-Fu55m@;FDkq*FvwL{nU>QaK!$*m7!}zDJq$qc7`Y(Kp|4TmZR1KMYGx$Lxq0`_el{?#bcU zv5ksz$160jMB(h4k)_Hj3XAe#5!(%BVp$VFKNgTe3i=J&mDrie|BxR!8_n6=Zop$`O-_Xb+1|jqh?!ir7OI&BofN(}$hj;L5R^ zRi*%s4&6E~uCS}v*A}5x5;B{3k8J76!Xhw_)4lM5mA5@h*uS&NveZ8;0{zN^EDO3- zRTH}dtUV+#LLhK+o1TnNNgk_qDLbGcSzXFoA~4Z`%&#UcTJnS2yHWS>c+7*tE$k@r zHx;tp)_Xh4k?iOM{tlkg@cp(3NL?(cKTC|t#Wo}0xuq#UxC<3GKw-VM;gG3uaSQMu zZAo-X;9nuMUFJum#1g?^R1#qY+eAeoOHx@-KpscAUt?`Cy$C=Fc@6X?Kv-_knidGZu=hfCV-Toef#esHuUe6Quwih~G^;t!jsq4Gt%fS4^r zi~?N6B0|Z5<1;d1If~y1_{eSAzO|klKNZ{AkhsDv8?7}!Eo)iAE(!X52s|6{J%uV| z{pLAHN&8T#eQcZ^zqm=h^BC$&ORbU!mHclu;2H3#F|WFXjXLHC}b{V zF1*?=Zjc+t6{?%{?E0pWq(_B%xn%Kh&JSnvk-=~#Zp0&^Bcda+CM&!)1vaBbIZEgT zJ-EKlF~IL=Re3xh8YDebeC0CLi4|n@o1L#AMybI@ZeUm#?XZ4aLpzAY(o-TWVEz9< zxf*)!&sPDl6TDRNF&TV5ehVSqFi3g2Q6vyOj}FpOuRyabHu9aGR$DN!%il+>BNwUStzm9}v?p7w9&Dv8QbQAWLg2fL#l6vxb z^7=FI6$Q#<7a~3r;MGld0~Ru^h&r}w z@;6@}6EmWIb8>1#llfR9M?RS}UYQ}IewAoFE;(QK1m*88Azh744xPVOa<`+;%Z(vL z-GgZiSy-4rirz&iCy_!ez96gAC@++hyb;yCpM|`T(O_-+;*}t}W zb+*eiqjy%8$)%kRBl}vxdO9Khr9Q#;Ym|X=8D4a&aSXo=#fsi!s4={8`dh-%QpS1S zd}Z|gk`%4H2v1GxJ$#_UnQo)HZt9q}Tds|)qb}`A9O%;7Qt7#y`J%w-hc*kQI}AD za}Q)=qcMNkj8)}fOt)Qe>`n~dYZy6U84-dTaLI!Mi#igouj_1QXT4O+^z%`OLd-UI8O1tskZr0xPd4r5@fVlOejLtRCA>FR z0*hMBf&5ft6oDpp8v->`?!ZW~idwLP2eb_HnSRjBjN<~2yt0qH;uhbVTRnfZGex+|Nr*Vi-3{}Pq4mUU_$mi z#exlDK)x6ut?NT#Y)M`~u=6q^-J|^G-jZOmkNEnmO8c;A@AT^g%ny?^ABALVq~k@~ zt}pDvo->ruLW~}U!1SMt zYD;~vu4+v;!LP=@sK5_aIZ3o2I4{mzMGm`5SMtsMG|VuaCClwI4Xz%g$xy9kEHaJc z&v5#tkiDgma!T|)uL!NoI46lpEnDA;=VxjWu(Nt3%ED;(sLHpaw1NPS6?)ts>gr?j zcVBeIL-050&C)j-f$vl!=Q++(n*r=g*o|h)=Lr1+t!XyMi_^=OW9ETr^@h>>uI&(= zN7-X%fHy-7{`m8JT-|M z=p18J^@LvLLRfR0fYr$9wU0`0OA-ZH&u<*l_7ZSU24f#G@5}E`1|CGk)rufCK{1mP zENtd5>9a?E1h{RpdPaLFEie&k{iK~<;B;*xV%b~Nx%@n=fK}GAOaA zf5428|vdsBIrx0I1N-Uvrme?Ntd@Dd~?=RrHS232a5%Xj?7YW0h znRtK4#r9||*~IoFQz+&iW+EhTmGF2_RaM=ZyJcgpS*L1d=RG-}%3bYu!(|t~3A^=b zKT&<1iJr4%X^r1hw1^IF!QKk|d1vEPxlW)%yI<|tYRBttMf=U<0XO~hupejU7{!Gl z`Uw(BLBH$$?ZspAxRw*!^Y@Q@U(VdvfLsNPU>~Ihh=&{H_uCsw2>edZM#(S)4;Aut z%SUwoq(ad!1Z`}-7&@;xo|G?wCXCt_mO0QxPJg%pxnh!K^B1#1 ziS@_;?EWnp*PJIY>;U%P-muZ7{;w55Z{>V{%CirLYV7oftDq-KwuVRKQm<*cyt11( zj}8Quz2klWPft~|A_sL)kc!@owwa90{kH>xN-U>3Y#$Dh zYu;jgf1gVXkWaZVau<|LeqkWVxiFJIDVf~DKu~jGCeJCEyuv{CB4sI;+(?*O@_R#? zzGU`Ve;PQo{ z{?eI)EKjys!8DUOh7wrEJ+k{0BlIr`D7b$mND%!?LS*b83G}21nmK>y%o&uR z+pJ)U9EwjxmLgO5gT8_>DawUv#-ZFUV-hk|$_6i~Qu8E~?~Na)ELiQPgWn48P+102 zxzr8uR5%;qysy*^^?p6|yl*?&0k@4nm&ui{$iHKU|0g!_zhe*ljh%)hMW^rw{b?go zj0@F_!QBxVC^8hvKQJEFBS(7BOuA59rAtGx)zU5FLkG5-iEO_-T<}Ok=?1<%*Ly@M zG7~E$ABbilS_zaqB^-PjHv*JA{mRpl@2@zqSA95e}3(t&9sf{>18%>d9nbuST5WC ztka4Oo2G_0iE*rB)K9?KcD?pOIS9RNVp-nIRL8kow}?VFrB4rF1bg&M5cvufI-bXF z;?c19Gn2LNe4A?Rr0$x8&pED?X}`9ebY#A1BkXD5qGPl{7jgd1=)!f(IIfh~EGb1= zg+e-$%&aj5!%`~qr%J|QwcLH&&<%Im--aIC-ER{5+tPTl|LP}gqQCtVNB*yVqEGnS z(j!U#YH19qKP?@fWzsfY@I&B=Ctvh9S)QkWs=h_4l*Y@BqI5d-VzNZnBbq)lWnC1V z^qk#fMqSF^B1J$8qV&5|Rf=$?00xnDoMR-;fN?#Rc+XDcH@Nm9v|;c4p2&Xohhy&W z-MChHEP*Lid-$X8r@!K{!uh|!tCp6l`E72ixFQjzDnOE}-*m07;xE?)I z$M^W+zFjMhs7?6K@x#e$)>nEV_LUh-AJD{%=8!4`z=H+KJIN6&&!)@3mv-Maf zz0Ps?Cuiq`uxnC^WL6iIqLMoIxn&;3WVr!hO;jxh_&=!gl6MSmhhN!pn3u!grd9vc~XVZ6Gt6zI+!{7hfGc&q_0Eg7Y~v}!jHtYn~R$7sU^srRH8<`b=4}keN__lMl+|Am1`d z7uk&Buv1UaEGw-FG)8k_nIMj6iz#!Xn0(6Ic@x8C3Stam46>UCbhY`3T)Wjv_Pg^^ zAFUIPmHQ%buH$NE@Pmpo5!tT?^n?;YkwQl@M>0p>OB$~9cUSvlZx?mf2a`xKE9Kh zk`dAnlM?Uk6YU94P@nCKvi}ezC#3oHFk2&}vKB+Fv=QD^=qsV<$DapA0j%}kcC~1vc zAzp%UCri&GP|i~CBU> zq}Rjbj#{I0GOyx$+JD=yuO@HYD#EoBn;V+{=R>;YYMv6U`s zbEG$~Eo9(F9_e^nW^-albI-BpU_@-)bdqYKV7;aVPX+aE>ClD)}MmuO5CXuolc!gx`|~>ODO0v&M1=g48qaga!a+ zcs9jS6#o#A+`M?Vc2qSlSXF*YY*A?e!Rx2rglep`#z#6p)eCtl;-=CrR##;C+4Iiojb0HVj))iz@fNoQC4ynz{=|SI+>Ejj zK4i&}B@t-d)6Fp|7jL@=(HPAkRP*yzUg-yD6B!MBAIFYes^!c?>^>kuJMrV7Cr03n zf92{s&_y8lMU#g<=!coD?|BYd4m2s>-t=LsG6ljNFz|4a3B)o(9Gks1QE74(s|_>7 z)0Eok=a3Gu!JVpkmjX$F5R%6AvkCWKC?Fs5{>C993S5-NitlH((W zVn&gO4_tItJH+;%KBuo}GWkZCKB7%uQP3eb{77`Z(Ts2;+BR(EK@{gpgNZU`5Q&5% zzZ|-RXW7NE2_>ia1k!BQ5aPh`ghD8zI#{T0Nc*fA$fim)r}?Q>qHp<>n_$}tx-7hk z*OyF}3dNfnpMhm<6tO?1`YHSB)I?@6W!;B zz=LhLP!i8-{wUE)Dyhhq6(Q!Pq@XaThQ4(CU3i9+6{VuXRnQdjri{SlaGL&PTdpc& z&DB2r^=!+wH8|sE%#c0D%`lU1o%+2VSe=$`K08W#g}Hl@k%V_hgk(!2Fo@g)VcxQx z%(pBAm5DHLTj?h7Caoyn!Q)IC_`sM$L|ak3nWQ+>CqyjeY`wVB1j5dDP4J8z;dP-F zjT;8OG8NI~xg8e{iUx;YMiH@caX98L?Qro;?lQ-qK6*oX^fTx5!!F=4hFt_rqw(P9 zvG35OSo-4K%Eo$Fd&06JLe8#jOJmfIF!tL+vfp}gwmgC0aa@{WsXv})&s@UL`GyJ8 zIJWLYd%xBom0tQ=B8AitVTXbXpsgqw)}tsPSySmzLA7M41rG7NT++oe=3w*+KH8&I zz_E|&7Kwn10K;lrI*PYK$@Uk9NbIesT^dJj-%~M&5Suc4XStqRQm`` zLmNhggl8#Wn3RM28j-;V7jf2KZ04rDntZ) zGUOh(2_2Xb`2l^m8^+h>^EL>TL?Jx#gFIRo0OkHuN=rEqR*7+EKSOiaEd~b6LL-qG z3@S{T+)n>h7_2SVf$#)St8hIh@-~X^!XvT)Xd)l~H{eI)g`E_Ae~>BDj3M9TK3@#$ zPQH|5Vo{*pXyp4sN>VH0c3C`$2}Kw|@Sn?pSH_>dvEtuqMpBXO}R@NW5! zE(?l!E#h#c|fg!eF2Sm7z$j4rhqQ_^(%AV>6!Zu@m` zDch8pPMp5hjgrZE)3Zf@PeNm4dGV|Qo*;X^j5LPACI@E5taLiObos^d3_ zt7y(gLCK|$Xbq+&h44H>F#C#V;;{k`8FNBs5Rsv0Iz^d$FfYGCBl&W(0W#E00yc6TUEL{ zi(G(gS5ye<(`y(5yo$4?5R?jb)?+0W{hgDr->|n3h=yU50Ggv0Z+O(Py08#}MRHht zQTcNm&@gI-6M_!t9SS~!BrY?S9}AnArLiqJz7|0>ZMdOr@D3Qh&}rL`zIzr=ju?~3 zJ^d0=e7k%@(UM_pJP8+TAMj#DoTe@nd{U||cP-4!%FRpoJ|Ykuj&kz{~fjH!#lE;R*h3^Ee-yC6K z3cgXQzZBZI1wYre`Q%l|rPN7DyGJ=3XFKL`gO*o-^@g-GZfn=ea0y}US39jN3xOg* zz~rRLU^CjvKY)gUXbamu=c5H8^>Il0ajOxbC$X{NYh%oo(cSU}YZIJo2=hnG!i-3S ziZEG#kMuGGvmA+|TyPr3knDj{6+j?v3>)?IZo;08DWOXGf!KjX@`Le0JLif7jU>|% zr5HjKjEG|HMabq6mVa-y5=gwP>#1vVlPZsxbv}=TiARAcmC6^%-~X zGcez0$y|^TWXhE3Cc*;LCTLVq|_;Ur4E%GT>JK&NFqfZUJ z)40zmO;C&?GMI3z=#(BWi@d}<4s?P_Ddp@0uRMas6As`2Q`^}@f5AAkHYXm4{PbPfGGZQdK+*y- zR!$*F2CJoA8950TK&7*(Pb(wsQK?AdToijf+}lk9NZ_+*g|Z-Gnb`?lQy;QrP`YA` zZZKUdqLw3JV+>^i_ye(yesViVp;&8<15+#SGG}2h6P0UNp;@^6nehKZ)jbAx60Cv3 zk8L{}+u7K*wXtp6ww-K}jsLN2+qP}<*6umyeebPbP1RISSND9F4^z+6Pj`F2YaK!* zVuWx`z7h0RFw=t;)m{>~m#Pw>#0y!9;2}qwPHMkONqG-q&58 zz$ojDtPiaEBP8!3@4pL7s8IDC+t|J zg3A!*6V4?YC}!CTVk>+mQ?#_XjPba$LXqn|o)(5IenUjQ8F6h-3WN&nfEgy2e7N!G zPIuoGHZ48Ab{B}ahP`9`jxNGg>W)=}ngIvvO3)v4NrJC+G)^pE3KeLsKD_F*fY}M8 z#b(L}1m~@(pl)$+H#sa(IIoJKsqvs;nU$27&|OG8#S@T$b@Z)}(0mjpd@+)AGuQA( zK`%H6+>X|aeN2N5>SzHxEhs=?5joaY+Y9BCsJts3V)=v1N)! z8I<(}Vk2`QW+KoFnBBY<-{#C@5`0EN$(?e>9sFU8{5TiBV3PO~#+)99k&@E58W2j% z47mVZZu2u#v>yO=nW#nD3EG(SCNn}#Be{C#+e*u1DL$-^pg3V^ZereLc;^p14-##` zh}D;^+rv>zc7&)w)tl(Jh~M8g^OJg>iFGFc*f4kxN(<^9HqVDN)-;NqH@E!lq*UBc z=r=0Phrd=Sli>kOitz~)%%eoe)cMY<4o=bxCM&`|k{0X~R&dmgI^37wf-A3opg5%f zZ)OmY1xY``BFTCKn*4();*N~8MK_>GKsr>b5y(p+RXd>1=l~oi1RFQngM=5(O2!1T zvwrF~KyYEGW&%r1p92!or!Xy+x1kZ)yA_Iy6pk-0{LBHV9PQ6+ao-eTWuz@9ED16f z8YQyeWvz5ca{H40pfD^Kgq_y8r+5~tQG=7Sr2sTFVlM|; zX;uNuuyA?MfpnI_AQ8{>JQL^|VNaImh){a}3}O#oo+eawI4eEwo+bzJpc1D5Du}HA zLVjp+R5Bd7L~w#U)*l7Ii8uWM(U*E1tqhoBZn*m(-faCSCis@vwQNM_Dy)50h_ywI zYSw+1KVx~#zf6*Oob6V3%H&9Vcr?&UmtK0*d&xoJxqAw9>M^5xciKXgNQGral@Km~ zaY4uhXZ1KUgHka+4h2fdcMAFq$Q1UOv3TI1S!tgTWFb{n0@o|L%3Kem*>gBl`R&RB zmaW${8I{O^jxq|xbhZq6c)qQ283xIQDN)$--U4i!#~AWnm00y6kEBheo=mzHrWQ@L zCNJ$xP>Iyj3(A$a!pcD!(S#P^$~7S@@iZp;dJsu?{soxMUJER7Sg}b^XHEuHy3BwU>Tdh;<3( zWS|?p>aYttwEya_62(C9zS!62h~(Rd3;T)4pH1Z%{M2C0w9C8u`D@3g`Go%dq)SU@ zJK}0E2e(b|nLqC6{#JjEx9fY?!~XXZL7>||&(eWzDPeAl1Y4hqJD=6{x8`*#ApJH3 zUl67Hu6K;(>-G^pa{u|_A<{i2(rpfP?@2Hc_;~*FYR~4YEn_CGu*c6}8bm~O8+`MZ zS>TN<@YVtFRs*EFmn+b>Es!i%?uIUN2aPT2Wp<4erv+XwcQ1FZSKO`-S4NL{?o@e= z4t@jcw^=OrMjITPQ^lTR*5U(-B1~T7#n16JIMb+xx&UVPs*k5Hi)4BkMD{AhBQB@T zkz~{A)GQCVsSv9An@TsKAgFO%kct5(Z3l;xB+huk|9uQBPXR7`b8I!0}q9cK_H z_!(A>B;Jv%HJ`2C8pU5 z=(_sdaV7d5M4<>*TM6g5kU9UBuUG#$EU9;8`{c8yh5PN zm!xWFvgHC@d@#Q~Y^qM6)UHCajU-;DnrsP)DApYk$S)Q=yhkY2LKGbQQVjfj#_NYN z#Uxj2)iy1_t-T8xXbV$lS2|MHSLL{1G%F-A`Fehy-(@(kB0u-90IfryU*|GUp{)b$ zQdMfKBn529nKv4N|L8DT+NVxh0`#c`6PwIIJ%2B>#&{xv9z2h;MQ+JaM6Eu2`Kq8A zkXn=fSJj|tPOs@IFk5$40rqp+s3B`Rw)O*>WS>c&N#Egp@8Md`lEg;way*5fu>eh`jpelK@QSvuyo|M&;648j}wrvk7Pq8Id-ZF!<2VGCLWuV2OrxlOtYnyp)=|I$))ZScq4{-L|6y<})s?l^?67 zL?%~Bb6iJxXC`UNp#+0Y`|xdXC$_wY1ZCEJ!8wTk);veKB@T(u^YM zh1YqUF;+Sw;dhH$M{R9v%bD#DZVH1bbqQ;EzevwWI;)p!sk1<<`qCk}1UCvSMM`m( z1jPng$1y5Nz4v=F`L;ZC*a^7_ zxd}b92t8wFVU(csy2?gr>D5=rOvRNxWn+|N%10^RGquycXY+;hX|Y^-@Z5WRqRuJUE8q~t+SwX zay_=)qhm*y1f;N_G{aW-60KPVEYgW+O-b-{_KfAXC@qcU=5wfa51EZCWNQznxc}K? ziEZQo-7oT2S~SG)*c>BoTJ*%YhKw3CXNwT9NZ*nJA^l>LQa5k;?F(~H^!q4iy`Ms) z>*1~r-cxrlmu}rD?D^6&=Gpp}EALQlrHR{Q+x97$W-PM0gfsWkB`#^zxB!imGdk(e z0?W2`AwIUU=c8swKf?3(g?Z~%_ZM6uAwj?D)2BI8!6yG`G%-u$L@6x$9~f>~)hc5> zpc}SzWfkWAOrs{oKS)_c7u=fNT~tJOJVaM=fVC{7Ue@}KqzI`}NVn-Q0C8aoLe z=|3GZe6(JpW8UG*EuPY~zW4c-d==|!MFU}FX)-Kdv@1DxUn>Ny~6uVd2!+{3KiksnOR8&Mm?O^9&Xc$LdZCm+DTm zS-qPT%eM%jYwqqmG%taT5X^y+RzE6t0n0el(K~Z{s_0@jPe2W8Nhr7#PmQdB^4)9d zAqa{X8o$CA$2rU!5o(Vu^%}XrXikv?^-G1JuH@6>Rdg{-E8VJs?((|V5^c-RLe#p8 zwIAu*;Gab4571NtOV=&*>nJxPjx8t4_ zU#7>@i_`!|C$?&nulxF4FE{;JZkQFeiAj+QvP$gJ;+s5jOQli|S9CLsXz?!SHNlN* zawOJcd~LdRK1%qlL{h zt2@KA3bf6+v0HJ3;_0oPs;TRb`;lq{0_a$ihTcPW8<2Sm=e-v=y7vFl&LCS8BpxMvy;7_U5-`AGzVq zmmLBRS9PESJMIaC)irUGyCD@cP_^>SvjjSzq#hM!K8)j2Oxen#( zxrDinq>W2Sw~9HG8*~kSj3^AXq;z*;8D;<}k{0P3L}7hT|5GE>h|MR!@ew!@T8&n# zrr{A#9RN<=5t8N9k+YxHw`i|B08@mkg@lG!fLa@H9puRfU661?>W-m}i|dUWPvaZ< zdl$d)tx|jriUHR!fISrhM3Z5%gij%wi{_zp z;8ilRZ3^l|gItLS8wnKqF13(Mr=V=B;1H6vY62N`4pE<*z_;IogfdT=_Pw!LM=rpMJav4Qn7Qh+9<|Q!{v;18Jr`oTXX1uSs6aD zbM3<=3o(wCPI1C-56-uY`Z6r4Lw>h3Dp*2ls??2ch3P^XYV>5HXT${7RZ#0vAF{og z*Sa0>nC_^-@wFs}Wgug$2GNbw>O8Exb6q$c)n$;iGW7bdccWTf`QSN$A6PI?Q)hTS z>0m?C9*Lh*&q`FwMxM8?@tr^PQ8bgg|0gYBcL^1O=%sHK}#XMr2_21OrT)N2n+6$35?L%+5` zr8mg4bJSP%;mWlW>*SnWnond?q?NXQ9kM_(^hOolCynwHK%r)c8x!RG@o@GmQ6}me zP#l~OEvgDZ$eP1>YE8)J(zx=@a-}@7)T$bFI8FWAcy=-*+%>mz-reWD7 zM(|#{qblck?f1~_Ow2q+%-82M0s6R zA1Gs46Vl;#k?xQTPZ!Iuc=tBz#+TmVa?Rf6k8ldOU+4UYIu2a8K&&)^f784`#-|vl zW~kr$5hkCym^Ek_XMQQtN~VC`jb)s);10l0nCLevzz2ET9~L3}t9Gs+TCa0bs+Yg+ z*QhBkVU`O;>5^SiBd!V}%vbFIljg3uK(4^Qbt9sSzMdes5a0I0*(_T7r3N=~WSCfQ z{XP`!TC)nv*QWEx+i606A}{|_0{xjow9O9t3A}k8rL}Zq7CXp)JdU7tTNYl#qZPXfm#VVihzJ?+x>|w zQ+z}lb6&yzW~x5xNwKo<&44L0rdUzm%rf^3I=wPe+;x_xbU&xdC3Joi%-oC%U`gbH zB<6QS1lPX$oE&D6VK~oByy;*IYE4{dQfO0OKBC_w@w0(3U;S{*?)tYF6_ibKA2OD4 z*11bDtJhEYZe;e4L+zkPF!D@;=~|38dW;ExnG2@XIJJZ!Ss7+(_SVI{4jo6m_Mod? zZ-i8D#!%pVo;5%2si5spDFn#+ZY$Db0!*o;Y1OmTG!>q$O2)s$LYUZsiPuhGCNU|1<*LBuu?O1O%Mz8UoIGM?|Ur;p(~8gr=m%x@*Kw3WDj zuIpB-X|&xuwB0uMJTcWK!+!gI7(J(dg-u7T0!1VW%o;~FR4OgVx3jNp=B$fPFDG-(%LkK0&d|jwVmwPp zQc*>N=X!*EHAJopKX~=C^jaJ?-m0=agOzeFPa=pI{){=hFXV(W$>XvMsv0m&U{i&uV04yie}E8)la&X<9GaQq43@N*Qh2V(g9 zUvE7=ttpasyKV7QsIliLX)XYC$);-Y#GK|oiG7|vyjoEr8MNQD^_yOuQlShB&DZDg zlva+Pz4r6xILBmgkDI^NebRoExn2GA0B4Yfupe*GiF7zAbNgRV}4gN5=H&CTjNedLnm==To8jcB}K++kqK4?R%QlQ_Nb+P zBbGh}5Jp4PJ|)3~))W!}J$U9HhCBbnQ4k;Pc)#Bph@wTVMP(CI_Mxy$;`!{ENNw2S6_ zc@cTYNNpn`?j9|9C14NKVA0eOIq5CbSBTwI=@lBt8MXIZ0 zj=Al>{ua3v4zvig2t2(HJ^j4n#i^F8Ico~(?_VZGs<vU9bZ zM_jXW}YXM+-*Mx{=8U-8!j?Erzj3Og1x(@Sd(9?Vb$v zQ$J6XM!VWU;en3`5tKpnBv7f7yq?xYcFL<8~h(qb5W-&wWpw@F~}+YX;6 z$)ww%s>n@wvhrs`Fho>3`ItH_-VvH!6luNk^GDijNw;`+!2JZacC|L;VMj`9 zOk0S2w5ps%S@ur^2`1(U2Qg`FLm)~$jg9Vh89F5oY|fNftiqWDoT0HHz9YP^VePW; zER8G)O-hu66ee~8R%2XPTnF4Ni3o{X30TSnoE;pd9Kno-gn{=Mg9K)KwmtjK=oV)t z?*!A@BZV)uJqoi!?{a^=z2T#3C??7RIeG$tgMN1%? zfgvTB6c$*8VUb}F@lX>OcAZ8#R>h%4FF&f$pTL{lLg$3!8RD5-0)!0ogkyV>hd<%4 zN}j36VZQ-W{kn$mfKhD^PVoc_iY7X2%)+yfQ-zR{9n@BZ~aVn361E%2YK6nn=(YPee<$2ZQSvXu8shB@j({DR)Kn%|NxY z5z1~0{z@LS8X`1tN?R;;u0TbwPrf>G*B?SyNO)m6;b3rBC*T|eMpt>ky(SQ$@L{IG zvtT;ZY&Y!MX-$m{39)nyVA5Imn!pe=&X5?zxj-)==Twm4@LGU|!hrIH0Wer-82)HnX#UtxIzzzn z23UexXIQvu1JvFEJIX7O5Pto7?kI59FDUMjVF>v7(UQZ+dL*zxV>s~sCwTkmh>EK!Q=o|v75LHVL8wwS^};2gwSPn| zNJ#OLIkLe$f>_b}h=g>pSP(Kpdd&sNgN5Klm!$P6F!x`A93X&T_gU~0KyVQVK#e3b zvDWwo`N1S&Nub_<_b7veA=9{jNEpb?s}&PVE6{k=fDrE{%sJ|d(dpZ_&Ztc6GJ=69 zXWq6cg2#dRJN!x$4F+_8sEYouhz$Y}OY)aCCQAML&KnBrTz%)1u{ZPfoK7_ z!NigOaWA3pqXybP01Q?*ndEubEan2zG6cHi#9jm1-0Njx7m|{wCypRfvS98H2w;(Y zF~M*az($WpaK!^GD(rm0=TWZ_jVZq@zCs+jL2_fPu@*6=+ zq4J6BGI&B`W;0SPSSm3O8+2s@!M5SvOLLp??btfdv;GIuTj&;YPZr#*7zh3d3XYfa zd-G6MH`Uae$9<`2#GqG0)~MTvaVJ9m@G8i9tJ3v{BU2u{5Jj%Io8#snijynGys+Cz zw6`zMH&M^K+uJW`Z{Ex}I|?So6?2z2RpA_Qy9~JcbxcK=5M#E*$=olLwVc$%DGTh- z`++|}GSG*UYg8hd5Ej^lCJ&0^E8s;<@X4v!ZS>7NkeIJ@1=zh*|C}E7oeDxoR+?I# zp()Rz$38*9a*9J@pek`O(9Q>k{Pj85_kLT51KKRZyLy#OHDR+NlLkwGo%6dGbt?6P zSffnG$*5^T{u0b0!Cp0i$XL|XGFq~5ihhv6z7pQ=u2rojbRWySHYv>FoB96P^QM*97LDFkx`zjc-?V>MDar z5UY5`1Squxn0^`-1Jij?3D*dwn*K&iG3m>W$5s`r-x@<5&1zI;;I^?C7BUDAu@=QC zWwL|lG9nvC8?hM5#$iI@M9pdjLq)^R#1>4Wb7Lchu|-j=#bY%RMc6Xgy&e$Y7^vok zqZxe%7%hnNHbx=*c;o|=$h+bp5h2?8UGU^Iy__-FEL(k;9ur_-SE1F9QsQHF^6G1? z)^a}X4T4q?1Gh?h#S-Zw*D1!on*5>$&ZHNn3soW#`Zz2aU_V12SmvfB_|E~pO{h4{ z7YDvSSnmcLyU}rSTn<3~ZW2SL(W#=bS%>&xs{t-t+t+r`iqt6EF{zp6PxuA$C`%Lq z+Agk1-=G@%5danq7R?JWq!~7vO1GHFDI=9J0{0-1GSA|IfQYY!dm@?^7MZ=6u?ol( zx5k1Ese(a<&OX51CDliV&c>%TF68-mqOIewfh<{f2)XyOmnx6qP{unJWe!UsA%T{N zLA?eOLfMFO$})+rkCmmhm!f^HJ>tk1AHw(&E)!Lca5T89jpAiNmc!fQn3dMJB3_yU zt}}s*6>~PGHm!2}bLV}aBKK9cBp)pD&jPp5S|Z=t$gN^P_}Ca8A$A%C=`annBu61K zpw6ko=Lczq+yi(U)cqxy+|pA(sy`c#J(_w;1^5(itcV68sITWktuQnX8kC4eWxrjB zaiK#XK*@lM8Yv#7cJ@YcU~3SjOK=}e6}dP7axW5p2@G=!s>=sHrL3gj!&_xwMyl6N zlFf?PT}zH*|Ho_K@aO@tT?B)|EWicb>iLe(*O;Z=UDm`}8H43em$Hot)s(uS(og`A zgOSE)kvtRg$Xj%rO4BCDox3rg?Uj=l*rpl}evg*_!N+!8!d#P#``*L$2zDZE1}jAJ za##6{#YwqAN8YkT;+FE0VW)e7N~wR@gsK;!`IruIq(Wsg%nwZ0$EDU zbM`1n+sETYhi|%{v*~Np=^If2pdh_B;rVSTE9G^fQL1%WrdZjKtizt#P}Z302+i7s zv;OD({dT{@Q-cs}!U}gdjRadGUDm`kLakLIR5Be+6|1x22!Xmms}2fhAzhX3WKahq zoN==6(2BMbaNH;MV$mEtpQW;{cE}e#2~(sx?$3xdy~e!DX`5=e$v(@)03ydl6Rjv_ zeoe<-9FLtplLeEhSpM6`Nd!qu=9kZuza2uX`iSu#o0`n6uyhEu6_nEoC_f|7x}&@?GS&fp zL*y&-V{pvY{@Y|UsGF$m!Ai`G>f5H8`d?cuxlhAB4!qU2?5bNAss1jgH?yj)S<6~I z6bL6Iq|0KAWQ~2Q;c6%SD0GESQ-K+WIXABrv}x{TX)7dj28afT2J#Fw$0m8j#5Atl z3qR(7hM=qj#~qexDso9s`g^bZ9bZOOnZA=U$mYytct{J03W*9&Vdt!CHBGWwoBc7- z&*0YEido88#;~iSS+tq|w2YUtjA>Ci%y9(&P*pLzP|9);jiy@Z{5iJi&ep*nd#fAJ z=&{dBNKnPTInPC~?pVzn;FQ`fTi6QKuR{sEKZMgCjIPbfz99$_d*N!xYS`e*q#2jj z8O}9%B-JWnKuLpAmDsNv3Pi2bT(Bs83Y5miR0oyDFpZ{Ktv-n+d)fnxpZ)-@+QuT7 zh6=^T0>UKy1)mv#ihI0z9jN1$$&tdCZjFq;8I||DOAC?zWS}2krhagTSL?A>Ul=}q< zMpI|m3@SUqtKd(Xx8*+As>XwfY&_w(3lvz9HinM&qc{yFUMbm2KtWhxo}`r1P((p( z0)HDH1x-rQ#Qt9BT$Jq^=mGnR^(xp5K4}Bv0h?hH#<+ZPM2Dz%i^`1>ej{@ho33zw ziz!#09bM~=QD$$ES&Ulx3q(jqNJnT>TJ1&#eMQCdlsUC9Nyy8lbxzlWPEwl^8lBsq z;c=prc;>{!T|4Wlj|hzQugid9wmt{6OF7%^j7(^oh6EU~?8J?g4z@pZl&ml5n=yKms+%>sIZSEKc#+QD`ky8B6JUZkd59Im=vw zTY_7Bgf_MWWoycNt02~~ojFE}5;?bX7DBy>c=mNAbRISh94-BEwsme2^Fs4N^NNSZ zbWE7Pn`pyzaH242uzD+@GEeLlBeiWb$5UPC-R$UdRFxhE}uUjKGDRmr?S=T-8lr5)=T4(h@J*Rf8tr%zcG;-dR zor9&fspO~UG+hO5!8zTv>m(Mk`kdADORc z3QY*E`V^|X@@woqc^a+d1jkR4k3sVw_HyXr5J64DaV z66?~1HlYBmy>?l9)mNCfvV~uti{zRaJXPga1(Lyl|M2Nwq|2a;u~${29i)sN-z`6; zj{{63cBk0?^=RnO$fqS&L*Al^zAHc9!2DKDb>!gE)QH(j{g36gzKgVw9f^yoicEAW zeRnesOUMJz%$3AA1Z*S?2LF~KGG{ebGA-&=h z)@)C`9(&H}T=KskFf#KbJYi4ij`NyXgNEK}>{-l>$@pl%+*IAYR<|r3MbJZ zG*m0X0c`E$YG}TP-C7k>Q-qL7ci@DrZT|}DzV9r3X3r0g#f2;~uO+Z#v3wA)I*m35 zhxiuX1RO~YS5zyu6eXsqUP4FzNX(MDl{T~SGi=;|FzS*rC-SGn zC&b?d#@CwEMVO$shNT`lAp7 zhys$gG9#tC8uz?8FYo3$P#u+NKs-P^KwiJQJDo0V)>oEp^oTwzAaM)LmNsBn1u{W+ z2Q6xx5}Q^yw(-OrT*@^94ju|WAH71`(uHFsGz;;id<--&l~BT>=IWO!)@5MtBn#X? z>xz^FTtVjQ<58@z6ZREtL6jRaA!QMoSjR|!HT|DN1lQr0Qn`W>dMtUx!Fyn8Kq`RF zw;?O&L}5OAGAWfxgHpgCHGtLp(e*#y{Tqv9@(DGR;ghVYfM|ME?a{Tt)xY_)ZK&|D zM)iM&MA!EK{p|^^A&*vZ`Vee8*Q7$sqsBc{khP6Bo=u%SeJEmz-$%GkX^ALGBX z{R&E{&)4^HXN1)U|2xxTNAH?a?Tth)?Z5GR7w?ZOr3Dv_!0Z42!RZ#89kQ*Mx@Cpb z@t?Yy7HWKVZ~G*_PJQ;V-kt(gOekzz*(`1Po@;TYeL7;N(2ScnTa{)sfrp?P*t6o^&|!pi%uhNygTqcX4aN`|#1?b}Ahb-#H=UsZi9{ zv}--H?aHHTJ5wM=AUGh<9%Z1LyQih^%rpS24`MUaseO^Xa{5XJN~_Y|W^bC(oI7_Y zU~$tlKEckr{?qi9b7YC-LYJOpy6}CueQfCrVBy5da2L0MbvRpqW#o5oV3#GA-Kn3e zt*}5}YJ)~XMg7dEh`pqxq7o?;^P{YP61k1E=#M@^)IcGCnK?|fp&9lqGTbwK#EK-< zq3MY(n0l0_Y7FGOnZNW;FS~Avft&X;=&s&NNX6sMY1EMlYxB za9!{YMyMcI5ER7hD3-`RnZTG@BbnAhDqm^SiJ)cF;yj@VV|Z5;r%IaABA*mRWF3ZN z2Bg%PLV>xkQjyLBdnFD^b_3(zURE`T%&*lcQXgP4OG8s*WM!>$E3>4aWF zod)yMre0TJ{C7gsz{^7ncy{ia(6B`o#A&zfNb036zqL;eKHbV|QvgqgefxDKWdA#0 z(d*kt+xLaBJ2yp1%WU^43o!k4quaCm8dgL&-rU|8%+Ag4qxq51{k!s*y}6ZQ%(~f& zVt-gqeXfC?=9iYkYs3=h<)X0|rqFHq7IRP?%i!QW=9iLBCPS**yk+Yjn5|WBN+=S! zNxT-Gv+=`W`QM+MsaeZ#2_s z#5JK!avY}X?o^{3t7)_EE11NjtVUj-jgRtaV(Mj>;)w2ke_j+oD+)$46R0t-EN(%Y zsuZvMEQ>aaWX_<8=|^K08KF|J}U`v}k;%6Xc&4|7p6nfqL9@a#)D1xz8x zFSRpZpr?+%4Hg7$R>yx8c@r!eD){_du2--W8WDn>Q!_Vd4NwYF3R23dQ93`^QzSdE z(LM;-txAc?e9$od;Lxi~1$;CgT7E6xJzZJW{+>{G`)1O5UG?>$D1|8xEYCYLQg|%& zwbd%G3q;9f({+GNorkocR!9j!dxD#kMKS)&7RQnslK=pT`6KIR(_d{H!}uq>r9QMowAduYTt?HWh9z&gl+9n_h8|V}7BcBpFK?FC1;0kZ8+J8Y#Sg zjx`ZG+v(fEPRnS>IM_{P8Fl*RGo|974K{Nj|L_7$`goT#Q92m&Ynqa*&vr_qv30VQ zvHj@Ywha6&vZkG3f1dtCvYUFpBg?d5YMu9N6wa`HPO@2Z*C{l|tR}&>Bt9lUDEN=8 zA|d?krm_JlPqLWE*+_;4{;3y$?{Ldn#ih6Ns`&-+@hm7>>97;7T;fpBO0qiG@$O7qsVc=!q_w{F zLHy=jbw;%CPRZtwEt>RrEydR75YkBl*@oackUSt?vX?N0tQNDw0wT3F&3kgYQ0Bmn z`LS&F&^J8AV0hmznikRLw_xqB3~t+232IO9OTF8NRe}P2E(3=}R6OEl0{BTg_eMA$ zdPw?O=lOZ!IbmAjyCqqYZxZX|1ov!LRbY>(wJ^VgtanNopAvmu+!NjT6$O>)U2peL3o(+B4jB zm%y{QOotk`JJ>gf?M)G2gV(*9(}ZUn#>Q&hj{d4G?!m7hEjDuen8Bs$yit7S= z`cwX?#egMihE98{8MCU8ufp&5=XZC#-wQj_B`xL#<99!c*i60kZwzFMc7wlOL^F@y*W{$jEr%bza(nc3}JXX);R?i<9plvmY|n3c!VGK=86X4;wN;pijj^I6F=+W(va5BM=IvTqOXD|Mg0s^vW78$3wC;T==8T++gqB)i5^z#X7l>K7XAQ0-wpI3hxY~`M{RPrSvxfZ|F zU_*jzBw=q#l87c10X_5GBkZOD`q!b$(un>!_|gjFJeXAvPK_ybMFt(lz5F?;=|KER zDP-t^?1Z~?5h%;RDD+U{PbDZ*lBm#xX5(PyInKp&S!n6?RTYT?ucd4$TlvSlfM@jM z>z^fu>bYGMRa!hg>|aVUc5iaxGTW_;OO8VsMq3KLJr|o%{)}E>YC&pQ>1tUJ8%aaX z-GNkE>6059?)|HzKUbHZ_6U3oiG=XwqC>D$_6Bm5bP5y0G#5qrMD$*g5KF3HrS~TT za6-URB%oNK{}~+;GfA9MOd1hlkG_sDPkcIGlGhp$O20kAnEA&H=``HrW~F?)rt&>6tKxyopa|+Xqrt|f@M~7ht)#X|w3i#-k#CJkdLW5&5O@&+xyA+b=0CI_=c>%H)@cK}A60vVN z(KB*gcsJUZbD9fJW4uXL{Ac_*{ymDY?@6*{dWw-!3c=8jtbap!P8dlWU-y*Xmi7HI zqrNKa@izi4(aA^Hl;0?e$Z^uj)^B(QTE2j7L8worsJaAn$;&xzmSGB6e;OOaEK&(*T9r}7f?+t$Eme^ z^2)PqlvFt}ja185tdxhWo;7S-Hy?SfPsYaSbGzN|#|pDckm zJ3jy!m`R`O_xy%l50ed1(raP=Ho?iekXtN*h-9F8kZ0PKT9JPUi|} z=Wn;uCjmp^_n?u%01>&y3qWL4z*EgX;T3ZqgeJekzfz5Bc=Tr8 zewrJd#*(5A#)ueLl{j=XvXDiAt5(!#=+|Zk4$i?w3zutnoQNj_ktic$fsf)~%n8W=Kiacb@-u{gx%N24o%t4u?dhusH`Jmi(5M?{v6 z4JDQ*F4kIZ^azBux_{k0GC&#TMWP2o|1;`YsROM(x(2vffU3UB1_#D} zTO%ABi}u(M9DFo91Pk-d5t!4gDV#irGSBgX31DuyAP>$se_1NGdHNl=jWckAxiw3< z-|wlmsrH#%f-+oB`*_t1ez0x&KG)!BQoDEGxHnP}RPBG>Gdx^^myP+ZUE3w&h6UjJ zRkmDosFm+TKqQ5hpDAJ%;CMR^-lj=v^Z2!Xyqw-|TBm$+>4gyiT_g+)Lf9(HQ!DfAIf3p*U6#F3-Xp2xtNt z=D#Nt13>}(JNex`-^Nx42?*%#g#`ozq+;%3V`yt&VeQ1={J&dzdt0*z1vzne7#tWN zARu^22@xeAAmG1e#y3KN009B<>o*Al0U?n}iU_K>XJ6*<{FZO)16L45mY~|0(V&uZ#U`dy!s8+)rhXfdZ{Wn?QU=3>qM|3x`CXP!8joTG z3*Fn^!kz75=iTrA<;~5)&GJ4u^-QyKIdygM(s`Zr`ZY1(>)_}(ha^(zJ?jPvRUj_` z99~WY{-Zn;&A${!38=vGZ$mA0&)FHZ_wm(Ll_NYiYKbCP0xSTeMZzf`#cp2fwd13t z_Wn%|#u=#+9mSoIvgh%9Sq#0~h}eLIL#ziGNfB)pAc5 zK8=GS1*YaX^!&j`O5us2quQLv5uO}IP z4|jJ;!m8?ONIm)FKtbU6Xjx{e%!Za0jtrTXV?;te%A(~e9c6moha+hRj+`%!?{8xM zk9#}*i_1$OdemrH$O85Vm#u2O?k>OYuFF7xdNs9K*Y_8DQ&h>k-)GlcJ}VLr zh*-B4-WEaEWji!kKuMB|s((sKO5r8aK`mKqZ0rs!1_nm-G5_cN#4dv``@0)g4qHQc zfy@~`aBN)MYWJAukF1U60*(O@sur%^K0Q1LyKM+f+KzGd+wqwuBp;pWq@@L&H|7}-2NCb<2}4e|D?Iz6aV3j)bAbl`lBCLU z$oOtIK9Cnfw71g-@jGCcB6{%n)s+?Zj~9pJ@~_)rz3)?#qW;?}!3{L8UT~i;D|)9lygd^q0V$8kAWE7b+l>;iw*AY#1bI`&%$V zDAMG7Akr2Q;xPrFKnXA^AjO|9RQ@%8)YR12*w_w;Uy&#W^^!9}CPzQ*#O^Zex-A*{ zgt1}bN1zGfh8?NWi0HMX6rI%iqS*_m$0jz#@3cn~Xio%?A;uK^PvmM82ehIB?Isik z5bw`{&iAJyTa=M= zRyh{-<2yJ8va_?}PFDu>Js}&e_2o7;Hlo|xnHEU%S6Bl{6n-09EzQ$eMKhP9O()1$h??4avwYnsAgqVs{hJzjTN#R0S#sdS%6|TvStSv z2czz2_+C)U&Sk<8%9&6zv+*FW8j&&0w7Q? zrf8wG|5KTDMm4!@+Yov)1dt-VBOqO*NeAh@sk9gZgbtx2(jr}2=tV*kP^!`dK>`X2 z2uQC=hoJN#-8VS*`8=NEx$oXN#`h;*=3INP{f#x&T>J6$CJ;pfJ(A-ZiEwopcg@$* zRRm4UW4w<#QfS}L7Y{UYWAE{CvQF~@047%|fmV@?=4Vn%&P1!pnW zva7AOA{+KRSjzWXs0fYP;(d2XIrUKc|J+e!~SXD=7Rt|ZWegP2hwB!v}X z98=DcS$Pna6sJWoPw?xv=6CS?9(+(>X-i|7Jvt4Af#%tZ@?wQ#sDTx{YK3Jno~;&4 zZ9|3Lo?(w9nj|$GZB)RwJ+bp zERY~mp(RqWu!ME%{$^@;3Cd*~C!n{X#IN@7(|&qy^s8)4M_k!)b zJ;gz58fNZ9IkB3aB8wpQ+}P=N?dyW8?qc)ZPBGtbDX?4m0Jc>nDrc0pUgrK@N$y!p zEj&cvcxxJ4*uxNf0y&ubvT1h!`rdnV@JuYSez%+LStN@Ua`G$_Ahi)hNZuYx!82ah zV?Wx{&pp8046D4>du~ufLh(FSsOvd#QF(R!wMGI#s@6A$u#=(e0be8--$0G}-6|fx zoI-}`uxgaXaX5x*5O5j2kOLX|U}c0m8(Vk?02I+-+o(pJ7;q?i={v=467(O;t5~N% zfM6AN^kvA&$%&Sh7RH2OlUxf~%peGFUhWPxMT!v1NlLC7Hdo?8qJ?F-Il%a`ub)H` z*S>mHo&!-<>FVN_3XQJ=7l)MGBHhNxnEuj@Ml5uapF8UyZAAKWfR&Hyl?`h?ax)>PF%Ta(^hUW2n%O?E-->8_=y}-Nyo;+JJJftW^J_39{ z|FR#S>g5(~mMg^%7xsRBja&Z~lki}n|G;qzKW$rN{65lz@)?&TKL(epJ?d#FwG9N} z`8|6p{Z;Z+?O8%Y0QZ?Mi?{#D=Tcp!(%mXSY7mwBgQXh`j3H(nYsS}C;-oM|xW%(Y z0E@ks13mU{{HGpy#=YiaV%5ZG;2@)Vo^OZ(-%uV8DwE^gX)jcevc42=zeXe?%!+Tr z>~1eFN$BL{bk95E^rg@{99|-IoQ$N7)0eaA4i7!sMiO#n!yH6HXIUuUCo6qgDomCl z!v)VN(P!F>!8q z^n$F^tyj*+pO``-jB6BtX2_*(P>ya10!~T5lffH*DAaArP4qs}YIlWQBAV6NFIz#D zVlVP_Lu32Su)$fiuc?{Y9I215?`|z}HNB0Oq8eYhPf7`sc%LeVO(m3wJwt)VZ^Lyy z2KW*M$tHk4s`1IC>x=-a=QO$VG)^aKt+`wY@iR_oVJ4`CsEph3T`ldwD{!F97tT@1 zptWkQol2HNT(Rl9uCebCJ4VH#)e zXhj`vl#K5NUF^oEmcY4o{Pg8~{C>%=B>+LOxs+bBAt}lBbi-r%(@vgY9Fhi(tL)A+ z`!caSvXye$#N$&~Rx$TNiD&{!>L@MS0X3*w;gKMg9Z&#c#IrXgdU?7DH;NUVo%CAO zy68r*T*Kidw@7`%dUTK;gZLsHt@KCDsU2wTtjqK;?tv9>2~d>M>tLbk?h zuVpj$;+RI!&~Hlt30BHTIZ%fN@}-4^a3r2~ z?CQ-NX&7gFK^0LCifG7#Mc(bxtBGEirBGAQ$w9^42C{GN`JX4VAJIX>85D$yN1|Eq z2Mf<y_Y&q{hjF%B@ZJ>2rk|kO%_U*mX|3PY zO!B<>wAO>3zuiz0lmBSVnE2@fGcqrX5gWijf+n9bNcUf_TJS`SxD1y+>Q%|8h}|g; z9w+OL?yR+D{qVDuaw#FJKF&R^WDDk$(n%{yh$sT# zApTvLH0y)BvX+2zuLS2c7d!<~a4E4*7>-5S?Kw>SIlm2C2713vdJw@gc=Y}gsFyP@ zXnB*~{r*&l9MJCr!8-mhYHqeus>!4Kx~EUst?SiimYOu$Ib=?Jj0s__EqP06JuM4X zLnY~{5=5+Q=cJ;}O5yn@W8TeO0_MH$6dJ+2)c7%N#aiiBEde9G{C#Kvj!x*Q4aYQb z{8W6e9+n@>T?o3qlR>f5Bm!sDp9VY_S0z`m8%x_rHK4RQZc%3pdD0Uj%&FLBQ(ZxX z+V%{S=io+WEi3sJY1x^*Caa?&Q{Zop<2?X<#cMvKv(nun`>rjFDdPvHIjCl&gA$7f zrTEWmx0{XB*ahCIpg&PT@CYc?LJK%kC9&FSH?7(Fpixr+`ol58oR<`+o&W@sG5A`x ztpR$W6SFQ<*T`PzQJ-{uB#tOl_@vI?z9qE7H$uEz{TVJx*r;~oofIXPGZ(*~vN2Uq zUQw|P+MAynCgTAw*IkRzwp)^IW+@e=-=I!DamM2Ve8%HTV3{ThR2|pgSZ=H7C+{A) z8o;fk7m6hog?5uo6;vzW&zkD5(M6R8zvFMG5y-=(V1Q4_9}b0H2&?FnfOL1 zFEEM}j6cr+1-s($wVj`z4-OhmKUpmb#EZBLNh(xOp~%gd4G#}DU$364#I*{gU5plv z0m3-kRq0S#*?U4lLb3hM=tR&FeLqc;#8rVqUi&Xu0${cykLUsg^{_Klis$TcXU#1w z>xMF@2ZGzD9XmOl*5rdrI@DF-9S1c#F)t>94GI)gi$e{-jqGH?58~QC=tX#cFT+Y{ z&~lM*7Kdh2j^ZsqB%_7Hh*u`mpB~;?UP4YZiH}jr2rd{1G6=(W|4pnOmdw5TuyXLj$ST zarjf}(iemk<^kfC^{Q|#%}e8?3$fX@t>+t9?Dh29kwJEN!sqY`$AM#&b|Oa}I)$h( zD0c=&GDb)9`c@Vp+myUz(~%(m>yVjzxS+lS@D_mf_ z%*SGh-57~bX>jRPsOoqKv2WNyJ?;a|y~i@$G4+yVwS0vNEeRcATVV7Xl?<{9hc95V zdfpN;Dqb4(RxcpP4&R34X`byMM_wezMWw8W6el=~gP!mG5m(7!vFK@Q>!D7BQOs;d zQ7RtvnY^Z$sB!YHmhQF|gaP?QG?Yjw^v{0#ZwCCAAI?FgV4>izPs+J2rUx3Tx+;}Q zHevc&xOmk6H<)2-IscbDgcroi8*vZf`EPiPzgwrLo4qFt@&7O{CN_Uf z&&5jreZ1eXE>iN7MMOmO$8_k&FH?bg4D9jn=3Gs6C6oPttdcs`21pQo`aRC=Y zrwh`>bm#*Ad`Q1rjGeO^#0&N#Wxrd@#T*LZ1DRv1^wbdM-Ltl7 z^lWM-BSBu4d)gl@YR}%Xc66rLpAY9{PZO*R$dfYTNKZupEGge8p>pq>aoA7;UH%;r)?{Kjpu{&dZtx(+-kOWZe{BFM2y_iN|Zr^ zM0#&?Re9?AL=xwhN|XVuWL-&D!I#CZoaP#L$gFU9nP8Ga+yd{n18%IH(`Ms&0s+*nYk4ap$E0roc+Rq*GHQRBupQ-2_bO3u>y?)PERP ztmzl{P61xllxQ$qO0rCLlZeILZ-!89?DK)+#B4Q(wlAY%>boZ;duWP9{wT+x%8*A+aZIM@Tj9*on zpK+L<^_jCt#_`aSoAY!ezS(AIla>whHt&jAK+;0z7EIhMCa7G$K9dHaaL%u9QLOd9 z@be>Cdk-E)QTfoxlfQd%jEHBtM9@Dmiv^bs_34L0gyY%?N+~!?>^+xP4tJIyMd|&Y zhs=YmR))_j*Hv>Ex?h@q#^0W?i1D6`nsks+8+oxG-*GFBR!7lEx}}lsF_%KZ!9YUi zt+|ywbpaB`8eWHJ)gd^YX|~_;%z9&b@aZW^hG*_fXFf9riVOcwgJw=p`|2^p|=iQ&^%w(8>g8kIhzblXYcLV~MNEPI1tX_3WL(%p)cU@mcN0n!cS_P(4tn>(dq7=rr@g71B9aV~@UITyHG>F8%@Q&B0~sT?XOq)k zJlbGS528A4-^M;pgm&Hos#5w%u(>r`z=Dr;e z_&rX&U}fS(98q4C<#R*!exk`vcD^{ixryXGk~-(q2*+ZUo&A=o04Xg|0q!`){wA69 zl<9C4jR=b?#58CD%f{#^VQzJ^4_XEShO(c)ZyiYk?;Q&|L~Z20MJl5OL=@J6r%?ig z##S=rnoJ(+`Ibw=mK`zo*4N&~Ru?XkBmy1nrfWteIJ~ z3~-*L6tNfUj6$qKjEG+6y`#*K#X}pqJk*ERhL;*&ffx@Gu%>D&PaMP6CDSj#^IWJB zgc(&-)LMr0UR}O4Cl~hOT~R5!QswQU1yComQ{#5oK#|he;-u%jVDpC+S+Yo@n6hEU z4_QF&#RIH#*A4A>`jyLJj<%b`gz-vh?9OG)6hL=;dAfxwX*Pl>?eQk4xFPd=TRql^ zdvFc1DlJNE<&{`gz7YaTaF>+H29(LxvJpc^>q%c=5}JS^z;FF9#>x1NK>gUyf#DEzfrAN zs!P=-yvtTCF?^L2&za<5fhI3O4|~3xBT3S+xk)&6M^k=HkzFg+puN;C9-9_v;IdSU z>o%%VW7ue80=doZ5_Lzj=8nKj!2*nAYsO{B1-W~0<5c70XJP2lnkeR>rd;>a9#>Ka z3`tb-6R~02Z%sB0udL^`jTI}fO4!UbtK<(RPJ~O#qenWtTiI1((jvp-O6#*Gv2QwU zF*ng$la?toDyCIM|NzrVo}DyDN($h4gR^Yhm>o@}9!pVXgszdd=Kj5o&p`B@318vD&?@1%o6? zAy&=Dacfm3mns$S%5++TET^C;N7B(R@Hu&E)81weL9%WYHjwTh?cW|3S{(sqk}f?d z*FI5W@)wtWp9D_+ekJ41>w9$LLSEi|#F+e?YM$wS;u*c9_s=aGI3$B)!qO+=pLC%x z2U=;9%x^sCTt5p3G6W7RC1#uWXtlR~Hl#KUi#WSkAz1Eqyj5iTyoj?s^3zy?z^ziD zaYlR>FM{?qjv_FHLuG;a)jM`&4A=K0Gs2Xa<&r1Rj^HGG!#FL+Vd`yiaTpm?~;woMCx`-9dWwQ=cAwjMn{a;$S7p zreZWFLeWxJLO*7tem)^vu}X0zB)D!dEEj@;opuTs#jov?%=6`qgu}u`GO`R?s`W%! zRn_ayz;uMc@061#@Kf;%XYkz$oQ0_8x##$){la6Y1Ks;S)CU0zE%=$lp1ZoIFcy(_bt0hDtyBgbeI&k~BqpyKC@ocx<5 z$*$s?FE-X|HOL52PcS#_TYsszH8yGK*0Xad(`r}Gtm-@HqTwrL_08l* zYuzpb0013TW5sJ4x@tl$5LdVz4Dm5?O-Kq`Nq$_5n53~%(V^~Y(<{{)N-ha z$$&=JgXJm9T%ME}74F+XPg?A64t?$KGv$eT@8>+h9}KySKFG!7!}1~%jvuZ2W_Ml{ zzsZ@Lj^;yTRKj~aRj?+a8U>z-oNz3z&5E~A2xss?PbN0&8lLlntI^D105o54*W;D< zw~0w)6Y224LWme;!!pR0iFNa;fV*sb(`FbdjA>_To_S9WmTBJrT64&rmxnE@Z ztoA*=Dxi*n@qKkY!ck`UcG)^Z>kPRd`%_gEt&idq-FU>chh2AsAMf+}k zEdeRlzQHQv!sqO>gfEz8O?n<;S%%7VBL1_IkL`sUH|ED!W`b}plKK>|)J2H{0DQu| zNGcAH8t~I8<>3mx;RgT!7tbH<_AhXOyTH8s-C=)at(A|q*x&&G7tgOO0{{Tv0`r1g z^wd9CKaTLT4K4vQJ^;YL1ptu!5@!q#06@UJyx^|(e{|!2W&Zi{ne-Q?5D5SPb#wLl zU)%75_)mA!Kk+f_pKq%Fs`8Ts_`^>Age~gt?&)vl^4E<2Bm@4imp_sJ8?ir|&Hq$q z;x{^fHlP2g4mjoSoBOjF{b!vEJNgqEztQ>Coc^=Yg}3{O@ZTu?G^_uq%FExV{4}rs zQu*=CUc~<4Krig;Pi+21px&QzG literal 0 HcmV?d00001 From 7faef870c8201696372661e063336069d91123b9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 22 Jan 2015 09:24:13 +0100 Subject: [PATCH 595/652] Fix UAVCAN dependency generation issue --- src/lib/uavcan | 2 +- src/modules/uavcan/module.mk | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/lib/uavcan b/src/lib/uavcan index 8966f97013..7719f7692a 160000 --- a/src/lib/uavcan +++ b/src/lib/uavcan @@ -1 +1 @@ -Subproject commit 8966f970135fb421db31886d6f99ac918594a780 +Subproject commit 7719f7692aba67f01b6321773bb7be13f23d2f68 diff --git a/src/modules/uavcan/module.mk b/src/modules/uavcan/module.mk index 924b730a23..600cb47f39 100644 --- a/src/modules/uavcan/module.mk +++ b/src/modules/uavcan/module.mk @@ -57,7 +57,7 @@ SRCS += sensors/sensor_bridge.cpp \ # # libuavcan # -include $(PX4_LIB_DIR)/uavcan/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 $(PX4_LIB_DIR)/uavcan/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 20a8b26ac8c34ccf906b7775804a59afa1311f19 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 20 Jan 2015 20:40:57 -0500 Subject: [PATCH 596/652] Fixed coverity CID #12538 --- src/modules/navigator/geofence.cpp | 32 +++++++++++++++--------------- 1 file changed, 16 insertions(+), 16 deletions(-) diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index 4482fb36b7..4fa5806781 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -257,6 +257,7 @@ Geofence::loadFromFile(const char *filename) int pointCounter = 0; bool gotVertical = false; const char commentChar = '#'; + int rc = ERROR; /* Make sure no data is left in the datamanager */ clearDm(); @@ -269,10 +270,10 @@ Geofence::loadFromFile(const char *filename) /* create geofence points from valid lines and store in DM */ for (;;) { - /* get a line, bail on error/EOF */ - if (fgets(line, sizeof(line), fp) == NULL) + if (fgets(line, sizeof(line), fp) == NULL) { break; + } /* Trim leading whitespace */ size_t textStart = 0; @@ -299,7 +300,7 @@ Geofence::loadFromFile(const char *filename) 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; + goto 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); @@ -311,43 +312,42 @@ Geofence::loadFromFile(const char *filename) /* Handle decimal degree format */ if (sscanf(line, "%f %f", &(vertex.lat), &(vertex.lon)) != 2) { warnx("Scanf to parse geofence vertex failed."); - return ERROR; + goto error; } } - if (dm_write(DM_KEY_FENCE_POINTS, pointCounter, DM_PERSIST_POWER_ON_RESET, &vertex, sizeof(vertex)) != sizeof(vertex)) - return ERROR; + if (dm_write(DM_KEY_FENCE_POINTS, pointCounter, DM_PERSIST_POWER_ON_RESET, &vertex, sizeof(vertex)) != sizeof(vertex)) { + goto error; + } warnx("Geofence: point: %d, lat %.5f: lon: %.5f", pointCounter, (double)vertex.lat, (double)vertex.lon); pointCounter++; } else { /* Parse the line as the vertical limits */ - if (sscanf(line, "%f %f", &_altitude_min, &_altitude_max) != 2) - return ERROR; - + if (sscanf(line, "%f %f", &_altitude_min, &_altitude_max) != 2) { + goto error; + } warnx("Geofence: alt min: %.4f, alt_max: %.4f", (double)_altitude_min, (double)_altitude_max); gotVertical = true; } - - } - fclose(fp); - /* Check if import was successful */ - if(gotVertical && pointCounter > 0) - { + if (gotVertical && pointCounter > 0) { _verticesCount = pointCounter; warnx("Geofence: imported successfully"); mavlink_log_info(_mavlinkFd, "Geofence imported"); + rc = OK; } else { warnx("Geofence: import error"); mavlink_log_critical(_mavlinkFd, "#audio: Geofence import error"); } - return ERROR; +error: + fclose(fp); + return rc; } int Geofence::clearDm() From ce11cc9f32c1d86e0ebdab3d114517995add2595 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 20 Jan 2015 20:42:01 -0500 Subject: [PATCH 597/652] geofence.cpp format --- src/modules/navigator/geofence.cpp | 84 +++++++++++++++++++----------- 1 file changed, 54 insertions(+), 30 deletions(-) diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index 4fa5806781..9bc9be245c 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -58,17 +58,17 @@ static const int ERROR = -1; Geofence::Geofence() : - SuperBlock(NULL, "GF"), - _fence_pub(-1), - _altitude_min(0), - _altitude_max(0), - _verticesCount(0), - _param_geofence_on(this, "ON"), - _param_altitude_mode(this, "ALTMODE"), - _param_source(this, "SOURCE"), - _param_counter_threshold(this, "COUNT"), - _outside_counter(0), - _mavlinkFd(-1) + SuperBlock(NULL, "GF"), + _fence_pub(-1), + _altitude_min(0), + _altitude_max(0), + _verticesCount(0), + _param_geofence_on(this, "ON"), + _param_altitude_mode(this, "ALTMODE"), + _param_source(this, "SOURCE"), + _param_counter_threshold(this, "COUNT"), + _outside_counter(0), + _mavlinkFd(-1) { /* Load initial params */ updateParams(); @@ -92,22 +92,26 @@ bool Geofence::inside(const struct vehicle_global_position_s &global_position, f bool Geofence::inside(const struct vehicle_global_position_s &global_position, - const struct vehicle_gps_position_s &gps_position,float baro_altitude_amsl) { + const struct vehicle_gps_position_s &gps_position, float baro_altitude_amsl) +{ updateParams(); if (getAltitudeMode() == Geofence::GF_ALT_MODE_WGS84) { if (getSource() == Geofence::GF_SOURCE_GLOBALPOS) { return inside(global_position); + } else { return inside((double)gps_position.lat * 1.0e-7, (double)gps_position.lon * 1.0e-7, - (double)gps_position.alt * 1.0e-3); + (double)gps_position.alt * 1.0e-3); } + } else { if (getSource() == Geofence::GF_SOURCE_GLOBALPOS) { return inside(global_position, baro_altitude_amsl); + } else { return inside((double)gps_position.lat * 1.0e-7, (double)gps_position.lon * 1.0e-7, - baro_altitude_amsl); + baro_altitude_amsl); } } } @@ -120,9 +124,12 @@ bool Geofence::inside(double lat, double lon, float altitude) _outside_counter = 0; return inside_fence; } { + _outside_counter++; - if(_outside_counter > _param_counter_threshold.get()) { + + if (_outside_counter > _param_counter_threshold.get()) { return inside_fence; + } else { return true; } @@ -133,8 +140,9 @@ bool Geofence::inside(double lat, double lon, float altitude) bool Geofence::inside_polygon(double lat, double lon, float altitude) { /* Return true if geofence is disabled */ - if (_param_geofence_on.get() != 1) + if (_param_geofence_on.get() != 1) { return true; + } if (valid()) { @@ -159,20 +167,22 @@ bool Geofence::inside_polygon(double lat, double lon, float altitude) if (dm_read(DM_KEY_FENCE_POINTS, i, &temp_vertex_i, sizeof(struct fence_vertex_s)) != sizeof(struct fence_vertex_s)) { break; } + if (dm_read(DM_KEY_FENCE_POINTS, j, &temp_vertex_j, sizeof(struct fence_vertex_s)) != sizeof(struct fence_vertex_s)) { break; } // skip vertex 0 (return point) if (((double)temp_vertex_i.lon >= lon) != ((double)temp_vertex_j.lon >= lon) && - (lat <= (double)(temp_vertex_j.lat - temp_vertex_i.lat) * (lon - (double)temp_vertex_i.lon) / - (double)(temp_vertex_j.lon - temp_vertex_i.lon) + (double)temp_vertex_i.lat)) { - c = !c; + (lat <= (double)(temp_vertex_j.lat - temp_vertex_i.lat) * (lon - (double)temp_vertex_i.lon) / + (double)(temp_vertex_j.lon - temp_vertex_i.lon) + (double)temp_vertex_i.lat)) { + c = !c; } } return c; + } else { /* Empty fence --> accept all points */ return true; @@ -188,8 +198,9 @@ bool Geofence::valid() { // NULL fence is valid - if (isEmpty()) + if (isEmpty()) { return true; + } // Otherwise if ((_verticesCount < 4) || (_verticesCount > GEOFENCE_MAX_VERTICES)) { @@ -214,26 +225,33 @@ Geofence::addPoint(int argc, char *argv[]) return; } - if (argc < 3) + if (argc < 3) { errx(1, "Specify: -clear | sequence latitude longitude [-publish]"); + } ix = atoi(argv[0]); - if (ix >= DM_KEY_FENCE_POINTS_MAX) + + if (ix >= DM_KEY_FENCE_POINTS_MAX) { errx(1, "Sequence must be less than %d", DM_KEY_FENCE_POINTS_MAX); + } lat = strtod(argv[1], &end); lon = strtod(argv[2], &end); last = 0; - if ((argc > 3) && (strcmp(argv[3], "-publish") == 0)) + + if ((argc > 3) && (strcmp(argv[3], "-publish") == 0)) { last = 1; + } vertex.lat = (float)lat; vertex.lon = (float)lon; if (dm_write(DM_KEY_FENCE_POINTS, ix, DM_PERSIST_POWER_ON_RESET, &vertex, sizeof(vertex)) == sizeof(vertex)) { - if (last) + if (last) { publishFence((unsigned)ix + 1); + } + return; } @@ -243,10 +261,12 @@ Geofence::addPoint(int argc, char *argv[]) void Geofence::publishFence(unsigned vertices) { - if (_fence_pub == -1) + if (_fence_pub == -1) { _fence_pub = orb_advertise(ORB_ID(fence), &vertices); - else + + } else { orb_publish(ORB_ID(fence), _fence_pub, &vertices); + } } int @@ -264,6 +284,7 @@ Geofence::loadFromFile(const char *filename) /* open the mixer definition file */ fp = fopen(GEOFENCE_FILENAME, "r"); + if (fp == NULL) { return ERROR; } @@ -277,7 +298,8 @@ Geofence::loadFromFile(const char *filename) /* Trim leading whitespace */ size_t textStart = 0; - while((textStart < sizeof(line)/sizeof(char)) && isspace(line[textStart])) textStart++; + + while ((textStart < sizeof(line) / sizeof(char)) && isspace(line[textStart])) { textStart++; } /* if the line starts with #, skip */ if (line[textStart] == commentChar) { @@ -305,8 +327,8 @@ Geofence::loadFromFile(const char *filename) // 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); - vertex.lat = lat_d + lat_m/60.0f + lat_s/3600.0f; - vertex.lon = lon_d + lon_m/60.0f + lon_s/3600.0f; + vertex.lat = lat_d + lat_m / 60.0f + lat_s / 3600.0f; + vertex.lon = lon_d + lon_m / 60.0f + lon_s / 3600.0f; } else { /* Handle decimal degree format */ @@ -320,9 +342,10 @@ Geofence::loadFromFile(const char *filename) goto error; } - warnx("Geofence: point: %d, lat %.5f: lon: %.5f", pointCounter, (double)vertex.lat, (double)vertex.lon); + warnx("Geofence: point: %d, lat %.5f: lon: %.5f", pointCounter, (double)vertex.lat, (double)vertex.lon); pointCounter++; + } else { /* Parse the line as the vertical limits */ if (sscanf(line, "%f %f", &_altitude_min, &_altitude_max) != 2) { @@ -340,6 +363,7 @@ Geofence::loadFromFile(const char *filename) warnx("Geofence: imported successfully"); mavlink_log_info(_mavlinkFd, "Geofence imported"); rc = OK; + } else { warnx("Geofence: import error"); mavlink_log_critical(_mavlinkFd, "#audio: Geofence import error"); From 9f32a85409034b2601e42959706a6e0c1a144ce0 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 20 Jan 2015 20:46:56 -0500 Subject: [PATCH 598/652] Fixed coverity CID #12530 --- src/drivers/hmc5883/hmc5883.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 95fbed0ba3..a06be72d9a 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -1351,13 +1351,15 @@ start_bus(struct hmc5883_bus_option &bus, enum Rotation rotation) } int fd = open(bus.devpath, O_RDONLY); - if (fd < 0) + if (fd < 0) { return false; + } if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { close(fd); errx(1,"Failed to setup poll rate"); } + close(fd); return true; } From e10d6bf603e8728061465271957486b727387d1f Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 20 Jan 2015 20:56:18 -0500 Subject: [PATCH 599/652] Fixed coverity CID #12521 --- src/modules/mavlink/mavlink_receiver.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index dfbf00b664..e5a21651d2 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -435,7 +435,7 @@ MavlinkReceiver::handle_message_hil_optical_flow(mavlink_message_t *msg) /* Use distance value for range finder report */ struct range_finder_report r; - memset(&r, 0, sizeof(f)); + memset(&r, 0, sizeof(r)); r.timestamp = hrt_absolute_time(); r.error_count = 0; From 193a210b517c958ebce3aaae8cc9b5709ff9b52b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 23 Jan 2015 14:32:29 +0100 Subject: [PATCH 600/652] Multi sonar support by jverbeke --- src/drivers/drv_range_finder.h | 3 + src/drivers/mb12xx/mb12xx.cpp | 189 ++++++++++++++++++++++++++------- 2 files changed, 152 insertions(+), 40 deletions(-) diff --git a/src/drivers/drv_range_finder.h b/src/drivers/drv_range_finder.h index 0f8362f588..12d51aeaad 100644 --- a/src/drivers/drv_range_finder.h +++ b/src/drivers/drv_range_finder.h @@ -45,6 +45,7 @@ #include "drv_orb_dev.h" #define RANGE_FINDER_DEVICE_PATH "/dev/range_finder" +#define MB12XX_MAX_RANGEFINDERS 12 //Maximum number of RangeFinders that can be connected enum RANGE_FINDER_TYPE { RANGE_FINDER_TYPE_LASER = 0, @@ -67,6 +68,8 @@ struct range_finder_report { float minimum_distance; /**< minimum distance the sensor can measure */ float maximum_distance; /**< maximum distance the sensor can measure */ uint8_t valid; /**< 1 == within sensor range, 0 = outside sensor range */ + float distance_vector[MB12XX_MAX_RANGEFINDERS]; /** in meters */ + uint8_t just_updated; /** number of the most recent measurement sensor */ }; /** diff --git a/src/drivers/mb12xx/mb12xx.cpp b/src/drivers/mb12xx/mb12xx.cpp index 9cf5686583..90f1791bd5 100644 --- a/src/drivers/mb12xx/mb12xx.cpp +++ b/src/drivers/mb12xx/mb12xx.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -54,6 +54,7 @@ #include #include #include +#include #include #include @@ -72,7 +73,7 @@ #include /* Configuration Constants */ -#define MB12XX_BUS PX4_I2C_BUS_EXPANSION +#define MB12XX_BUS PX4_I2C_BUS_EXPANSION #define MB12XX_BASEADDR 0x70 /* 7-bit address. 8-bit address is 0xE0 */ #define MB12XX_DEVICE_PATH "/dev/mb12xx" @@ -83,10 +84,12 @@ #define MB12XX_SET_ADDRESS_2 0xA5 /* Change address 2 Register */ /* Device limits */ -#define MB12XX_MIN_DISTANCE (0.20f) -#define MB12XX_MAX_DISTANCE (7.65f) +#define MB12XX_MIN_DISTANCE (0.20f) +#define MB12XX_MAX_DISTANCE (7.65f) + +#define MB12XX_CONVERSION_INTERVAL 100000 /* 60ms for one sonar */ +#define TICKS_BETWEEN_SUCCESIVE_FIRES 100000 /* 30ms between each sonar measurement (watch out for interference!) */ -#define MB12XX_CONVERSION_INTERVAL 60000 /* 60ms */ /* oddly, ERROR is not defined for c++ */ #ifdef ERROR @@ -133,6 +136,14 @@ private: perf_counter_t _comms_errors; perf_counter_t _buffer_overflows; + uint8_t _cycle_counter; /* counter in cycle to change i2c adresses */ + int _cycling_rate; /* */ + uint8_t _index_counter; /* temporary sonar i2c address */ + std::vector addr_ind; /* temp sonar i2c address vector */ + std::vector + _latest_sonar_measurements; /* vector to store latest sonar measurements in before writing to report */ + + /** * Test whether the device supported by the driver is present at a * specific address. @@ -140,7 +151,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. @@ -170,15 +181,15 @@ private: * and start a new one. */ void cycle(); - int measure(); - int collect(); + 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,12 +211,16 @@ MB12XX::MB12XX(int bus, int address) : _range_finder_topic(-1), _sample_perf(perf_alloc(PC_ELAPSED, "mb12xx_read")), _comms_errors(perf_alloc(PC_COUNT, "mb12xx_comms_errors")), - _buffer_overflows(perf_alloc(PC_COUNT, "mb12xx_buffer_overflows")) + _buffer_overflows(perf_alloc(PC_COUNT, "mb12xx_buffer_overflows")), + _cycle_counter(0), /* initialising counter for cycling function to zero */ + _cycling_rate(0), /* initialising cycling rate (which can differ depending on one sonar or multiple) */ + _index_counter(0) /* initialising temp sonar i2c address to zero */ + { - // enable debug() calls + /* enable debug() calls */ _debug_enabled = false; - // work_cancel in the dtor will explode if we don't do this... + /* work_cancel in the dtor will explode if we don't do this... */ memset(&_work, 0, sizeof(_work)); } @@ -223,7 +238,7 @@ MB12XX::~MB12XX() unregister_class_devname(RANGE_FINDER_DEVICE_PATH, _class_instance); } - // free perf counters + /* free perf counters */ perf_free(_sample_perf); perf_free(_comms_errors); perf_free(_buffer_overflows); @@ -242,6 +257,9 @@ MB12XX::init() /* allocate basic report buffers */ _reports = new RingBuffer(2, sizeof(range_finder_report)); + _index_counter = MB12XX_BASEADDR; /* set temp sonar i2c address to base adress */ + set_address(_index_counter); /* set I2c port to temp sonar i2c adress */ + if (_reports == nullptr) { goto out; } @@ -250,16 +268,53 @@ MB12XX::init() 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); + struct range_finder_report 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?"); + log("failed advert - uORB started?"); } } + /* + * check for connected rangefinders on each i2c port: + * We start from i2c base address (0x70 = 112) and count downwards. + * So second iteration it uses i2c address 111, third iteration 110 and so on + */ + for (int counter = 0; counter <= MB12XX_MAX_RANGEFINDERS; counter++) { + /* set temp sonar i2c address to base adress - counter */ + _index_counter = MB12XX_BASEADDR - counter; + /* set I2c port to temp sonar i2c adress */ + set_address(_index_counter); + int ret2 = measure(); + + if (ret2 == 0) { /* sonar is present -> store address_index in array */ + addr_ind.push_back(_index_counter); + debug("sonar added"); + _latest_sonar_measurements.push_back(200); + } + } + + _index_counter = MB12XX_BASEADDR; + /* set i2c port back to base adress for rest of driver */ + set_address(_index_counter); + + /* if only one sonar detected, no special timing is required between firing, so use default */ + if (addr_ind.size() == 1) { + _cycling_rate = MB12XX_CONVERSION_INTERVAL; + + } else { + _cycling_rate = TICKS_BETWEEN_SUCCESIVE_FIRES; + } + + /* show the connected sonars in terminal */ + for (int i = 0; i < addr_ind.size(); i++) { + log("sonar %d with address %d added", (i + 1), addr_ind[i]); + } + + debug("Number of sonars connected: %d", addr_ind.size()); + ret = OK; /* sensor is ok, but we don't really know if it is within range */ _sensor_ok = true; @@ -325,11 +380,12 @@ MB12XX::ioctl(struct file *filp, int cmd, unsigned long arg) bool want_start = (_measure_ticks == 0); /* set interval for next measurement to minimum legal value */ - _measure_ticks = USEC2TICK(MB12XX_CONVERSION_INTERVAL); + _measure_ticks = USEC2TICK(_cycling_rate); /* if we need to start the poll state machine, do it */ if (want_start) { start(); + } return OK; @@ -344,7 +400,7 @@ MB12XX::ioctl(struct file *filp, int cmd, unsigned long arg) unsigned ticks = USEC2TICK(1000000 / arg); /* check against maximum rate */ - if (ticks < USEC2TICK(MB12XX_CONVERSION_INTERVAL)) { + if (ticks < USEC2TICK(_cycling_rate)) { return -EINVAL; } @@ -414,6 +470,7 @@ MB12XX::ioctl(struct file *filp, int cmd, unsigned long arg) ssize_t MB12XX::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; @@ -453,7 +510,7 @@ MB12XX::read(struct file *filp, char *buffer, size_t buflen) } /* wait for it to complete */ - usleep(MB12XX_CONVERSION_INTERVAL); + usleep(_cycling_rate * 2); /* run the collection phase */ if (OK != collect()) { @@ -474,17 +531,19 @@ MB12XX::read(struct file *filp, char *buffer, size_t buflen) int MB12XX::measure() { + int ret; /* * Send the command to begin a measurement. */ + uint8_t cmd = MB12XX_TAKE_RANGE_REG; ret = transfer(&cmd, 1, nullptr, 0); if (OK != ret) { perf_count(_comms_errors); - log("i2c::transfer returned %d", ret); + debug("i2c::transfer returned %d", ret); return ret; } @@ -506,7 +565,7 @@ MB12XX::collect() ret = transfer(nullptr, 0, &val[0], 2); if (ret < 0) { - log("error reading from sensor: %d", ret); + debug("error reading from sensor: %d", ret); perf_count(_comms_errors); perf_end(_sample_perf); return ret; @@ -519,10 +578,35 @@ MB12XX::collect() /* 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; + + /* assign the first measurement to the plain distance field */ + report.distance = _latest_sonar_measurements[0]; 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; + report.valid = ((_latest_sonar_measurements[0] > get_minimum_distance()) + && (_latest_sonar_measurements[0] < get_maximum_distance())) ? 1 : 0; + + /* intermediate vector _latest_sonar_measurements is used to store the measurements as every cycle the other sonar values + * of the report are thrown away and/or filled in with garbage. We don't want this. We want the report to give the latest + * value for each connected sonar + */ + _latest_sonar_measurements[_cycle_counter] = si_units; + + for (unsigned i = 0; i < (_latest_sonar_measurements.size()); i++) { + if (i < addr_ind.size()) { + /* set data of connected sensor */ + report.distance_vector[i] = _latest_sonar_measurements[i]; + + } else { + /* set unconnected slots to zero */ + report.distance_vector[i] = 0; + } + } + + /* a just_updated variable is added to indicate to higher level software which sonar has most recently been + * collected as this could be of use for Kalman filters + */ + report.just_updated = _index_counter; /* publish it, if we are the primary */ if (_range_finder_topic >= 0) { @@ -545,12 +629,13 @@ MB12XX::collect() void MB12XX::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)&MB12XX::cycle_trampoline, this, 1); + work_queue(HPWORK, &_work, (worker_t)&MB12XX::cycle_trampoline, this, 5); /* notify about state change */ struct subsystem_info_s info = { @@ -564,8 +649,10 @@ MB12XX::start() if (pub > 0) { orb_publish(ORB_ID(subsystem_info), pub, &info); + } else { pub = orb_advertise(ORB_ID(subsystem_info), &info); + } } @@ -578,21 +665,25 @@ MB12XX::stop() void MB12XX::cycle_trampoline(void *arg) { + MB12XX *dev = (MB12XX *)arg; dev->cycle(); + } void MB12XX::cycle() { - /* collection phase? */ if (_collect_phase) { + /*sonar from previous iteration collect is now read out */ + _index_counter = addr_ind[_cycle_counter]; + set_address(_index_counter); /* perform collection */ if (OK != collect()) { - log("collection error"); - /* restart the measurement state machine */ + debug("collection error"); + /* if error restart the measurement state machine */ start(); return; } @@ -600,25 +691,37 @@ MB12XX::cycle() /* next phase is measurement */ _collect_phase = false; - /* - * Is there a collect->measure gap? - */ - if (_measure_ticks > USEC2TICK(MB12XX_CONVERSION_INTERVAL)) { + /* change i2c adress to next sonar */ + _cycle_counter = _cycle_counter + 1; + + if (_cycle_counter >= addr_ind.size()) { + _cycle_counter = 0; + } + + /* Is there a collect->measure gap? Yes, and the timing is set equal to the cycling_rate + Otherwise the next sonar would fire without the first one having received its reflected sonar pulse */ + + if (_measure_ticks > USEC2TICK(_cycling_rate)) { /* schedule a fresh cycle call when we are ready to measure again */ work_queue(HPWORK, &_work, (worker_t)&MB12XX::cycle_trampoline, this, - _measure_ticks - USEC2TICK(MB12XX_CONVERSION_INTERVAL)); - + _measure_ticks - USEC2TICK(_cycling_rate)); return; } } - /* measurement phase */ + /* Measurement (firing) phase */ + + /* ensure sonar i2c adress is still correct */ + _index_counter = addr_ind[_cycle_counter]; + set_address(_index_counter); + + /* Perform measurement */ if (OK != measure()) { - log("measure error"); + debug("ERR ADDR: %d", _index_counter); } /* next phase is collection */ @@ -629,7 +732,8 @@ MB12XX::cycle() &_work, (worker_t)&MB12XX::cycle_trampoline, this, - USEC2TICK(MB12XX_CONVERSION_INTERVAL)); + USEC2TICK(_cycling_rate)); + } void @@ -750,7 +854,7 @@ test() } warnx("single read"); - warnx("measurement: %0.2f m", (double)report.distance); + warnx("measurement: %0.2f of sonar %d", (double)report.distance_vector[report.just_updated], report.just_updated); warnx("time: %lld", report.timestamp); /* start the sensor polling at 2Hz */ @@ -779,7 +883,12 @@ test() } warnx("periodic read %u", i); - warnx("measurement: %0.3f", (double)report.distance); + + /* Print the sonar rangefinder report sonar distance vector */ + for (uint8_t count = 0; count < MB12XX_MAX_RANGEFINDERS; count++) { + warnx("measurement: %0.3f of sonar %u", (double)report.distance_vector[count], count + 1); + } + warnx("time: %lld", report.timestamp); } @@ -830,7 +939,7 @@ info() exit(0); } -} // namespace +} /* namespace */ int mb12xx_main(int argc, char *argv[]) From a23c39eec414ffb9d6808e3402d21f322e14930f Mon Sep 17 00:00:00 2001 From: Jon Verbeke Date: Tue, 20 Jan 2015 16:38:59 +0100 Subject: [PATCH 601/652] Sonar driver by jverbeke --- src/drivers/mb12xx/mb12xx.cpp | 99 +++++++++++++++++------------------ 1 file changed, 49 insertions(+), 50 deletions(-) diff --git a/src/drivers/mb12xx/mb12xx.cpp b/src/drivers/mb12xx/mb12xx.cpp index 90f1791bd5..0ef81e4319 100644 --- a/src/drivers/mb12xx/mb12xx.cpp +++ b/src/drivers/mb12xx/mb12xx.cpp @@ -34,6 +34,7 @@ /** * @file mb12xx.cpp * @author Greg Hulands + * @author Jon Verbeke * * Driver for the Maxbotix sonar range finders connected via I2C. */ @@ -136,12 +137,11 @@ private: perf_counter_t _comms_errors; perf_counter_t _buffer_overflows; - uint8_t _cycle_counter; /* counter in cycle to change i2c adresses */ - int _cycling_rate; /* */ - uint8_t _index_counter; /* temporary sonar i2c address */ + uint8_t _cycle_counter; /* counter in cycle to change i2c adresses */ + int _cycling_rate; /* */ + uint8_t _index_counter; /* temporary sonar i2c address */ std::vector addr_ind; /* temp sonar i2c address vector */ - std::vector - _latest_sonar_measurements; /* vector to store latest sonar measurements in before writing to report */ + std::vector _latest_sonar_measurements; /* vector to store latest sonar measurements in before writing to report */ /** @@ -149,9 +149,9 @@ private: * specific address. * * @param address The I2C bus address to probe. - * @return True if the device is present. + * @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. @@ -181,8 +181,8 @@ private: * and start a new one. */ void cycle(); - int measure(); - int collect(); + int measure(); + int collect(); /** * Static trampoline from the workq context; because we don't have a * generic workq wrapper yet. @@ -273,20 +273,16 @@ MB12XX::init() _range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &rf_report); if (_range_finder_topic < 0) { - log("failed advert - uORB started?"); + log("failed to create sensor_range_finder object. Did you start uOrb?"); } } - /* - * check for connected rangefinders on each i2c port: - * We start from i2c base address (0x70 = 112) and count downwards. - * So second iteration it uses i2c address 111, third iteration 110 and so on - */ - for (int counter = 0; counter <= MB12XX_MAX_RANGEFINDERS; counter++) { - /* set temp sonar i2c address to base adress - counter */ - _index_counter = MB12XX_BASEADDR - counter; - /* set I2c port to temp sonar i2c adress */ - set_address(_index_counter); + /* check for connected rangefinders on each i2c port: + We start from i2c base address (0x70 = 112) and count downwards + So second iteration it uses i2c address 111, third iteration 110 and so on*/ + for (unsigned counter = 0; counter <= MB12XX_MAX_RANGEFINDERS; counter++) { + _index_counter = MB12XX_BASEADDR - counter; /* set temp sonar i2c address to base adress - counter */ + set_address(_index_counter); /* set I2c port to temp sonar i2c adress */ int ret2 = measure(); if (ret2 == 0) { /* sonar is present -> store address_index in array */ @@ -297,8 +293,7 @@ MB12XX::init() } _index_counter = MB12XX_BASEADDR; - /* set i2c port back to base adress for rest of driver */ - set_address(_index_counter); + set_address(_index_counter); /* set i2c port back to base adress for rest of driver */ /* if only one sonar detected, no special timing is required between firing, so use default */ if (addr_ind.size() == 1) { @@ -309,7 +304,7 @@ MB12XX::init() } /* show the connected sonars in terminal */ - for (int i = 0; i < addr_ind.size(); i++) { + for (unsigned i = 0; i < addr_ind.size(); i++) { log("sonar %d with address %d added", (i + 1), addr_ind[i]); } @@ -397,7 +392,7 @@ MB12XX::ioctl(struct file *filp, int cmd, unsigned long arg) bool want_start = (_measure_ticks == 0); /* convert hz to tick interval via microseconds */ - unsigned ticks = USEC2TICK(1000000 / arg); + int ticks = USEC2TICK(1000000 / arg); /* check against maximum rate */ if (ticks < USEC2TICK(_cycling_rate)) { @@ -579,34 +574,39 @@ MB12XX::collect() report.timestamp = hrt_absolute_time(); report.error_count = perf_event_count(_comms_errors); - /* assign the first measurement to the plain distance field */ - report.distance = _latest_sonar_measurements[0]; - report.minimum_distance = get_minimum_distance(); - report.maximum_distance = get_maximum_distance(); - report.valid = ((_latest_sonar_measurements[0] > get_minimum_distance()) - && (_latest_sonar_measurements[0] < get_maximum_distance())) ? 1 : 0; + /* if only one sonar, write it to the original distance parameter so that it's still used as altitude sonar */ + if (addr_ind.size() == 1) { + report.distance = si_units; - /* intermediate vector _latest_sonar_measurements is used to store the measurements as every cycle the other sonar values - * of the report are thrown away and/or filled in with garbage. We don't want this. We want the report to give the latest - * value for each connected sonar - */ - _latest_sonar_measurements[_cycle_counter] = si_units; - - for (unsigned i = 0; i < (_latest_sonar_measurements.size()); i++) { - if (i < addr_ind.size()) { - /* set data of connected sensor */ - report.distance_vector[i] = _latest_sonar_measurements[i]; - - } else { - /* set unconnected slots to zero */ + for (unsigned i = 0; i < (MB12XX_MAX_RANGEFINDERS); i++) { report.distance_vector[i] = 0; } + + report.just_updated = 0; + + } else { + /* for multiple sonars connected */ + + /* don't use the orginial single sonar variable */ + report.distance = 0; + + /* intermediate vector _latest_sonar_measurements is used to store the measurements as every cycle the other sonar values of the report are thrown away and/or filled in with garbage. We don't want this. We want the report to give the latest value for each connected sonar */ + _latest_sonar_measurements[_cycle_counter] = si_units; + + for (unsigned i = 0; i < (_latest_sonar_measurements.size()); i++) { + report.distance_vector[i] = _latest_sonar_measurements[i]; + } + + /* a just_updated variable is added to indicate to autopilot (ardupilot or whatever) which sonar has most recently been collected as this could be of use for Kalman filters */ + report.just_updated = _index_counter; + + /* Make sure all elements of the distance vector for which no sonar is connected are zero to prevent strange numbers */ + for (unsigned i = 0; i < (MB12XX_MAX_RANGEFINDERS - addr_ind.size()); i++) { + report.distance_vector[addr_ind.size() + i] = 0; + } } - /* a just_updated variable is added to indicate to higher level software which sonar has most recently been - * collected as this could be of use for Kalman filters - */ - report.just_updated = _index_counter; + report.valid = si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0; /* publish it, if we are the primary */ if (_range_finder_topic >= 0) { @@ -676,8 +676,7 @@ void MB12XX::cycle() { if (_collect_phase) { - /*sonar from previous iteration collect is now read out */ - _index_counter = addr_ind[_cycle_counter]; + _index_counter = addr_ind[_cycle_counter]; /*sonar from previous iteration collect is now read out */ set_address(_index_counter); /* perform collection */ @@ -721,7 +720,7 @@ MB12XX::cycle() /* Perform measurement */ if (OK != measure()) { - debug("ERR ADDR: %d", _index_counter); + debug("measure error sonar adress %d", _index_counter); } /* next phase is collection */ From b14e849fc4bcf81bbffd47064e2850a929652f6d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 23 Jan 2015 16:33:24 +0100 Subject: [PATCH 602/652] Sonar: Set min/max distance properly, add usleep for detection --- src/drivers/mb12xx/mb12xx.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/drivers/mb12xx/mb12xx.cpp b/src/drivers/mb12xx/mb12xx.cpp index 0ef81e4319..a4fb7bcc20 100644 --- a/src/drivers/mb12xx/mb12xx.cpp +++ b/src/drivers/mb12xx/mb12xx.cpp @@ -277,6 +277,9 @@ MB12XX::init() } } + // XXX we should find out why we need to wait 200 ms here + usleep(200000); + /* check for connected rangefinders on each i2c port: We start from i2c base address (0x70 = 112) and count downwards So second iteration it uses i2c address 111, third iteration 110 and so on*/ @@ -606,6 +609,8 @@ MB12XX::collect() } } + 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 f5a1680fd6eb17c4fd1b7663cb16ffe126289cc0 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Sat, 24 Jan 2015 14:18:28 +0100 Subject: [PATCH 603/652] Add landing detector for V1 boards as well. --- makefiles/config_px4fmu-v1_default.mk | 1 + 1 file changed, 1 insertion(+) diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index 18be47065d..852edb788c 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -62,6 +62,7 @@ MODULES += modules/commander MODULES += modules/navigator MODULES += modules/mavlink MODULES += modules/gpio_led +MODULES += modules/land_detector # # Estimation modules (EKF / other filters) From cbf4a5af1611ee0969ea0b8d265441308e2dbc91 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Fri, 23 Jan 2015 22:25:48 -0500 Subject: [PATCH 604/652] Simplified i2c handling for flow. --- src/drivers/px4flow/i2c_frame.h | 82 ++++++++++++++++++++++++++++++ src/drivers/px4flow/px4flow.cpp | 88 +++++---------------------------- 2 files changed, 95 insertions(+), 75 deletions(-) create mode 100644 src/drivers/px4flow/i2c_frame.h diff --git a/src/drivers/px4flow/i2c_frame.h b/src/drivers/px4flow/i2c_frame.h new file mode 100644 index 0000000000..b391b1f6a8 --- /dev/null +++ b/src/drivers/px4flow/i2c_frame.h @@ -0,0 +1,82 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file i2c_frame.h + * Definition of i2c frames. + * @author Thomas Boehm + * @author James Goppert + */ + +#ifndef I2C_FRAME_H_ +#define I2C_FRAME_H_ +#include + + +typedef 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; +} i2c_frame; + +#define I2C_FRAME_SIZE (sizeof(i2c_frame)) + + +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 sonar_timestamp; + uint16_t ground_distance; + int16_t gyro_temperature; + uint8_t qual; +} i2c_integral_frame; + +#define I2C_INTEGRAL_FRAME_SIZE (sizeof(i2c_integral_frame)) + +#endif /* I2C_FRAME_H_ */ diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp index 9c9c1b0f81..bb0cdbbb6e 100644 --- a/src/drivers/px4flow/px4flow.cpp +++ b/src/drivers/px4flow/px4flow.cpp @@ -93,38 +93,11 @@ 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; -}; +#include "i2c_frame.h" + struct i2c_frame f; - -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; - int16_t gyro_temperature; - uint8_t qual; -} __attribute__((packed)); struct i2c_integral_frame f_integral; - class PX4FLOW: public device::I2C { public: @@ -150,8 +123,7 @@ private: RingBuffer *_reports; bool _sensor_ok; int _measure_ticks; - bool _collect_phase; - + bool _collect_phase; orb_advert_t _px4flow_topic; perf_counter_t _sample_perf; @@ -261,10 +233,10 @@ out: int PX4FLOW::probe() { - uint8_t val[22]; + uint8_t val[I2C_FRAME_SIZE]; // 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 + // 0x42) we check if a I2C_FRAME_SIZE 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) { @@ -469,16 +441,16 @@ PX4FLOW::collect() int ret = -EIO; /* read from the sensor */ - uint8_t val[47] = { 0 }; + uint8_t val[I2C_FRAME_SIZE + I2C_INTEGRAL_FRAME_SIZE] = { 0 }; perf_begin(_sample_perf); if (PX4FLOW_REG == 0x00) { - ret = transfer(nullptr, 0, &val[0], 47); // read 47 bytes (22+25 : frame1 + frame2) + ret = transfer(nullptr, 0, &val[0], I2C_FRAME_SIZE + I2C_INTEGRAL_FRAME_SIZE); } if (PX4FLOW_REG == 0x16) { - ret = transfer(nullptr, 0, &val[0], 25); // read 25 bytes (only frame2) + ret = transfer(nullptr, 0, &val[0], I2C_INTEGRAL_FRAME_SIZE); } if (ret < 0) { @@ -489,46 +461,12 @@ PX4FLOW::collect() } 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.gyro_temperature = val[45] << 8 | val[44]; - f_integral.qual = val[46]; + memcpy(&f, val, I2C_FRAME_SIZE); + memcpy(&f_integral, &(val[I2C_FRAME_SIZE]), I2C_INTEGRAL_FRAME_SIZE); } 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.gyro_temperature = val[23] << 8 | val[22]; - f_integral.qual = val[24]; + memcpy(&f_integral, val, I2C_INTEGRAL_FRAME_SIZE); } @@ -544,7 +482,7 @@ PX4FLOW::collect() 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.time_since_last_sonar_update = f_integral.sonar_timestamp;//microseconds report.gyro_temperature = f_integral.gyro_temperature;//Temperature * 100 in centi-degrees Celsius report.sensor_id = 0; @@ -828,7 +766,7 @@ test() 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); + f_integral.sonar_timestamp); warnx("quality integration average : %i", f_integral.qual); warnx("quality : %i", f.qual); From 2c0029235a08864039cf1239f86a0af80039548c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 25 Jan 2015 16:17:03 +0100 Subject: [PATCH 605/652] Documentation: WIP on better block diagrams --- Documentation/px4_block_diagram.odg | Bin 42200 -> 91712 bytes 1 file changed, 0 insertions(+), 0 deletions(-) diff --git a/Documentation/px4_block_diagram.odg b/Documentation/px4_block_diagram.odg index 95b6f600b87aedcb06535d5b330afbdb76d91c41..e66e1b1cdd704ebe713a2127004488aed7fb2218 100644 GIT binary patch literal 91712 zcmZ^|MNk}!0<1eYAtAWC1$TFs;5N7u+}+(B{^0I1z~Jr#g1fszaQA_^bzaq5+{ImV zSFd{2o3C0`0S+D;06+!+SS|Tw3gB3V7y$sl|AY(x032)_EZlvZEli!A?QP6W-EEv4 zSv?(pu{fE!*|@PdIa@gXax(XDuyAx|v2rzawze^MQ~kdRljeN9R>c8;{|O%e0BBl! zIG8z_+St3Xy8r)?#o5s+MpaoB6@?H5005x=l#@~i0AK(B0H6gK761TFHRbCJl=69U#H*4q`)ky}3qnLd+!=xdJ`!CT(lvH@)>DF&v z@TsYQF7Ameu+`i8T($8hyU<-SJpF$Falp5JSSduFsI09qpK`a(Dv%Ly>neX+D!55u z8vXfGY}#|D*D%28`MLV7x30I4?XOrgx_Ct6HXb_VkS`^*ShN_`|I_H;P2$CoUyT{a zYMzhaNgz>T=x=?aZ=b~C6v0sD`#!$Z&xGK=b@f-j)l`vpXSje-Ea~(?{VO5WoFQc9 z5YbmwIBWoXPSiP?-i!f>@n@MPq;Wr}P$XIwMrI!-?GL4nXl3Ew7gsvy!LmMnU;ue4 zjnXRVz#httT_|g!!CT)02bX50scHkoEkZU-c(^Jk6Uqko)9HL4Ru0RcY{)`T=@urJ z^#oOa#S8A-Jjdsg8tejP{&46Fbmbq62`HzL%% zCm=-tMCvm`_CmxndHU5clySmeT<4h9$Sp#LF|CurA}|HJzog4gYV4|h^Zu5~d;@9b ztMuL-;`o;klE&~aIyem_nxIhA9DYzZ&);M@FBk<2_G1&9j|~|Gn9fEH?b388Gzl=f&&ukdChr z;af|{e{c+>MDn3Wn41g3|6%a;!uswPY9PgN7d4;{S~-?in`of0;hCgWp*pLji&H)w=EG=I6T6Z!M4~E^KYkm=Bv^mdz zAyvo+T)TF$mmy?HY?25F9gZ)Z-%JM=4+_SCMvhK5WBGQvo1Y3<&SisKIGF^F@nOng zErt6eS_*G(rU1|;<`STRfux#mA zNcDg2z7Y~m0INbHx(kf3F^xSlLtl0u+is{BaU36(9}NSG4`NC-aZf7vS)kVCl#bLl zkMNO1*yo`p7{M?iB_(=O!oukkz5Qc-8J%3L_s&Llxj)+?oGpCaD_I+=cpba2#iI{P zXUekr67`03Y`pb9xCeqloj)>T(Xqa-8E=t9d6U4$0~F;B{l`A9%ufs^!qhbh3eIA# zZ!*;7%j&-h%ZQku!j4iI{0%6eS$Pxm-G@TZzK^(1wh*>=Ho1+_4lvznB5pBo!9%Bj;+8wLj|BQ4S2D>583Y>zq<7cjcjT|0cS zfP051q7^Jv!=Y)6dJe+uNsX%BL6R6uuyR%kA7nBe0`LwwSA?;{z?X)$hpo*DrAa;A z3!`0MR}#?f|8j9eJ1v{6hZ- z3pUd4s?(#%rizr4`B3Qtm!r%?$2G$0MIFjmvY^1DlM@EA?!GP+$@Gc(<0L@Gl~nkF zWzW@*Aylz?WYwfq)5+E^C5zf!9ZHZRS;p!H5!JtJs>xUL!bztL26_!0?J7YV`~Nxd z!(;BP?#(+%Jw{hy4)#B$FI#6X@4lIvLoodRh`2NLu!KqjSiXa0yEIa~pv%*zYsJOn z#xk=w>Vs%Hq&WJT@eb?sMkh{(>YWHHRNh~T-aS1%soj+U;j@pU+0Miidx$-s$fg*U zOswVXa%`cDU^p16V93PqD_H+Mbpt88_&Dm<&kF00hU+PN-{9=&sW&5F9(;Uz^D(;g zu!J+wyUq;=&geY*P)-wGLu>?RBBt16xsgWeIdgnhNR)7<55c!3rO0VRtejlRh7Z~& z5-JT4nE;`Pus1?pH1O1@Mt?(3X1dZ9JaFid-$Dwe$Y*`o+yZvQe5EjUHMF0en=i zv!=MT#^J&~S;dIa*p77A8j6~iyF)bij@*R%4Dw2?yh#bQB06Y`__0UjKTnyIoZk(qismiI?%jXk)B7D(2JnR?LUll5!CDc_9cRCj`x5v30J%t; zlnNo~s~7+c04a&{o8+WIf*>XKdP zI^Dq~FDMkHY6M{V3cBCIa|0Nfx!-(s)(qq~;eq|v|3_MdIiw>S%EJA0wgUMUx6C}()0$+meC zM#8o}WT#X%&O3?l>V*h&s;Bj(mjWFyd1pgv8wT3n_H``$j5W))N6n=F%$93osCr&{ ztAg3-(G`DP=(ml`%;;_SrqU`O+F0*wBg3Zv3`9*?ZZ%c1V<3RT^}AK~Q)ocDR!vG} zVxx4@SK^RDT89VPo7*u$?Obf2_1V;-92k~Aairij%#}+q!gvV^UrG5?a*~cH(CqZ# zpH(er-~%1%Ih8dUJp`5RtM&qtadD8Th76Lt&9nzxqCx#yu5Y&fMb;l5=1sw~X}LLa zbVm#c*_j(8%o&rceGl>cz|JusMHoqxI-wpy@tIB0t{Ses@|uQuy44^leJmellY z+z9gn$EQ!kxp4L>r1kCPP5)c}b&~kqkrnN!ZE8BB9c2-c8rd=FN~&Q#IKNBzF z0bIbWzqf6r|0(UbzEx}9?4d8)pxRfTep?Ecp;|bYKY(myAS(f1ovlq2%js4qx|YUa z$jSvUA`_MD8Oc?6p90z2e)3 znk2fN6O>>QhQ@+q(@<+#G8v~?w|mx)>V$ot#w}|6iV3?^Ako1aLYNa#_-G7eVqYLP z1FCP#paufaVYTtZofD4iNzj>KZwiL7{A2@P{>Wq}wF_DIk1LaWyjdBh($2VB_hO_z;ziGFm{R`iM)`Q+NFb!=lC z^Q&)RE@?}V7ZyvzXPt8GOIjD_IzE0KXqZ3x|L zi<>jZO4Uv>b&z1RUid;2T7}t!gx>woYG!#pc|_fp>jtIt?-Q2M1}K9=#bh-P55Q2o zAo_NRvaJ4uO4TIU=IB2+vZ4>WO`-w0rwxLRJuR9B0xq6HW`sIweu#){O;+W2LMuuh z&t)1hYcaRkm)Ye!yu33iCUUhn+MUJ0Gas$lQYk#gdUWe+AL-9sSwADW^cd-?jGSzk zypA)Mw-NXfh$|d+gd6slmj~q>*v+3%IWjb5j2mMuxCvW06k;yhd|rQIpOcr-FgDqL z-zqh!_BQQ?!u2^#k+3SoQ!Fi-*)Vcaa1;R3Qr+?|okKB(Ju>IPYu{j`lVvuUpjc{I zMLf&J9@47FyFoJ!i2f5=p&#zDr?xibql+YbN>IOpU0b=bI!0tp;H2qe>d#f7XIm5I zx&dR&&jfVBgs_a`*S^_mVCzNCB^()K_o~+vQyT8pdb+sp*>9u7_i=JG++j$-_xF9p zjMG|6hJIHQ4+jH|#;xmX8Y|=yZ#;2iCAz(ecnyVvq3IPemEgW_LWOa@Q*^cc+vila z-a*!E;R!VUv%-A`O3K^SezRzP+18D~ceExWpQ}x*IE^33p zctK5=*0g|plL(L>FM0Zt^0apduC3ZcX_)kWP6p<&`L=1(8fU#kK^a2j@b}`djY+01W$ z6T01UVu`Ri!Z19%HhGgy(z%~2!1><=f!q(ONs9Y8g*5f5{efuEQmn##oGz@CJ8@r+ z#M1i}y>lFm>+;ZvX~J5>8W{x5`k&16kH&{=ZZ8<$i1mH>&lk6&n1$tee+x)Ex8J%) zR{2PtE052F@bHTA?)JxPMxYe2R*M}UX$!8^gF9=Vifz)LpDSZi7BeEK%4xMn(j`uQ zpQ~7OW}sUQf4Y73(kh7wDYI+Hm|8P@;dnj7^)AGE5Ojw)U)KcLTODi0ubodic{W1gUxoOr!{iR6; z^sJB5T19UKN~B7u0>@tm_aSqGt9G0CVjzM01Sr$sL#aX>8Ef|){QNinTcIjk{5G!S z)lzgGxS(%R&A;o3u(_L@)iMWls&WH*9X0P4G*PPrJeVmyS`#)(&}}l8cc_C9bg5qw z1Pg=Gknv&2oxc#-BISMYE>z)j_Vj?+XCEJqm~iQO0Wy$oq+;tDzY?x4OvU0JhW;`e z-D&6e@stVT&P;4bL_TyQUNNs`Sv<_{r!kT&v}V`9`cZY>5RdSNY2WY-?J0yT&wPSZ z1I)c8K1#dZs?v@7}d!ZBOM@db`t2|}QV#qV}+!iOd z=YHpM@j#wt0>Z&`)98SVNN8KQx+vMs?Jwc0(xI!USyK~;Z*3GM3P2EXAVN%o2JL7) zJD5q;L~LEp6!Ap(W`JUg@T`UVgZK@@=y5(DU}Go{i-LgwkQF*`iyHhzRVp?$4c4L}1BS z8~$duKu+da6$!RretG4ROWL|if!a;cH2RZ@F)<{=X0S-}T9LivHfB4%PM{&N2HZ^a z>IXJT<9i|i!K+BC=)}nQFfaQP{S%xuCNXdMhKTK|fDMh^&EjZ_vww=RvCS|FA4o6y*s%HOmNd>jd04$(dv@^0c_}u{1T5r zW}2Ea!xBrKopei7^mI+_>PfSD=TZ})(t(c}U|tts6^cE7R-M{X`yx$!Yute9i_y8@ zz&ZHVAjAX%FkZ@*G1wlG9&6tVz=2nuj7uYy$CGZ*N+Q~IYoSu#JC}XYQ0WXuu0`VM`jZ-WH_v>()9Z`50Kynr# zd}z+Bd+3J1TLZQXMGdN3;v~f0MB6FjHAv z4EJX5h+Jg4u6?!_X|H%EO3I(GWH7M(=qFVHQ*cL_?eH^(%A8}!apy3g9EP%#$cq1F z%N2@^4SN1~pEjI?K#pt>?y)NSUQ~ThHPwl^S(XYg>@#c`)GZQ>C-pNGHH5Bvl{k`{Ehv)Jb8g~fS(dBI!brKMKI%3z@DH! zqNHF9F^S15{3?(H-A|gSC`X%(^{S7H7JGe;|Kj`Gw$`^7$_+Bxkkx(ny&Ft&IzH)2wTe45|2{w3#Y@`Vrc;asyX-Gr|2)Ni189wE-%;LcP|^meEzQ-*Pn7 zvB2s%E-iK8N&=4CV%a@i5mEiZ1^D_tj_+v{4z{^Sl7!*EAi7*c$gqM-s(1YA<^{6IKVxV zRbVr5XWd*RfN5igEa20-b?C-$gEA(QgCF=dpnE_XA*icofK^dSSK8lbUN^@V?BNg#}t<5IdF9|y%synTH(-O zd~y&pFXM2CJ9wq3<}FLI1Rcsx=Td? zonfHI^>^?)(bX0hyYhXo{7ek-Fg{dV7T&B#2W@{XDa*?-J1MW;+B)9mLG{ZA?>Y!a zy0!}ZcJV?R;_+foA(@cI{M+MazVq=4tKGl-aCckYH!wlbL`m-^RBQ@fxS_vu>JuDh zHcV>xt+8~~&XM7Ug`sv>VTo0>2mljf{Vk4cY-@9O+}_tqJ_z+yW&^aYVp&q*-3NJ6 zjL`fb1=`~7O~9@+b0fGuT0X1(^6gEpR%?i{)Nr))zo#F3yu0p^6GpoULsng(0FPcF z_$!KV+&Cb9V~NKT;Z!3>Rwk}>Z?=JO)qDxM{7T!zs9*`+cb$^0ycm3zGN?1!iC_eV ztGbx_E6RU4v<&+MzS!B<-vzmax0C@@cpkYEI>$zs^-!bf zVzlvocG7+?b5lKr_i{ZlDeQCb1y?f25oVZG=&D5Pm@(;7(-R6c?V3u*HGh$(rX(L$ zGMf910|G_+9b2`I6p>JEw3fbF*{}UaN*0y9T-itI`(@?eky)asD%Q1|@ZvH=H^XdU z7nJ$K_$qpZgd4)q>4%x~{`xKHZ-r~`?S&Wpl_JUl4@nfxTorzE*~bPFVp~%O|6=*w z!ul`&??GE!V)1T&D3F{^c{N9^5bP_a^m#|obp^b** zJ27K~%Hk}g;HfLY)u5frkH^h=Mh3kQg}kJ|?4L~x=yvu>(blBnLKy)Ud4ke|5>&%4 zcNcrs>2Wr5WaQHi_P9 z?2^vrSH}?=u*L$Ih?R{LYC0r_9hb($w(7*SIeSkO{Q3BtYkU5i0%o0)N+1l^ZGaJ2_-=nRe$1}XGgoMX=W0ztZ{%TCV{-d zu<;KOwF?CeHCqL#i-k8J1wjL|LVI+VT{^1g*oFV?G~CCy$DxYTFZeh>?=iWOOud`z znTC^6(u|gjr2}^)76WlkTjTearsoI9ZdMv)7!u;caIPI(Dm@>>p{pywcDP5!S%clf zwaC*9R2(ywrQc~*4c&Tv5N=-ZwkdTs8&DcaRU8v+=Kfr~z1aJxV*a^w_wV69;Z`$R zs`0Ug`Fs+Goc~a?MpRkpRUzU|RQu=j@bXqmBjP3qD0`)!$p+o`EHywQ)Y zvqBasv1Ai>U@*|*<3))Rb#S8ag#-swT687ZHv~^pSZ6qqn&(9-FI~b{V2+WeU#{oV z@C5b~;Gw-)utK?1(@@(>+*lXh|9p2+?jp4^7hb{TVL}tN|A{E)3~taIe<^8`esyJU zwWcXaHnaY*=!jsP+k`^8iU}0GpJGoUTCU0GnmOaYcTzq@f}sv}4vSS}W9#}AaA~E$ zDBZ?bg)`1FRJ<)(P)afg;=Vqy`f5;6Z_uiKBe1^I9>Ty4Vk&KpbHV*U*j@f;kYiMy%lR9w|A$b}+eCu)jpa3Vm@pC6fVx2&x0pY7xPKStv6 z?HjP?TzdD>*S!DM)s2=-&F+@4T7Gwkfox&L`IlwQbapaet})`wFBOew@^zTlM9ve8 zq>og}va)Oxr`KJ;O$?`x-XM`P!eG79cP9R@8V`DWyc4Sci?_!n;5AdZTte(U|okIn{r!@UJJ)?V~)d|dvqDgs4pm&?W`vZ%iz z-L`tp>TYyXy@hn|i$9KmW?nJa>Z!$rSu&itefvT_p}^m)nQlt})xuD=EcU!2ceI%L zMkB~=n^%U+r`&*-p?%af#7&Ngo zg4u|BpasU$ec3APd)-B8JaFyl_(&3QaW?I!p>IFszm?>(DG{$v4vo~PKOM6IoREwzdCRa@v$684l6xgeF!_1wyXd(YILY#hF$PiqJ1~0 zB)8@)4?>MU*CC%4l${y_|N5u7Iz0dMHn0uT=wH2+=v^}fit!-yr?$LuKxpyRwxsr9(I zF(IKJqaTma9nN-s(aH^e0C+sZop?+{$a^>le}me^CR*@}m_0x=T>KpuMhe4nP><^b z{T$H8gX91&7*U216rjp>p=^RPkZv$oU3q-|QPMpIBaLQh0^%@q?@5R)MDK@Ydl(QT zipBv#R59|xUqo;2oPt%+s9HK?G-P)C(|6uN_36I5V6F!OB1>ny6ZqeG;l(L}`_eR8 z8)at#Tc_-+`K6VV$aisS(qv34d=rks*VJ0mv*aRAij#mfD*+tm0biX6$-@~uML?fX zB!-eTr-nwf@DTu!7wx*;){VBniur&!KsUae%;PR`_T}9)~&7l`+|pKV9#<1^V6& zvNOVy6HMt9Da&0IPQmDC;>^Y(ym^9DSa54V61*uyzZ!y&CK)KAec~+fjetxw+A6Vw zq>W2p%>tqeJVF9s6Y>ahyqqn6KB~3HU^5)3unfws26lot(|h>x`5pe{pa*7|HQ^>Kvl`RH+VV*C&C%N^m2kVF)0LgCke~9x4NwB`P*97s z|MnG`Y*FE2t-qOxcI?hZDC8 zgh~T;o#3hdVaPg~vQrNBYpzu&bFsth*D*XRebjQWe;8Ni3cnhVf}T|cww_j33Rsz!olBmbORzKk z(0VM)>XaB^^fiMl-2TX5(HK>Rx||1tPqyBRay+lLf9YSz%m?$}!pZ_*z>DTAA;ok{ zM3r7XkA@+>h@Ic0kKOvx?k||cf@w~LNvB26#flmf}W0be-;B&Q9$9=$7{!&jE;HfJrqm_hI z%PWzFWO3>Lnd!|nN4nM(#26GY^I`B704o-5hj4wPzx|rHaZL>t7Iqt${oRG=74$9( zMXfTtZz@6SJt?Su+7$Iaa4XABI8U>S;0Xg_Bz?2!(=44u%u7Du>&fxl@p^_!u5#*| zVOiqqa_+Evc7IZt%yo);wnh;QdU`Ek?ZaPU$65K|L4~vUH7s&4$N@eP5O|a;QT1^5 z{1=CxQ!l+T=BsoG*jEjSj)=^6>W7>w9$<{2BR-l(9;3;K0yev_i8W15hBd~(Tyd%Y z?v7XsD;m5RI3lUAP@>U9CDWbceFGM};w@ZBzjAO=IF@&dP1H`Sjx055r5n?eNtewm z0{OLFQL?2g=gjoIF^Oi!|7D3U5Oh6(+aVJ!I*U#HT=x1r>mB9&0xJtJrfc`DJ(nAa zm0rF6oED8}=_u&&cK4o`B6P<~Kd!P%vt0@q$RjD|5#X2Tu(;5|>4EF&02c*Zs=XX8 zsL+A_uE0*T*a2zhNFVJbv>C;*;%^>0PCn1-j1Q|LOfyN#GUw!Li>?0-KTS*Qor6Hf zcLX?$`I6ZT$yRXk@Fc_=B}QAh=-TnAlbbIGDk3$em%mhI#@y;oJ8d?AN&O+)1NNSu zW!m{L@i}*t1cm&Byl&A7tW`Cb|KAEQ1I@|*9u2QHeay~ZG(Zn#dQBE`Wgo*e#@lNI zg#l_XdPim1c0VF%6NAo|I43aDHv-PLqR@yLEHd&%JcUT-0(j$L(;kkcJR9S=54|$e z)#aoJzk~s8#lIN>G{aP$mi>K*?N-hD|IHE}1(h~R>-uG|xA!PSoR!0EEzoM~C77TR z;CMCc4;+orGZ(ej)Zm*1Ira*bOG#Tf5NtLG5F#m88ivuI@!lZ2pu(pC9)(!rv-}=j zuWd&242@hjQQ-6}J;O6k4Rc!4_9$??fEBJ1~Gi;m99a5d>V8wxTYL z`ATCWYMK#!CAn>d>!X_X_9R?8`YM(7_WU)0sZK4to~f_a$(4L_WQXg-XLqmE*d^J%x7@Vf z?|^up#*nJMr3lD!F~{O}^WhzJ^wwr;4-|g14tDYY`NkduOEiDT@<37kNZDs4w1`UuM4gpc8Bj^CR2(g)2Bvm`NhKxUz7M|0r_k$`9!}&_uLF=*jZJ!9^0C z4;EmmA)#J&`-^&wK;Pp}90R3UoP7SAD$`kyrq;%9j#hy@R~sI8!;4Rb$!VX&RU~xM zIVmP!H6O!@rRD+yjz<#-`-o?*M3KEraq6mnxDNH154GjLxUU3L*KeUFwHL|>ERNet zC#k}I!{!*%?6~aaa@fX2YMgq^86BlgXImCPEsod5at6ib#^#Pqto`9BYeyPMi*>j@ zoi>?tT9k+JCuIsfbUxVV^#&9pgK?p^2yHfpRKMW<)`*>j_(g7a5xaN8XtDMH1i8+aiD_YL{rxLug0~PDW_aXDzERROjUQqfP`GhN3{a z-NcN0u9M~S8n*-87H;ePI2^t&__TtatyT2hj{Z&}mp+jC!;nlNR%{tVjJjg*?(poN zoI~#SeY3%7X1YW+Xpysff_|pY^Z26T^K^t&Ie$yQeL+JMB0-Z)N+t|NDtO80s4570 z6*P9uw1$Ug6gOltzvVB^uI^n(M zb2q*CrM=Vo>mBgK09xnaYDYk)-JbKZqO?h4`~%me35lL?SjNR_9nM5Zli!=odnN?k zoyFjJUG_dsuHn(mKV5e?1YSvSEsOB;rYN=m4K7Ewp1bll)>@b`*zpv&8xU~!QOSeh ztqUnE;=T2_JWlrBt$wILoq`}IJgVPDHESmx<-z*SnSpml1+cseJ@*5CPQs9*I!G6u zN-l?zp&o((JziHWr0C@39pF!M)BCYB<1ZWTHfT01KXlNdS5{rU;$|CRrp(2x&LxSh z4Uy3t7JXZ5BcMsA>Q?jTo5Apms6^enn;Xg$kYVk;`8PDIosofS``so^HnvPcdnGt> zZ0NyXpW#%8JGv4qTaGyHl_`8`RGI0F|6sa8OMvA9m>q@Cb`xF_ zJ2En9{aRt0#259D_*vzE?o&#fbVlQqZfATMCU7h7D-{_%e5 zgfrqp&dnw@uuXslJBm+IMDB%l>w+Twg1l~~O?JaL?WOjr3+3rS5M-+kv_eAA@%pwQ z+GaEK${|^N$Inf7tmyA|xGsBtsJI(fAh~U3zGMK|BI+mZ4ihplj8tKx?EGn~;j_mI z3d>+E`*YzO9&AlIb?WD=<@3|>CSZRqv8Z*s_xp%v2Lm>5vE8MNCQ;Cs(RLx7=AZlZN}QeNLkPb>B{n*URdvNu^s*9@^iE1 z?MOLiMXvq!#5d>tJ03^*!$^r;#!kPq&Pq-#>5!LH{Q`U zD5LmR>aCz&Z{XlY9T~FO#59Hq4~x#V&gCw;*l0-A@U!Dva;&Ms&b-VIKdQk-jIkf^ z|2S@S6K)?=HoRS)dROFRDAcTF9BW`y(H{R)W=&20novmTR;j9MWA96fH24M6BfsDG z?M*CQ!$HLX0Z>zWF^npuimZy3Qjr2a>w%oyg)NmX_nzI~Dk(F(mtv;WcjH%NRuFH-224{yqiw zD*f5u!a2IjG8?v0`Qd@1q8RV>U!O;{ivDFVMZ*9@=rTWqz9aUU4;3XW6&tS|~?fGbGc->jy^!d-NOXRTv zWW%^7LI~Gt0V@U+xbFe=Qn&ACSHfim4bb<%tuP}TVW;!X2)~+lTZ(23|Ak? z+2E6tbuSGV#Tu-+aFeo-m+jkv>UTy{sVv50LV+~! z-@Z8|SOZKACav0&ztnmiU6ul->Dt{5^$jv%qiqBnQRwpUo=n?=a zD2&eDpUhBuUd26n0u3xrn)tFvJ5O;rak!69zT(mL#4ZIK4tN`n)9VhKi$1?6IKjim z0bI>9EIHP|`P=F&ISouRB}cZDq;bEransX}6Rf!kbiG|1yM?#-h1sX^{<$_t{c7tn z>&=uKGFV~3RTyo9;SrKzLg)r}iq#ZBV@3v7J**16yM=Pj+ihfhT)TyMBvSt^qjg(d zoEyh|yB0G)al^Axo%cHu{RG-?7zr9n;p5VlM_;NY9m~pK>9@7N=KeMmk8Kn$PYT8W zMDI>j#l!{rVG>xtajq#Y=nl|&_k$p5u0GvYn;EpPRgrOV`-aGr$^g-}>K3z2kH6xJ z1<^18D^u%ND%~U;|8~f6p*6y|%Ju}~3bz__LqTQ!x0;a~ieE(ylgpw~H=9Jqgg)Rk z7YD1{+=5-okPl^Gq*_jCb418QW5bC69T~=!C^FgK8wQu#Eds+Z_+W30huIYA7)t1v zY4XlRrk6W)5~3TA^_Wl8cM$&ZnpP}&7&@X(xXX6)6qqj*j7ot^D#VKEA0;~MW{n{2 zG^H8{Us)Z2E1H~?HL!i%a~6!^3vCEmk_9Zn;@hmZ5ArR4uX%BS(w8iKt5un}(bsg2 zy5>UbFXNB{Kr=|dpQ}Y{IJ~o(ta}<67v@(je0M!t(8E7Y15fG1>z1I1>bLqMs@sjV z)*i3x?G4AST2(q?VPCGCDV`6B&#c~x$MLpzh8QN9PJ!+dpRh1Vr;g|8G4%`YUfE%f zwVHx&god5^5o4mObUj_5V`pw@c9DSu54Zqn;KVSKOtiC>Nj%sjwelO4ur&U`v3q~^ z+9LAPh-cON$Ri?$6$8^f`4)c9|_9cF_vy{(%|w4$G%m&CP^0bK(S9u0Vh% zG=Iu!v+XlT)PurJD$VG7UtG^MT?jANp5aG)Ad11-xcIfD6)&W-2SI9|>^n(Ep8v6W89P%Wy z1u5k6Ig1)PUX_x+{m`JTTsnUVKBz*`(=$rJWI|!TLGq_O9Gr(B@AKa#7EyT-=i+zN zry5?JkudzS#BDs9esZtX+iq}D@pL@if8pwMryX#4HPM{>7+PPfBSKy$J-1njwW!z) z-U`Dw{;w*&#I%y3Uh@feVq5>(mi4$syW%3Bj?l0f71f1O%3Tx8&&mBKwv?+sOA4{| z5=u)J_&jG7s0H%IkZigj#B)PzAYjMC!*vXrfNM9O(^@UT_Um+4 zmagm8F42G$5-Pl7v)JF;y#(5$uNqcQ#H;U@bpc=0GZmkpb7VVKRL>#rJKRFEUrjC< zjoPfZSv@uzkSW*7XZXmdNcTb| zXcoPjqarj$v1E2qsMBTf+3~YHy=|f~{Pnz8`&DwK(6alvBxn`S$?vkmO4sXoDo1ce zd8&0-5c<^jX$5Y;Z+P19LX!-25^yiAu&U(nYPF=U=Dmcp6};Afeq?jEy=2|W**`p- z!g+lxGxMS)CLL}E&Rh~4(+Js#Xn!t&CxX%TFuIV&9DZpBSbXLtBd6!y@C%j@ORx1n2Lh4>my_5ySBlM6XLO7<^$~jt&k% z4)!(nLOVGtwM>0wSz4U7mM50`{Ps1F3zE%*r z9c(1kc=?t$uQv6AJ8li=)*Et}!Yf#o^m@E=<5yE97; zmd591ca}XgQ0nBXVM}U3Zg$*|F@=_?r_0)+(pic($B`}#8VC{y1mfw6%1J3;{MDv) zu)SFA_&q!wVy+@ueuT25!xXBiD9oEPRk3iX5w{lYadvK;cTKh_B+9Ms7QQvrQey8| zW~mY3(^<@Cb_+uk1s6gUY>XR`Qx9?JQ2%_>hKm0vz6nF&Juvb=qEWNZI2!MdGr%Yw zQNdzhz;*S=l50te>#-q(C_2IonXyw3B=osw`9D+h}1_n6$V#h z`eOpw0@GCgaG6l`A}*eHxa1-0f6w6NzS!@p91k$o1yYhNr|Xg4^38^u>&X;q>z4dWSNbgGL4`EBkZW zDkN~|w|ytQ*~c%CLj+3L;!%I$-PE1M zp>dQp7|;>K-tp++u#^__J%+A@ngmS_SA=rTI!MTo&k@HV)q`9Ie)mVV!&=+6Z@uLj z=fXlS>5hNP61BrWo_e)7H;=^UvM7Cz_vh;qqz*(#N|OKKVwFaQPl(=sdMx4#^1Yj1 zxwvv|uh$($rtPLT!X>-iwv7d%puIf;b!$UdC|~zYJ~Y5aYE1i&5Z~jEmRfxB2uO zFrMUPv;FZZz#$dHJU|%?IGjJ;4p=daEjIXZt1a5YFWGi8P=S=;y@Eq}DeU*O>3=^} z8)k5R3!JN`to(W#==>HUT&GqVs<3?&mk~dypFoD2s5*C8)v%$D)>9gvGqyFBcxp&O zl3&fWv8&oAS*{prhuj6R*JG`d(|=cg%b?cB^0xOOTu-zklB#m@YIYZfLq&Nb(yza= zu+5f1-DzOTK{X{UnNIkH_`j2x=;27)*yrrD;^UhPRNK~3+GRMB@k^}xrHju#7AJ)i z%Z|>%;<0+asV=TD%=UBdvy!5s5ai|$#HRY7pn%+<(x5<_JDepY5Za{oo;ZdO^0zo8 z2{J(2hVyrq6Dk)R%hL26V)Cokd6j!^M)E;UBKRC16oS1IfmUVp?%`HcJcX{38u4e9 zYG3j1wFI}+D+NV(bUTkGc87T6tEQ5)BT8tttHpm@eIIhN-8nS9U59~nT4re|R-u;o z2G=LXL{QLS5BeIFES++=_Y%lLhEhxs9TN8-{J9U?LHG;HNkMi+G)~6kN0&IU_yA=v z1lX>a91#&AN4@{Gr!M*CRb%C0ySG=ul0B!b-RDHlh$BsTNO8h5+I)(csnycS+0nV> zq$rAXzfq8vw+#dx?MRtWlGRR1B!H4n?5)?=dquzmC$9==0W~t`YJgdz;2P(1+$z1{ z;>kq-O+r>nAmP1Ga7on$ne+Wu`I-R?iosF|q}aEYs{-{b4y70&(3{-CtELT2?3P5tuPM?v)>{ZRv@kIRbZc|4cH3 zMXNm?YlX@LxLG~Ux6L-$o}fkFcHzU2-fi%L=B=2izA zPPAzVC|t~r>CgZz_zZT#IY8#(B_!tKvHkXK`&j|JLC@6Or< zzT1n_9^T6{Ydhm$Cl1<+68S1oo~ybpAb@o$QrB4N^_^JAdBSX%@Qg@AS|Lf~^FSu|xXfDbdxj&f zT2k|6z}GQFabdk2{wz_EiXXm{X3IwTa4Fx_>C!>FYdWVn6DI5U4l#yCnN1z-Q?i-^&q_817{sE39*}T-*UVqYQ#%-VnV1CkG2+I# ztUDX-tVCzS9lH^T8rABUZ0fknADOuvehu=HUqL;U7dMlw9Bv^@kWn(MqrZ$P63=M9 zNa41T1-miAjhL_BVkOD` zKB*0!*+_}mCKpc6U>8E4;m1OD>i+h-0P;(jt4gMpmUqVf``RR#HQ~j`(4M{ja3!>5XLD%M>^+b} zRBSLz`*`j)f^=V;7D=L1NL4i`^d`u8i9;xYv0=gPi#6Hhg^7uYEfLpYxIfx4A?(IT zZ*OnQJcQPA)GH+n|A(A={*J6^ABCR@CblLvC$?>4V%xSeu|2VE+s4Gn1RWRnyC>gwHHRcqG*wV&UQg>jE++6={AuvVG{Tbiu2+`*2i zb`lwk^nI9Y%ALe9!sdI+1Yd&EbbIVlSMrmw=mQsV1*okL^DFh;t*i$_aIO6<*2=-! zqg1)P!c*fV$tnv z;GrNp2hw8E!Y{7X4^6M>3_Oq7cillt;u8LPsOeX*x5En&E2-*PcQ{kkcis6F{$uZH zzWWEyx&{hRAUM$HIL)~W(IcMn)pp<~Q`5Ofgd$-T|Cf9F|BpBQ|Gymoen6h=d?!i9 z4L<+9af76&oJh5he!xGwK|n!4{n=jPcjVv4pY0_~oDJyQZLFh{CT&+4kb*9JV1&<9 zh|Bn;gDSNn@W-UCR}sn%ZaYMI=DkNTlm z1&Xq*|0%+XXu%hqr>ZAd)LzZ6{MMF&Qz7pu4`Qz$16Rwvtlqg(69TfJ)KpcoTpc!< zOU?z-GGT^O7P8=Kw=4d5iQ3(vx&R6D~MG&@WD7*5@9e2pP#)pCOMMpN^|=}?yqK|c)NC^P@+L1a~xt%d8}demjr zW7@`vQMuUI%PKAVEzZ*}$h?|2VmgzM-(_F@k$n4+vsFFt!%Zu@gSV>B`f4XPySdUY zrX3qU80SlU@CMRxw@6qdL(CFsL2IFdX5LqAG7es72@FG&VL&=5%UuAIlw|fG#Bi$+ zDx2r`xTzW+N(9qVm5->NYUn4qh!^K4ccy0$9m^-X4gV_GS43Yff^A;POG}$$0!OPY zgG1~hMHCao*S+kk(2;{lr+B+-XGRjXtl64gJSp`$nTAZppA900#mB;$-6NY+-BW^gAc^G!>n8*pPfLYDV3kXZVl^Q?$WQTZu@u zQ%RuV?a%IHmtjleoA4zOwl~$7$oG2vNebjjCWV>miY~OO7CUklyPt2d>k~1N5{T{G z7v>|}K-Wh6uPp{};JJ&WFO+XWzh=smBHn?#ae!A#p(M=-?}lejnfQ2gY!3x2#Y<41 zWCjtQ{H!qo_hzl?=4;Q6qpj&=`k8~XAyZbWOE-ORGyfE-j5hDsq+0PKv7;g{9;7TU zObTvFyp_)^Yf+ppZ@!aqeD>r9`D)Uf*`Td<@UlT_ue0;jk5w}yX>X6oNSZGszS(1< z`v!fRWdhb{*tW{kV!?OS%C5O2qX~o9OxFn=v4X)|w`fY^ok|fRFFZc+w!W@z+v4Xl zZ19uTt-Hoech1cZDWHEc2-D!G*7~2&- zaiI@;S4B>GI6p{duJOih%?j12>zx!KlpmCcTj&9@bg5=`Doo!dv!f%!&_`rFr(iNJ|&af3b;M{f2~1W8KYR^!^Ez|Yp=Zr-DN z=E!Vkhf(d#;e|h~7vQ=o;r7YK0{Q#Y@bc)n%#I3tL)K`Drwu*C-FpR^mosS)MD2*` z2esG(WFVg3XA>g zA>=o$AHXlI#W04Mw7Rgcz>i&z?fxr|?Z7S>TM}dxS53XDW!1NyzK3O9sIG$G6TX2+ zqKl>#{FTy$!NX00+cZ6-;C`YJczc#jD9lu68EHj*$IioVw;44mE*a zUz{QHpAiQymel+o^YhbVe2Hy@4dLBt0)a4YVwwU>yj6z0YBB>a!)S6LofefGo8f*l`Le;%aL^?3j6JGhJtkpR7b~2>b0f9um+yhK& zWEVF4dx@7z?H1n72aSXPYpP3i#)`*#4CN+Y-;}k`|29&8*jbrMN5kvXl(5GMF!{QAIw#%l)9(u^#KleqCIkqG0NMZF1r-(q z1jNYB*7^75Ozt+;r(FO?63Jw;M?O7+{%qW_Ln-Ft>S9Lp?49j8V<}GN&P;p&;4T3a zGMbcHz#q)t`QbI(BlA5f+2g$A_~AY5DWOdbKC;UHlPMuaPn|zQ6NIIj%5+;s`TGq4 zKR;aHfiV+al=!C3Wta~5SSf!uR@8ZS$K&0Ayn*-Ch}y?0f2^Gyv=iW3> zh?Wl#vS72jqcvv|zo)D7_4Y{lR#FT&woc|YrSse{4)c(*#LFie@?M_tZo(vUIZ~X$ zvy&2OMUbFTWAS#mV*Nu*9+4nX(rEV9v`eXiPcDH<8?+`8uK^HP|FHYKbfW{@xbs#c z*?;%3n`_N^ioDSg-C(((z9#p&5VmvGoA18bL(jY=P_t8ev%H?UZMj^@EPYPwGXgHj z)J$#gfBn8P{?cXOM|E{2{9|T|%NJTL zXpGtQX^&7yP{zs@Ddp&BZ)iSaMf~IJ_{`;l&*QL)4&S<)12?L;VZ)p(?Px%MxfOkD zC0%Y62OirEqO+OBnS2?`lB}AZVhrfcq{TL4YR@yTf23{J#;9-3} zeY{$oSofI9;PJM-=fEulW~}a@!ul`d$n8~@^LxAdg&znNdJ{H1(VKk3bGWk1TPoyW z>(a(p3-CqVxPO*(PrMDCSNq7kp0DSSh^^b$)alHI^Az?#263aQ_D6m)@!cUFU0dzI zk{8Ri>gjOLS>uKGN?d!oow}sM`c4XRQk?U7{%FV~aI@!GLG6=Yab%A9ZOG7w{4ipd z;(5eKUnQdV{aT=x1BlJzQ45#?nfC#FwO4_KDA{}L2GIT452IQd_Cc!&aYm!0PcK2f z@8zkWyh640oUD8wC*P55C|E>X)RRGxrySH@h{st-7>+dquX7_?&}KKg`n$S6@6!9)&Djcm8a+ zcSbQ6>Qi_~x7@qemw9Kl);FcjORS1{J)L_75Hc@C%p``_(?@0Z9JupyK|uQ%e2M+N z>+9Koek09ZeE19E_>O!a4vUsYA!(AKv}s|7+^-#^{Op*}z>Lbw7vQ%0_4<(CG4t!g zOu|CDRJ|6AYPJBuJ|t$7LNSdM2B%qbVl!8{?Zhj8XfkcekNRjJtx1J^9>tBYbVo(D zbxbA2S1&IwbAanY;+f(Y+mm@~CnqLgn4WO8l5)QOvY0*i`JF3M*0Uv0F+lwi42_Jv zES;yv@=0?-*=v6ypV8Lt#phsu+V@b_SDIX5X~Vd=T}|E3={~~PS=q-oF?*g}?B@U* zZ)8-dx#xMlJAe$|i~UO`oEcEO^ZSX=)5w)|NcKmB^1!ZZa=g_3gz?qOQ5o%XD)$*5 z#<;H?bg|ckQ54qv1wnhPh1$6x6W1a_2+S4L1H-|GM;Si(8-M7z!4FDul7iT#*sdVA zz~|-2+q#0Gx)l2p{m_-7NZNGOyQ@uY_EE~|P^B=@Z?nHZ;W)cZP&+fhN`;y^5k(}M z+mKyPJ^(Z>rzK7OnX=hG6S86HdDXC$=TzDN)`hL>N^|rFuVr!02)S~g6P@0XpEr|< z`BigfwEVL+z`q|zGvUVYzv-;KzmY(hIP2%Nq&y0~3aOyf1Yfwka}pt}pU|TIc;}>T zwhja1h>!VMx0Xk#*Kg+B?UOEESM9hZl3;Vx0i@qnw>baOW%78D7i^VQjzYI&ISzFq z{fL$h`#|X&x9Cji%8>8(^mrk;8M`JQ2;hCki@rh~3iK1)t>`DBM8PTQ5%YdC-gPQ# ziOaUjyEO1lJOeo@+eQt8VNMoQ=+V;U1Ld~j8b@2d!^iiWkp$EmC#)Gy%O@BoalUhS zr}HCY7x3fypK*Oce#6HGA_|@ZGoJ7QLpO9#VhFt6`cB7pQj~~z;mA=>4Z6SD&L`2Q z1lT8pD81BOqa;%Xo9Ys>kdU}k;!|9qh&`(9Is`OtECNY&rzrU)39GI+t|UfQ%biD( z3umR%)q}N;NLRyvt=H7)t~FiZz~D*Eh>J$~fHJniHV&nF9|Nx}Pj#!{n4wP1c>ex# zGQ^!a^@Egg!RKM^na|N(^rtD1Y3&ND578e7{yQ9_w*jUO`4(VZes+$Wqo@9hPsd)U zBcJ9T&qCl;|oPHJDyw#eZ(eNSb{$1Z0)^PGRg(-kz^{77N=@^Bb1 zzqo!G3^e}0#!-tZ6OJ7{o=_=>`DV772(LSf_1qHPwJ@1}T`=g!Z6$?tpL|AHVRjR4 zzKYyQQP5rGZDgY2rmWbJVApwPJ;tliNIP`$YXoOK%g^2Gs+^M?^A;4}<_JgCi|L4M z?4i)abtilUjXk3$lyd`1_LvOLI34a1CGpc>og9ZE+6O-(ZbLkuCaxDWBEt5v^j0d* z@8?SxoQmB$M?J@b>&|+R%J^C(oD%or7FDW?eBmm1#?_;W^xqc0D(t?JH%is%i(vxaaBZGxU1Lzb}abkQZ-Okjfe{25){bh*IZp*QxvW&pbu{%X3|jgGTu)l^Qz%y{X9&P8#C~#Wh;^x7v$K2%2 zGMIGHpSDL-;TCzPS>R{4&AjQABU#`!3(4HXd;E)hQd9^(V4{J{G?|ExCdF6Wr$N zXhHe)@OI@q?j{E<1-Jb3Z-;WbnHpvW3x{aLJJ!>F0Gzc*e6YtQJdCv_#1a zeO;Hf78KFYj01kqs4EZcz?CGr5{k^z9o%y=ZL^N{7Byjw1-L!j?L|0~24B(|xeMkr zttu?5BBzQzRsO|hYf~vD0Fk!wLC1|Pk@*y*vEY& zSrw8B+L+gcTC2|B4&g?Hlf=joQnF&y8teP+YcDiPQ{oBr_euIsYv}DhZgm*f#?aSu zuu5p-^bkL?R(n2^kH-$q2Sb|b8XOISCw{nj5Wzzh>~4;L9cmP>po7fbp@ zRXvhT*y$cVznS>y(hc%tk;U4N%|pHD-p@XCX?*bAMyIAos4N_q+{h*POl;-3r5LYA z7CCXc8Lu>vj~UGc=HJ>qB}+Qu@CAgKZkx4UxSh~F)^7(8yTOOJqxiAmZf?k-RxV!z zKqPk64m4x4*HhR*GH=b;b3KYHd4PKMTfef5wfTGHjMM2;jyEeZ%R8XQN3x3OasjWgI7LJl%e3^8!dXMPF!!}J4gp4?Ol&PfR_5tbc!0O^m@9D|kJ2&92b zw1M4b8lsSY0Qd(QC2L4RIH^L$&aOn@g#T#p3$aLym-$_SaJ;YK7sD@xshFmzY|6A@ z267Pwj;jqQ1GsSpnpN~pFlhC$9-<;d;!;E`p;n=3qQrAzwqK8UC%3uQ)`sCxRWh2F zyO*@NZdoL+5;_{Xmsxg;tGuoow1=`Wc+1v|8A;}aNBzFG;W~#W;Ktc#OEU~GiE|j~ z;x0K>wzoW9or6~AvYjnLddkWX$%Vy;(dN3`pY$Yiyl#AV? zX>^a6J;6wUJXxBPme8O^BSpvEBvBTE!Q{aq<|F+Thes8p#1SU+FW162HY!{ZqJQbJ z2IIPCc;v@*DM2Aa$Vw376X656V7)qJ^XOtoHnF8 zXA&Ot@S-Wxvp*JQpsLWsScHgl;av|KsG5(mmzTg#M~037%Q={I8}WpUz

Mo5S9o8l3gM+-LyGYB(47-L{T ze$T`TaS!^#_=87ietkMkoU&NxPr5k?gv_#_{(s-GJ)I^F!-#HTK@9UxOTse2O(uUt zddH?Kdf|0lKI7~o|4|2s_T{omo~{|Lhld`Zui|NB)DhHD@Q$qRq^@2+V5 z?=a`ve}+@poSrl0B&a7~Zl?`4Jl#9*KCJOOEe3u?jRAJcTqTMRA_~Zrl2gQEA_~J= zemG~eRcWEP@)720Hw zW-ca7S?p1eTHs?1{Qgk=+a*GalJ>aUj)YXfYF_%5BD54T4bJ|HopB6?#;1#9VnG`} z_O6kfaXnpYO3HRTqv@-2FggOWih6mcD_>m1l=I%bAp<+E!)S~aW4U5CS~<1NRRVq< zgv9APH+uTZ2X;*3v270pK{j-aV43tD$1za)p6m4E3ch!(ay~7!x1Wg>#|>5GdQ4V` zzYMR^*{gY4ytmye9%unx(dj23=(K56T6YygC=KW?n=rZ&c8%!mzt%dxwkA~ut+$si zt+YG>68KiH%`y<;Xw4f6q0UuB8PuAe7Gjn?-z&V3#{0~DBDvV~Cs=#TUil#l5jSGq z%MFgnPCl+*-RyrJz;o1|XAyWNa?e~I_y(Gs{=}4vK&I78eK&)pm{NXRJSaSV<5fy?9pWC{;qjIcrk=9_(q&E9kmI2MG z{N)Pe?&gMSDtK> zHCQXz)L?tegZHbao1I_U6Oqgs(AA{fTp2%?ExYp z=%*LH@j7)^U$dq>7E9xUe3`S=KGvD{wM(U$p}bj^WWptFL1(A&l3r)f)@2|#es_59 zxM+OP!@o#Wv}?va&?5BYHZs`Or-rkxi;1U}ug2E29+80m_QHCn$Kivh_JP3$%4nIb z*bp7y;wj@jq?D?YAk8JjWw_aRYZ1|xjcl4yDQ1daChs3@WC%<#13t0onruu-PRCCU02;~0Z znP~q06J4ij=RC`#zJa(+m%M}~p~@w4%}dPFCbXI_tE{3&KXT=~QXbbc&mVEG$SP(j zEU$DnoD-eO@^;TUUrbBj=VJQP_gp(Om!~4{F!kUGUzN7w7o7@KOt{1YV~z`Zf}9YB zxOsR*Z%!V+Zhk3G>eJ1KE#CK@kAn~0Zkv6g3b&2JR~@Kx&&3RFKM((n4)>8JE5F^n z;@gYYiW;vpT3waX+wvn`zI#kx8QYOWa5aPb$krF;R+yXG2jp`NI(~k=k+)k?Qtm{g z*g*{`2^Y$|M;fw1?!>sTUm8-Q&g4WM$=7x3V+S%0T<0=E$1)BpE|hsUgBDX;)m1_MD=+&ky6fetL@L@59 zKYSUapBqXf>({`K#vN-CGQ_*rTFoQKSvUCk-MUsrjNg?BHzAW|cZrl+RvkVp(g6>! z@_c*9bEV1*Cr)6{1W($`&quyxT~iQvG2imZCTM*4wgITkuCBND zV7qteFBAHC^>Z!W-MM|jWDt3!ic-0g_`>Rt=M_zM620kpyGyz|q!)bsJ6&TP5Nq9?s;dD81VJiGQf zwcDbc@q>MLtV8Meap%d!Dh48##A@6-%cL#o9TV zmF0`wt1e^f*g-itEvs)$wtaxKY<_upTf1`B=HC6l$(s{nyc=Wrs*yRmV=7FEPMeDs zB~lh)pZ+yd*pdb|_ZSvRRjt-CEzGbgmEqDLh(w)ColKoqNBu{Y{=ygKkOdh7n2I{8 z!}3K;q{?c270^486$xamvLJGsANoJ^?}7|e7W&K0K`Jx{P1GmRiffMx#|A8E8sz-j zBB>CMZW8B5HcGwU!7_V1Z%)=40+(nyd1Aq5F_kQ^9KIF-e^P;(gPMaL7Jwf%p3>bE zU0?Khwwubr$vDVbwGt_xGE976aWB)a;B-4Aur?5ChK4F0dbA;dl5qp4E-j;x zY9BW}I&W6MlWmHCk&@Vy`I6Ya90%06t##uy`QrJl$;a(&3-Gw;tURkJ4&pVniNScs z1gppV(MXw5`BA^Z(M_lW9oS4YHdPm0%qk>LqLJ z-+N3Cr@w!mjzb1L$a$8PCU5<0FE`w$f~aZ0&`u&k{?`U*yhnL~ThJ&!8Lu{S3}MtfKo4&pt1|?$q8!EIPcwu zy&~9Cy0f1~i;V?srT*~8p{?w7N8b{AiC>VCmgRQurAizfc`5MiXhlPv9h&( zPRf9)E_Ew@>nd#f$`zDeBRuquYRHlVAGDRzHZp9KxFJ1NF+8jvZ0FdFKZc+F(xL0U zj80bv{yCA+)}Ip#F?5Wy70$QS7qe5|a{vIMdo?mG`7zdV7F%%8XUyX%A=a?7ChgwLR);>w z{@chnJ5GGv%l?` zlH$iQ$%gc)*zdzlgxC~+(Idq+ivYb6$!Fb5Sm-!sIsS70?p{cQy_vxhKFnQor6Q7I zp81#er|1}SZ@W2C?2Dw~u`d)D|DeA{SQ^~%O5C?-cy#^Bc=bH_?N;nbwMf~lC4Vv4 zBsEBa0<$@-Kc8M#N5^V_lQv7O3(1d0WiUE~|2^>+v=W6x7KedLzOT!?90(WeG)?_k z-_v@HKYY|o+Fxy7oj-GnHi;dyTptjCzo6zZ|8+BUm>DYvwTPYm;e037p^n0W`j=ZV zM+U+gwp~pPX(B%o7wR8599--%2#a>V=D>?|kc&i<*44OUU<}k|F^cJ-ll~!(oWvgB z@V4?M+uPbWrHl~)DEodtMjXHU;W}C0!M6VGYRg07F2*tYA&`!JUYd|}I2h?N)B;`W^{YDWD#4E@zD2r# z6`ct?Sy5g*9vM!Rjr=6)CkuodGOc-gybaaIp z1|RA0_w<<`^BL59eubmcs{SarN2W({79(GIc!Z!GEPgZMlXn z61a3Q#Bwe3!!2+EUC4xXS;?WfeP*XS_uF=3vbciDkLd%>cbF982@;22=yVe9!Y*!F zj3U1%P+9D;MO0IA;d~s@QfA_7$v?0Yh&3RF;-Ik{hLYy%>Re~*B$!Dj<5&hre!;N^ zSo$dSnkqm{%6~fywQOP|+e!*I)KaiBoKNfWA+NQ>ZHV8NvLDNG0k8N?9InC*$Hf*v zCn|6p;d8eid}K7PIDk}*RA(ET72hy+p|b=zO+Z&{D>G}jQ@N{oe~ll0u+p;{pE=op))S3^Cied8 z)dz(yHfTZ%{Q7MeGg8^q__Yv9PlS=3jg678JF)mt6Sv^kqh@RS zm1%d4L&4(1OtMEL`uTk3L!;0*XUlicNtqJOVne5$(&mRaM49+4Pjk9PADcczQ+Mfye{r>NOrZ zKRaM#nt0iFlQNM1 zwSf`kV!M3>`X$jUVJm4XY3o*G%X$24L(hISi^H;Kf!iiRj6s$`mci$mVO{2=O0U4u zNn7_x2j}NuCZBx=Ys9AxpI@W=HyLbS=;(BHUN&DGxNvxGO@UMEW8y(0jk&bjjN6Rc z4iE09#7kKCJsB;sm!H3Fj=6KlKqW%|)zWLcy*`0waD4)>X7Fm4{GW*1fH88}6vGpJ z*Fhhi2Mts>^dBuEhH17=OVB955OYH5xxcT zTqCZ-LmMtRbi+AWM#y{Sb&VBdNY>5~EYQi@i~Ep5H)0-yzvqls&_!VD?a~#_MQf@` z;y=bGFrN$(hK|OIkA2==F+K#mR#HzRvDHTUH^FeN)$EZ43re(pfXh)vuuj+2>g*ZGycH^^m&*l)D$_xIW59|{Sh{$}70{9m+&sWGTdz9bS@?Lh zi>?VFXn$TBf9TQnbK0oV>q%cRJy}q#Djxop{i_}p{ZOJsoAt9!uz&?na;BAxtodc3 zhpijmm)PC=*`3qp`HMF{)`ys&SH1VxHQD&XOtKvbk6i;@bA&noE6wPda(s_1p{nk+ z#t*@_4J*+rULaW@S>XPgfa(JMNe7mR$*!d^A8Zw8!?K#1=FLe7z=tV)(nbsPAFIkB ztIx7l?147gO>u_ERvRa^8&UmR)efRWZ)&w5! z;0^zvNIl57Ta2KzjjrXV0BT&E+cLD1#F70q$pyb7s~pj?UL2L+4Q=t8JeY-Eujp48 zi|#Xu8rtMh;!Lu?WF3+*{ga#4M6+^}|F|E_Rds`J^2k)-^htKxD$wyj2xtLIrxH_; z{zEHhHN+2 z?lK(!--_z)GoP^*4Xyo;9!Gn09wvvDpC=~|K~?|I<802~N!K~AjZ9juDA~52A>f}% zki0Zy!x9T>`WI*AnIT{x=W|YC##RrUm>YQ0zskmYU8@$~?*{1YppF0aRiGKx=YH~W z`%|F^Y8R9d^dBy}?ZeAGx7mQ4`n;Y8)yL{ciy8-L{o;t9P-F=(@*F74vgyfp7R<^&{u*tPD>zY?3cJ-GXI%r zQOf-M_+)bdT@D6%5!zx@iZj~Ip4m@dNI^gls@I=k%+-n$?9VW^b88|Tp->DV4%IBR zRkgW6lmR>;UhutGZ?UTm;E!cc-1%oH2ZBc^Um&>oAWe+$M+}L=%tgR7;(#5bLTuk; zJ|~8Fq;xuHQx$||H&$sRC2f7Ww?R`#XUp!jOm3p|6fr&+$Co2gK?*?%!2?F&12LW` ztu3Nh!?K_0R>k%)k##hLMl{n%IJBsWmFTFS`J(KDqJN`^ioT24BwsdRc%r^k#xv-xfBB5#*B)-m-b)~^)f>rtSx({E22V7`~s=n5PN8-9^B!@ZS zuX=o`hDbe$RP%dp=?P9}1iiJqaQJtmLYPIEMfgTZ)P?MS^K7n((5r*cqXYIxMYgU@T(w$kke*Y<{7_u}l%mH^SZO$Entga1_~on4UQk4Kn( z4g7u$air>-4y+D2!q+>*r7(H$?SD`pY`X>bgi6cOP2eLZ=?QU8f>(l9QovM(PFsix zOzyD&@g1{@SaP5%mAF{?sQZqw`i?i|^FFD{(&1_PcS!?gcz=eF|DI)0R$upHW9coQ zG!c%JaE^m}+sE6ZRo-Cw#PaEO4Afb6;~Lq+c9QS4SseVwt`}J?X)ONmxr$O zwi*}u?j@1{vFjWZ78I6jL>9|g80?Hk`N=gssdwe;9F$6wN?-I^YAb`LuODS;FVY=% zIq%vB{5Cf;1l61I%E_7aLC-gxT&*;@-mYD`nF50_+=z;q_6}!-{sF+5pSy-U2m!|)uCr+6zo^U0rot&CU zqkxMv9QyG?6-K(7M~PqJjYICvh&Sib(@NQfTos4YmYvB80l}?E1>*Dm{D3ptBkn8c zDHnn-woJknll#s1LTnBJMeF+bh=ZMD1DKa}+o&4OGbLc3_~2sV<})W(xMP@OnB!EG zqqC2M5A$9rWwIj)(AYld>5bB@7&lgGUSq}eax-*e#DEEQqYJPg<>7ELD0*j%n`k1$ zBFG}h;=qw!I<7e6pcx)(V``Ho(Wc}}9a*^PQ@{9u{xj@rbryr>@iL$jk|w5=xo&sj z3mxg_l>m+i&O|XBct^!L;Tm-;(P&4exKyjil3b$ds5rJQnOjywaPL_`xE#7Iwi&is za(nVEn4d||LHhP|40+Cixc!Mc3cazPX}$##}p!A4~GnRUihL=3A;y+(AH zgPa{T@6RM0Am1{B#RTK1l~|w7m@Nn4lGqjN8*BXHL8)E73s`boqqaHhpwOtqsKlsy z=cxLD5aS)=rYT)g1e}qT6Ba{mR5>p3%7UyY1=KTprLKn{i_@o*(4QVH!3aqRoNnI( z`4b@0u`a5%qQ(`5*|yS4#h{XJ$ zLe0h%U~yo`E)PT~;a{tp*@7U=(XsKk9;fE{>!R{B=hPwmSYDlbHQR+3jbB*vO3KZ{ zl6b`BlTUbf+os58k$92+aFLL&;(6t&FYX_>kg@G43IpI0a+y#ABnK3raW#=(9TQ~S z9ffk*!7v7<=uE#Ib{q1n@JO|#2IGXN zzzTeU6@BNWJMB6WB|9``#>0(-z|Xi}H8nzh;V9X~XmkOfM(!-^x%(ZGR@OQQnXtpY zTOp3vZLJoHQ_B3+Nd}W4;Z}^*b22Hwjt_B>`2J{%hD?t7M-C5p&3%CO?d|Jqdl52M zh*b}~hW|KiBgBcGpF3vf@4qxjagrhpIxrt()jJV;o;s-U-&jwip8n)YYyMxw8xGxJWd%`?SZ_-7sa*{Z;w& zz-?*p;veF@7bWOD_IShe?_8~t!+f5MZFv*;{lKKGEJ z{v8V$^#iK{hdBU+Bof`&G9}B4cEbe#V;)ik^1n6^k|3mw2Jt2(pYPt}`(ksppWXne zXeR%Z+%U2`$AyMx-0y}w9eGIrYVgOl6Xf<3y3v9GkKOYxoXTYa1xFP<5>)^VBPx;k=aFFEC*jAgVE?ghOAD5E-wLkkV_sQ(s zWiNR7-8fXm4Nj7Y4fa9F zZlezSAjvQ1WHw;NRnV(6c=K=KpPGTQlzjloAB+8R+`IWYsNx<4Vj|R28mxfR6#8y` z3n36CIa3R=22UqVZ+35?ShaSsq zIuE1WvW1@39Yigyezqw|Z=`v^@bVy*s{zj#bMXG;$?yMAf3MKx($|E*Dn1J~2z5citWhS0j>v%$ttP$0f-7~U)TOcG)0ACFOl>bt-c zLnja?X{h9?PmgE%Nw*hXiCBH~e_Sy3Uib>7uAwJ7Tr0y_lYXPa8kuZx+HV>SW^k$I zz^R!i-k_mpPwN-Do+54?!;?mJ#O+w>=fR-`_)v;iQwL-yScA3ZOIqJ>6t=Mub{4Um z2n|^kjukoK42GxR%4M&9US7JF)A&05)4HyGqmBHwI!%+nBNVnBsGQK5w@GdMVH_ z?tcfKyr7j;di%nQ0W_OY_5P*Bt0sIP!DMvTM8vjh=M~-z3SOIU9-()<2ozPEU%yXX zz_1CV3te6ts$UE#-{q@c3NDjjyA zhh`HG?DwNsUp26ZwqYwSfey4HAO|X1C5L=NX*n(|IUpoN9})((d`T)Z+I(-V=8xu}Gp3Ys`!P z{jL@IyNxn<%ncN-TWoI@jhf@z^T_306EQgXAAFy{ZFx=@dd7yYYlXp8vuM~}p1B_P zfIVRTifF+KsS2qIk7x?5W%x8A2*D|#S(6@$crx5c{NR2b581_dDB!5zsNgwx;0G<* zNLHB_9fI~=c*uO9>Qrq_$vce|jc|D3wVb zTS~~1{Bk8)i4CFbekV(w>ci)h0?cDwwSze*q++D!A(OOZcrky?92xPY{wzJYBkRIbl}T3^RHO!Fq(rdr=eg3gxucVVOe)z}rUTw+?8=7Z{# zdkm4Ls>TeI1*K)s5{Hw6IHS#~Z2wG@6x&UYmi=lJk_HRc>8v#pZ>5`LlXeSTT?(jdmv!ez|J-m(Dh=netcuiAFQI#ddbanwUmBIu9xxde0?$l1x#tQ=+^RJv(>!JxKJ9f zs#CIA8mU$z?j}F5LFz6S0({rMCv$h>Uv}Y|(qW4=nw?J$jf_lx9y>u5W9#VGXiU#( zNGSMeZkF8}^%DjKYVu)91~~n)WZ_lU1QJcTh>~3;YpoU--nqOxL{5KyT!^9US1CeZ()4WZ58! zu1NjgtACvWJenxc7mDd3K)gA3Mngdsxxer=M1R*Rn<@I>snZwsq*b+6R&|Eabrs!N zL1&KJ&OoOrWGeK)37$^4 z$cdSMN1_Txh1;6bU+9HLn!I3x!JR#O!gYhq7p&1r_bXl2YK8S|4NT}={%*YXB(G-p zTvcXu4y?3ciS+!C#;;95$LF1ehl^_U%8rf7Z3Bk1D2nYjcc#Y?ca{@)!Leib63O>l zC#LUox;!2t#R#!Q5v{YABYp{G!o>(_Tq9P;xT?HAQSr8%a3GFeHU@j>Nf6nEX2LIw(M3a%t}*+_TdiTJVOVdG)r$Cu85$?i6E+n77wmq)?BI@9(mqv# zd-fFf2F&VsREf0w$!qe`|Dx=igClRczCW>T+qO2gZQHhO+qRR9oosA#V`Jluv)Q+j z>$;!ke&7Gz`qb3SR5iY*&*`42{++I-*D8gK0`@KMiq<}p^w&##A6eyA@AW*ePNk|{ zf6n5mFzU}W`?U@^ehtT{(J>yw43vY;&|B*Pqgf?qTG^Ids>dMhYxF%OcVfYYLb^pS z?R?0DiaWJ*OD@f+hxRCPJW1SF+i9(W?9%5tCkm^qO0m zie?W&QQQe0^uoF#PESyA1#`Wx&h%uym&_gBj>+(aPenrbSoPoM_<#e=W68qOLtD5EbVqBW90Bt>V+KXf6$wmAxFMe$taiE zhjUks5bZJGD95>b!M^%giZjuA7S`A6_qQ;)q#q2xfDkYDLPZWHwcsc5Msyk8Ot9c5 zIk7Q2-{`?dmGEEvjf(x-UmB_et=zxp%@~uQU#nyc?R!f@lm7eAodTWV zq!B6Fom$4|)|U(nITGbx((l(IhIr79sggYwO9V6-*2pCTh1P46fmMH&^sn-}G%4&B zDbpdu%RNyYgHkK`NxTppg{KfG`AJ#YM5TNQoA?q&@!!Hm{zn+fe+iTOA7PVF|0S&a ze}o-@{+F=D{}q-apvtf&#uZ3TFYP8QS74Wo@_eu>mx__U#^Rd{g3PZe-yt>K#?L-{L81}NO$TfW%Ao^ z*-xXzNbipxN|JlpFdTF^F$Sz3X+E!4#HdItgw+A!S~Yh-FYXx3T6q3}#=VDkv%oX- z@&iC^4lL+X)oa;P3vE9_cugTqv&KO?H@_%otFlK(5BSS9`wQGt^G8&PuD zr2j_LWdDCTjsgz(--t3b_%FvLgGc@oQO|lTj2JJeo1$_jEVR1K6A-9ikLZ;uWcAZc zOC5Kyo@tXcy=k#r`TesEz^pSDKA;+~=|_nRkwDRzaWP^W^CZjK9K*5s8D7dte% zW?sOevw>h;!KnXmc@P_wa)bMU5Ik=34eEL{d4PYUSkoR-x11w~{N;1gn*9_*9v>;- zUZW}k2hRVU0*Dic6X;nH`1uGI`wHD4PQS_iV|4)4^w#bsV;8lfl(*%_VD{)Rqvxrt z`8es2_|&W}h{)z|at=n?(YCf=zx^W)xSRW*+bAlaB3lUdqYBkzIi>&C&OhDP&i_iJ zEZv0-`|ZH5W>YgqVqw}Gah>CDH}Hs5|E6OFWPb628sKUBL)2`%Ce#Ba{Gmcu>N~K6nx}j)ZKpw(LY6u^GEI}nXAzS#I^8EpU_@Ne9v8s)w}Ivf?a+snQkU01(+lO!aBHB6Al01zHxCgsbPG}qN=+V1O_xT4t*kl1 zqYlv!ZFSCtOCz0=7D9`N#Qj2-xN`ah*eNR6C%W zerB-Bma7GLrqA<l%%aqEs5T{zrW~>WssO4W9=X7pe3~HfuiEvZzqd$&B%cPb zEbw$PVA?F9-JhUlyBy&$q9F0T0yGvh_5>u>YuZW>??n;rMR7b{szIvZm2IG^CUm*( zy~2#aZz=!cy}}m%b?|}5X(q`zO(9?5uZm*d3|Dq;WdkPIO$|IvTtmwlIbOy@f}%IQhqh`zT*##o z4S-XKzp`5b9}%ixe{CaIe3z(YuK~&V{t~Oim6M*Oz!qLL!i0lW z2b#XADPAE}AywfuRN>X8c#&3vuAXhNrIfyMsEuA0Pgie;`~!^`eTI^X)L5r#6PUtr zNTCZ3F(7rS&R)q@aY=JXTG?wfR&5`81d-hC!AvLP_jI#Es(o5xLHt<{rxxaa(p%^P ze@^dOV^yzos~M7{2eb6X8DXIu{fF!1J4bt1IMkxRXB`{YH~|^ZgA)-f|AXjCuD#LM z6iEF5CL0?YBLN=@*|V18Y12z?HZx)7*DU;v<5*D@xt6Ik&wclA*N|$W5a()j*6Z{@ z6KU?!lW1l1`p0*5)}uHbbr;66)CAT_;#k%{YT=^pLE^aANKH^N+#h)FILDw=I!H}{ zFmlwIEuB;4NjF2Jx6lsE^m{TzZpoRG$7HziSk7*?%Z0k36wqoQg;bsRkgAZY@+hnD z%kL{SyP<$G@-v=3KkQ89>UWRRY?kBN6Hie53%^YF87fwvVmDMq;#pHP$AC7JtIsXD zUdx~!pPTYFH7TD#dt`JJv^lhID$BWymZ|Nk7-ZH^Hacg+w{Jf@P}~!_Yduh!w-O=j zr4kbk>YXg1Goyb;4rjBgzliM1b{qBXsoRq)c1KQ;GAGN8%5Zl-Rbtu$e-NH11F4R` zh}5;w9+0YNhQkjx}|G5}I@wb-He(R?6y+Y@T&>)VN?=B_oV zkaQ4_>;IXVMFCqqdiHGHo>usccUFI}1@=gD@=JbcW(z_@|7* zC?sB{(OFVXFnt9Sr3I_#=d8$1zDJMb@Lc&D_cTSl z7t*hkU<9a*h$B#?Fd^``5T^YWj2V@B&PLAwgi5FohHN3dA7PMTEWo-3EiFR^YAG!g z*>C+&I)_0KDBLS>>r@DAL7`W~SauH^jK99zBcPlt=ZfLVAuz%>F~cbJ_S<_!$USy5 znm*`>ZGnf^b{WL8*fSpf^(!UkGaXf<#Gp4z3f(pw8Tk?+0>|(w3ir~@-HN~ZN7q35 z(6!gCZV0v>zBMFrIKlT`iQ<$KvPOnTX|mpzf}t1SFt8iGIozB=1yF<*VMj=O4=P6t zY|1b-r8!1NiyPB758+?ZH4O8Mzknu?wWj(ngwggSSMpU01Km?S>L1cez-;sq^oSau z9ijyTosqCDfZrV(zn#|0HO*P<^asp^%rw4pS@e*pvEm zbi)vVlm%;?E*L`^WZg&Q3jZ>#d^v<(5GQQ`m_R%F1#HU@@=%u!P&UT1`sh<^Azqos zz&oHMm!0sQECQWqe^UnpIKjo5Gh4QgdJ0P$g0_G-BomQn}SGD%L z5)2jXQ&cEY(E6GtNNW$%a#W~73yWoiTzBdY}SjeC8ZqKNU( zfF0s5SwK6eKAYe0ssO2cd@Y0@s4Hjb$&LWanK{#r31j{X_AG*_*BBz;#wf)1bX-C! z;tDx@{yqDLJ^ovZIaJ{2SW=-NaAvWuf856n?l-qQCfYk-bDE+E-p69-;Ftp5nkm7} zk_P;GgDqolR1N)p-$32H?wfvuIUoJRtp9 zAcEc@e4|=%Up9sa-$0S!MNC4r*g+J-gyOKX-PHEL#0kNjV0Z*-XrGe`MdDQv;lD-T2`=-k=(pRt$mOKIfZJMEutz~jm6p}q!vi_a6=xk z4~)^%@Wxmz;r;H=m?gdkvB34<#Kuf2{s;Z;h5f`ma;XI21s(tbqu&rrxPIRZAS^}4 zrAP3ZjY{)bqWiz0>kNxI9N3JvvZOIVoMgrw6d|C(a?;hLMG1=sh&3mM!T$mTGvS+9 z+wefCAvgc6LZY+U=|%_!3J0!eC-f%OKyb!n7;Fp+!yd!M!cY|l;s(Z)Hy9aS!>&lT z@aNiy*gj3}rSe5Fra9|(vezAk99hAUn*(ajle)5H2sZ!2nZ|h~%DpiEc*`zgRTJvR zTHcz@@meDIlV+Kzd%^m|HL>y>u5TBPxB*RZC#EsK$Pn5d+oD1+02SB-K_0jpp7opD z%hT@jM#TV(y?N2hDHo*lw<3@^z&(vZamfkgWi89nPS4D-y&%xic0T3hzSYxUb7KM%M6G$kq zmSD|mK58D~0K2#^mntDz5-Tg77W!N{?Qfo7E&TH>JzmH;=uxpyAx3ksQ9_1b=2KDR zYYu}L;sX%M0&s*a@)5tl9*m9TLh`s*&<#jrZ%8-fL+((}c#2GM$^lq`_+a`Gq;%XJ z#TM&DAL4!0E;y>5%9k2;=Eqp`S}FxVa}vC*Ee z%o4sd)o6|d7WG{>2Lw&gJKpeBdOI~8-NT*`-2lh=x?6fna0td7R{NPr^1;)QHck z?%TCvf>jAH-1QPsMYkD>cOQXxlnxhO&#d%q$0A~Gl%?c3*oRmx+d<3I_Pq?>9y#_~x>@_HXSVz5coAzbl+xS=rH^uAFD+gL zLB+4qlUukTIN<9t{N>JtHn2umG1yMhi z;!%pWb$j#n3|aAQGS2ooR3Mo}oA|w5RvdE_siOOJvIjW!K+RAE(BtYs_=ix<5|Rk7 zX`3&p0DKmK(QM&~o8sO$?S*NwEP5|05zZeUFP-zQIj`$Zv1LH7VBVU|{m~pct=Ib? zygrK_zCmv;jPV;9xVV5Mw4iE8zCe$ZN1B*7p6}zD9(-X^VZ4+0tcLcW%oz23C$q5y zn+uR%Myi2Fps(%SNP0A?f?W;MnV7|tz2IFeP+mz>K@F6uxsPG}{H@h4FJJW9&QuOwPfT+-C+_#wn63vCT`UY>WXld{C`hKM2+|8-3 z^q#KaH?1I1On5lAYYWk&@V~ZExUl{xRiJo=QLECk7G?40+PmHm-1g<$JH_2Rqw&OH zylTcLN885azkY}f-hjkWx&ZB8LSG9FDnwR379&lu{2YBP-~|tc+uoG94QI4F5+{s< zTmnw1Z#~g*!E^dOB}wo$a21X&7|?)9I9fcy`OwG#TIoUyb!`%sXu;NrYJx3%S4OJ^ z9)XC7a%)Fc+(R?*mb1VVA^eL`S2HuJZfb=KtAQri>Q@G<5~g_+Z3$Y_L;;(DZ;+S) zi5b%lx)%#3;@RVkY8Y-&^dx8STw=cu0Oe0aKO&zQ3HpG+F_D@f5j%UUJ2o;CIuRqh z^4?nk*>Am*KaiWAxuOAb20cmf+|mJ2@a#z7>PTirHmu?3gF2yLEI-`wS@Dbza<@;i zN8Ej^xqs6TbC#aqCdL1tyyLIyuXC?r=()8}RMo`Bvmht77lWf&V>kysz zsmM{8FS=u5N~OKcpDrq9dx}mg2HS+afr(w8!EMmr08qxB0^{$)K#a4DTnAB;YAD`6 zw=*={#ZEm#$DZp4&ZumA?A+4^8i7*w1{tWVen`WO zK}f?HF&*f{uHDvf?BALVrgm+u@vbcr&1Br-=;0^@mj|-LfZ3E)cH8A$5`65Cs^%b0 zJh{yTo}6I=VQOG0hx!o$_zJQRvLjfjFN(D|iHCLA1VDh{0+w>aLZU-p!6gD5NibPc${v2 z-QmAyytL+vAe{BgD?K-5#4BmExE0qY%NGLUG0Ixwv_h9qiGkfIDdrAYz z#g30=f~N_5O&gdil+QOTjGlK8aGmCvKF_uiK6Y8;QmKmlu6(CqzPgye49q%P!ghX_ zckuC|f4@XU&jp^?4m6FR(lyLkRH@AB8dWA|I)gvCR2l1l-4>ucXFee&RL2;qTIvF? z26j#bT8*XA0*lJkkQEw;K!Q6!{h2ZgHF%!IwwRTHUv`)@iB{H76*@UUopBCY&83mb z4$g!{#TYDNv7iR^xkreprrUGd=V?U((O)x`F^)Q8xIFBs(X#%aJi!7EG|w?b+3n%# z`^R`1SNX@k`+wD@ayUe+M65)=9f@3GLEKTyvb_*Mey=TC?Me&jWb2JW#V_`@wZ za(GFcyBQEiP<+8J8?ovcv06)n?_ffJ_*aDB~6>iuNBWR-#}5Al-^5k$S( z>EKzy?s1a$V>&8!;%;89kiK)lYy_&^s>yBkvn`(@@w^d)17NH8F{=PbOv5-BrFyS?^+i};mr<_!$h`>Rwcuelx zGnQgjpH|%==lH99`bYgX%(TYx9Se54cg-4y0a4qZiNxxhgetJgHq??er39xY!j2?q z$$cSERhH>BLozImgp!|1Nlwy|Rp3c6@w!Q1i=9eILN-*Ax-^mNQGNFi1NY?E|7iorgDIeyenCWn000Vx~e7^2T-z92V8y|#@vqN(@EEGp@F>iEI|;5BO7+b*FrGvbF%y%u5|#3wwFKFR$!kzF*_Mvl6B4TDn|aW#?G z;Rhe~hYA$;AyX*K@-sibS*9X40rje?v;>k@Z?ai7GC>e_o388`Wvx{Hh>IpZS_8QA zEO+!ox`Q0L-?Kw+$A~w59{#fWQ4uozA6nLGSk`EH`j4xbus!_or<2BQuWKlRkm*tA zQRwsG=~wNk_4}5Vk2M9*4VsJ|Je#x_5rHN|)<}_&t3cS17=_VekeMB zUq8xs+0Ul)$j5&pw4>kVe*pOhaa{Ysy&g!y0qnQmdepSO#g}MDORLkZ=p(D=JRtyu zPGfxEk~LZt5Fh9C$nbXh2(X6r=4#wEM_h6~PxHZsGh5UJYh~E!rX=IK4VCve%f=9; zAWygcGxE^EweC`uio()0(t5KlgNy%{)=KB5y~YUy;wX!LMB9Z4tf$h*xK9XUP?yA` zEIJT2(}sm|$8ZN4T>%qNFb&?bw@YBnjDzh`1=ne!laIM&kjjUVy5a;u$JkZG`kO5e zCvvV)#9dm7Ad#ISX6H>&)3UUo*@ueCxnwH0hVoV^1!bTA^_7)aqjlt>1vuznwGlijuh%X?5K@j9c$df4wZw=&Jw5nlWqvk zM;NiLBYf~#5|Ygl%^uv%YgpYU>cWcIna&qE@o~Gp^43@%oVjrLGrUx}bD#VPnfo(b z_P0`^L3Z(P+UgWdvNKZ^UHRY2Tx8+Q99Mx9tOwu05Uk#C2Ihke=IgI1xmgCwa&`vGRafA3 zoopz9hcnW)r_1c>T{v7>uYvqLzJ)lJ6fZFmDG@1=&VyHRQ{zPuz@9Fhf64iAQq#my zyn|gafVu;rWo}kU+Y>>bx?KeZ1rv#2CW#@^jSa=!a;!;WgU+NY$M>gNYZ0OLK!Mp4 zFzGw4)DfoCs>(F3=?J)kAjIqVVL^ph=1$ld$J?N z>#qbq$_H%3p5?yqGkea{H+A=1GmSIhc>ch!5Y357+jNP)wkJP0WA$OllSwiZp_Ko^ zAC~g3QAZNJ5>A_xh8|133?HLHUx2-WLJ$YctHd`k_=BnI^n`ZMw;y*Pi91 z8tuNe92(D|B-(Hw=`&XCFd)@G)a7)80U0s}g8_bpi*E{h?$exI>dV^2lsZ?aANFl| z0X3<%$z(K`48zq`YR_ZUcI@A4(O*1Pv!|2BDFL}AJ{)%uZ{dWxEk>LlhZI44vfl1p zv!K3b%N{r`i&0x%D<@+1Kjm?*R}S8&;GfK`*Ln4uY~x!-+kZPIKf7wKT@1MXe0J51 z(p6lueBXfKalt9M#Fp$Lfstp}Zg*JQ3v0xy^?9EZw@CTmdw1t2Z3=0PCi@v@!}I-e zX0w&XhC;1FR{*I0=Pybn$t>wOP2nt|nn#$mr#NHT%%qb{Cuiif6&a|L^3pmmQBq=Jb3`cY2;+!>#{3q+ja`es&jrZAEKVp zk&jTUX+I!ZR8?zP=IrZA6zKY1eLrb)J}ou$=;q2%U8FMriTC@-TrI!xyPz5y6;6qb z&%=aRsH(2kx0)YSdIW^(+TTrsP1laq9*`IY)e7BH$TU}YroN$S*Sgzuz-rfS4@h~1 zr<0!J1$Ak)1aoAy{xO1iq*dKi4O6U%##^jaJUCy4sM!|p7$|$VfAE4``o)(b#Hnz| zO3zLx7&Kb|r#`mQo?&bz=p&Nbg6q1mGNK`txZ$Rz_p9c<0!93QM918Bmn?QVKKZVy zayUx7GaD`5?_Ab=)m~pew=qomigm$3a*!7fu8lo;WvTvc!Ni!p$Fuqytd>a+3awFX zt6ImoXC!^w*o6;&HwFe&6+|s_G{$&PIVB7|apEO@#kZC-{QFYy&kV2JLq{(2g|1MW z8*6mRy3w{K`ToLz3(3EJ0UarGFv0qH$xsVVqMA`0@i0lAfMLSSkmsS74r{`-?CnEv zJ@>D(yZF9kKz@Edy@L#41(p%)M;Xh~qcO_<``LN_)Yr3fycibMpRI6dKGf9)?5kCo z?5>GdR=MRYnpg7OhLN{8urO3Oc>{78#d@AJz)e9WwIjXAC*0w z0u}R_sF!sm2?*VwFL>ZqQ~+Fh)6_Qa)c(QCBGZsrh3s~Giq{DSJ3 zu%m1|kZm_qRmjh<{&c#c~FxGP6KG*Rp93mz z^P9`>qBE5#nC1`2&NyV`Bm+_-DA7vlDz9r!w2VWWwEvzA+JB@-V>+R|vM>#eeM1P0 zt8u0`%d#{L83q_?k|hL12KGIa|;#M1??RPMty=@}^PYS#d)G4+@hCc~cEt1J$8$g9s)c?yMC`?ttz$ z2DXW1;P9+j?ozPHKS6@`v#e5Y#54OCS#e6Wu|s4?B7#<6jVpPr&V9gxaN(RT5ipU^C9tV*F>s;ISSV~ta9g@P?`0fYdlk0WY?N}ohRMGOK-JEBPu762rKG6bOB zT}~~-kv%9CA{w;b`X(4m@ra$N2#XW|U{pxha62NW`w9aL{hqNtq%NnGG%=0jL>x2T z&ul7Xc`*yu(U8zV7A=RXB2d{9+i8oloDsddZpR4h7m2 z)sATXa(k^o?CP{&%4!r&FrAksHbaDF84_IVA%D~R4#vQXOq{A z+>Z%l1U%NX-ZYLJKv6fSr-Y~%gfM7Yh*#S?-5z|{zkGW-S-VR<`PdQc5YmM*=$ zW3w%^EDUQY)oYnkW#7_Rs7&je;7WKga-bO8+jZ39tsBmI_5o2IO*O{W$e6Z^BdK*_ zfc@rUt4;220)NrQrTCr^#-gG$pxc3^J(foU4zHsfRr?THMHS6x6<>b1e)J!BY? zI{XCtag68*B*(51UUTLPX>AZUJun0PSM=yS5%|i0JMS^skQvTASx~pr^!S9DFx_)( zdd2b99V26N8?pzLXD#RN5+Gc9ErODs{wQsQytSX-5XHvQ&I^k@;3laf(l1I@h)Odn zTAzi^z~99Lz?fkOD3xAk z{=BvsLIwv*6LbnBL3H^|HCa?yEFus|<~3})@h1{liW!`V5WWb;C*}eeWgwk%iod4j z6~lf7FuRn*r!uYF`zVjhC8BP5jzlnhGDF9$an`t#l1&KOmPBP;!Xi`Kjr0E3{`yze zB4--oyE^5LIol3FC9W!$oa*^fBTbZXzIRB2j7)#1!^C*~ENzW}c}A}8KmqN=lNd6L zDs0dGS>9EfH}piPLXd*qj7zzo)nLjReIu#HsbbtH@*K!-0566DPRy9iV#)d`Vu)}g zj6+R<3iLS|1k`*A)+nKE2csaQhzS}n6c8S;2FS|q2sU$C_&W)CPjA-*sDV&oHD+5) z-Jo4yY^##OBr8$QWkCVs4$lx7ABuG~ykGhr>RG{*PjJdfj>5G>OBngzL&$~E)Kngn z{1R9JZh^W6?sG(XhYCoB#g&~KW(yAFwM!}zH0RmlPGL$|8%$&^WKtm|iK>ME zUZt1zVI$!bPv2gP+D*b1Jb$1TFpswSZwhU7Y#Z}3OG~; zOZ0Zf;1(kn(q_vIhs^<$EvLs=Vt5#M*%xMih{zX0qRkX}-1S`Uf9J%c9e?}{J)SFEv!8Y# z^CBS;tQn92ww8S>SnF^&ZFFGgu!(S1H<0+M7|lEpUR>xC;II^eCI5tMCeE%E_yK>L&^S>1$~Cd6jTjxHAd|6F(1_m z85?CS*I`xj0ILbo;Y7@go6w#@KQ$>s#c?<4HIxCG2AG<6W#?Nyz=s4Zt-3>+Alkxe^y(n)nzClL-%lmZfI{GHfpO05k(Ijc}Ni5W6^c zAnTf$zEtaFVl}k$j_Nd98hmjwY^HLkZGtC95btCFh z`{rHl*6OH{a-ol=NkQit;#qa>CF??1D15MfA1 z>~S$ME)v*`j6EdWQPWGo$KgT&!m(|&HH_bl@DuGva==B#MG@VmXKE--qg6){)msj%CTFi|b#ok46%=YXe=Vt*KTiLZO|{YFsd#?V>!@c}DS{O5BLG5{^m-D7s0sG^V^(eE`z#@s`ua0>=uYN|G%`~v0i zCfLnW0VWS!Nd&0B`R+%7o8XdSCn|$s2aW(0D&|r0%##)gFAc2{QP(ebZN9NUc8^L_ z0oMR73>OY)ci$feB8qPyeGC{Bi5gI#oFZUs-!UBt$a+ZHNAhsPneLWswVkvH(?#AL@ZX zHD68WQED9Ag}5z{Yaz|3PEk|zTO^>DfLRJ|Op@4gG*pR@40c3m{@d9gxOGwDp3X4s zSMUO@7zVS=H#(FVq5s={Lr0cH1-CjAjKP~$ zQ#J?x9AZA^W5}%Jnf#*uTEJ87c*vj2JlXCPjm1EE28QoOU0?LYe5@UwcCx9)MpqZLQ|3BkB6ZUkYp zYOC9AH1Aa1k?{F-^!+XK4So8`4KH|L@EAwujke2~#i}h0`g;5Ot9YageVJ`=^h5Lo zfx13E-%L3ILcF`jRgT$};)rh)cuE6Us*t91=q4E&5*#760P9@6#cn1pIE!VxBJW=r zdf^xPVqcL(6x}d?&cj5xgXzh?eR*_zvAE{t`PqX&oUmWSaD6XUNl zbAM6>U;@AcfOh!+9vvF(_V-R`<=69-s8ukR6w2KyQGZ=BrPt@$wFW&}w&@q7mOhg;U-Zk7-l%im#AGP(gYxg6#dgsCJH+ zd)%oXcdN<1{eEfXKEv&iVXaQ%M2=~%Hu>Q6osLJ2kElRMAT$6PNR%u}m>X+$WAj%759%tHil1zvbKQ#_xUMy&KPRTow_& zy)}sYR`_k#6^A$h;}FbKaGNjTNr%i1=Qro8sNDreP07UOO4qj5vD!nmtf?K%eQL*S z>32xjil}BeQTA*H^Q1cR5?$E}6H7tE%hBtKG@K5w*mWgot*o~l*76CHsVy`5awd-N z(LGGg>&)6|o*bB@9vRj9;<9yf&>dzX8W9bMhPpB#sq$e|N%XR$`yeZ)3v05WMtU@wcbYm~;k4ZGwsugUCGf%z1O7Pk*Y!>iuOeJn1BAFX+H zWs~#Sjco_dk`d|7BYx|){Dq}S(#!ZF06*(6V&bFx-5IX~hWr^i5X@bc?=D1zO? z>5IEO)sp=9HqG-HhMnFw0t1*e;MM>-gP)uC2|z6z2M=DIS~1&l6q9emunk}~K-&H2 z4ZH!iX)-~Bm!tF3KHVAxn1DIpW&oW4P7u2^^j`YU9lTtzYqf)MiURZsFsneV0`{pD zGXR~qEo?KGt)TW?D_z4hA;r6AZ#UlTS9@SK-k?9Lq2Zp6b|w)B%#zQZj>y`cw)NX} zxBsYmdvU$ojV|pOf7hwmy1wuSd|uetpznm};Rpbza6tci1McVb#|&m%V)QCFD@_Ri zz~_q&001ah8oRhUnL0BtF)}hTGBbX?F|r5?eST$R;$UZH7ZMZU;9#V8urrTTkQ0Z4 z#)bv}0C18LB1!-N$Rq#&$hIHY8_cVt$}x^8BhE?v0@@XnMAQl)Y6$I zoGODpM1{WDic+chYlYrPr}Z7_W0p>2t(Fqy0=`eIh*fRcWNn1Z%yP7Pdei13%TeywRF7&cs@z9lI_1*FEv)ek_tLkG>rur&(?DN0P6zML!viLVVF#-ysB)zV$Xn@S{qZZL zWNMXO?^27o|FqOn(@9s*Pqe)n&n>T_Fx$gcemGn7O}-238I>ktFz)PEO*|N^3{4D+ z4&2v~HdX!78>-#UoUT4g9=fl0%U16bLv5=%as3kt^4VWNqoD?P@S;TYR%+V;1l9F$ z8+}i@LjCV?7KEc@RBcx|a{cU-g_}&d;d?Mf<54Y5< zbw}D`)!HPf7%Fe{eTjdm?0U{eId$rithm2S>c$8&oug5$E9t!3&vl&0c&kly$5&O~ zmUmDqEr(j;K~+!5sZh#`&IrzlbX)Z+Xnf3&fXvrety8nXQ+R7}J$Q|;$%YWs;y_rN zYHX!1)BU`H836bjYKTQGQ#|jG+C~7Uo)?Qn+vmd};dxS+O--UI;6U)sLm6>5j*|FT zAqJ0IUG_%+;S$g@vC$`n)F&yB^^A6HiV44PA`%Yz6O2$kmw&)@`~)Gj<* zd}ikAKtIJbkYc9oZ)%|p6i_tgrcIcD0PJ!@&Hkm%v^tOrcQ(Y22KwsffsZcBr!S*2a9}3|SVNU&lX%O~#~;Pr zQu90XB!#6M9@k1LBku-4crNF=Pe<9NYjd{RV7gkQnz2@VdHSTI$uw+nJ}nV&$4sM3 zq3-KfZzVBmw9z*ae<{tTpYm<$mpk8F-Q3TGu(FKz#7#VSV&Bp(hYWvMBRSnV)`p$= zauQmw;d+pT33PZ8Zz6x14bO%;zHbjQxXFDn)l3^TpF>U7?I@CeaF|y^_hMnnxUs=MRtkvO7j|g+?W60wB z4O3GC_gUx04>N5#cCG1`)9j zdVg!lWig{?sCeYtu>7*J40P~2mcr5DDy?4YAC5D72%!wRFpDPu4`ce_UvtBTp7(naiM~SvXzw9#}Xt3F5EiL`AzwGUXO1HJ7oce)WtreV@imd5j zVtUz_y3>3M-SxtP0mB{oF46V5(gXVE7-_R9k7b<3>)+bYKN{Q$&e2-^fL~FW+H#z@ z<8^-6uN*qR5t#cA+T(R+xuJa42`DtsU>6+LjB={o-Q$2+6Y7HctRk?(NSlqgel?aFs$Z8;AR~u5ZwKT-@N!g5e|HSR4ztmw zY0~EMDE=PS@-%1nH4bwau#@Bhe*61vU`_t-DldmnVI~F`v0-DgdG6^D`kQpWw)f26 zQuE{Ymn*!|+|a+2)iKg4t6=}_&OQt#Xq{ltaVy>eF2G?+6wSF!64i-7mdEp(^}C(A z!sT%Uzoh=E67{1&Ca|nvtjl2Ld-4LDH!!5)!VJ(H30Pv?)#4F5Jw7CL*3 z8`768q6Ql5#Acxv`>8xn=zf2s`CC|LqudYz3M*ioYVOMYc~v^S#2MaPcl_uqetM8y zl7IDc*WX}6dRKAb3|%(!CilV$^h6dstS~bA^SBwOQ)sRD9yh#2AK_5#p#{G;Lz&H8N*z4f|M+KmSYoEZRd_7! z$tGdJj9Nt9JC3tye1l6&47yHlVOYmWyl5~UhjIz%wN8uKT zL8nTI#l!J#lCZfR&qwbKj$nBLfhES*%Jz;HX39$yD~*}Jzkfd+_4agW*#>9`rRG&l zSTtQiR@CSlhwq!Q9d&H*75vBSfIve{gs5Zt2-=LU?=VfHw1AXkG2y5u+`Hu5-kihc zlM@EVG_21dl>rJf#j9Zcj(v#m{#^5Z4rjhW2e2uhnLa#r!S5M*9Bd4}Oq+&3y76G% zko~M+(7{Nno?!F$EdOrA=YdJB6MT+>jY_MfGV%TQGHp2t4aEJq3XfzLeAvmOg7y~!m+gRf zKh@GQNXs@r1`iw+Q4+#Q*m%5no8#IaW6ITaqQrkDkp?{Mq$%ki%L~Qv90G4+BnH4i zQZC;~Va58N$ooW9mgm}_)rG6CF`NuAeBotf4h=SWK4}~dk9(W518Pv2+SgyN6|ZyD zc=Uw09VLO7`={`IxZw+)RI*;{^aVSYPG$mO(H1ZOn)u}~{A0qpQ4*d99o+HxU1&q} zYe0G#Y5PLopF;_p-u&-c%9;a&yTAY+)gFK6ttQ^&7jlXHE@!HbeIx%dGJ^tk@^8^i zwJHWQMxEWXjWGzIOdN~AK3lbZ*7M}gWFN+FiwBTP{LiKt`7zV}3Mo@QOkF%Oet zUo~q3cDGo`kgId@Pii1-t;}iCl45T3}3w=^+ zQSOzKa@gQ4sS6)ZQLOv*lY0y__&sg^v3Rc|m)u76#MIHU4IuD>M!>07%gRQwb@V$7 z5Z7ai{f`m@{%C?U)6tpP$qSYP8aS!}Aw2+XS-ayR6^n&H(8UW~ZU>*9y2aC{YCxCf z?*>}k#)E*vgBK7;#d$d(0IvcWMdbIOSA7@07DwPd&3^O^-p``_dgwf~7(VT%Zu95U z1I;`Dfxe|A-0GZ!z3g0g+G-Nqxm%WhOUj{h&|0TRDeCpbBb)4%_gJ<80t_uApvLhy z|0sv&_f=7Sn3(wgTbK`>jn>)(I($}lAmA2_PJ2g&gf7zc+0_p5@@oM#fM_B}Q(gCLang=yulC9Mik{yRtM)KU zs8T4n?(}t{|FkHRuZb{O^?Pdf7di=jrBa@;0yseFbCS3^N=7XPnzDWVwfr|~=$r7; zr1Q&LCM>z$W2i}F1-ifhGU2}@M3%d+zGKMkSywrFe8c)TY-G*2Xj#ltoV6?xE*=b%l!rF{56@N|)%5qj--v=p2};)o73nT%1wVURR#x zu52P;@4v{ehw((|2^&7|Qh5J)!zX1Y%q3W6hOVb~qdH^5eQEfZNqo1RhC9N0TD+Cy ze}Ce(6XbG=P%-f^ifXJf8!8EG>^Elj)sE;=E7f%uD$KB5B z%Tt&!5E2+M=?qeI?o7f^4G|Tf{$5;#?@!um3Sm0Qvmp1}C+mriFqD8rRW3`JvC+&I z4P=mT4&A?T@&I~?P#D^V2_7}(8J~A z^68P>fW=L`2IK7dY5M83m}4GQc>7NYPpp}gpM=|M3V(D6!%@>JTXU*B7)k(2f9OHQ zL7{rVJZZTCKLNq9e+)T*+8GRPOB}ryV16Z!zy-Ksj^PX^J~+Z>U+jt6jsD^a$`2In z?n|OhgFS=t2&^}KEGZ2Z-@H~DE8ht& z^bmI6h=qQBMGlE2)PM!`b1^OzeayUZ(;1WrLjr`rd*}{2h2Yspd*2f$`ka47aAEJ> zUb8&7k`qgFR4qjaBnuIv2*oD2gp zPJQoRULH^Ajp;DV&p)*@r#>k;WqZDLbthLjxZv8Y049fXtZW+Zlpx~+f%S!QCpi4e zcu;Z&G+oAL@}8F^&#AT@4JQ@p6X%J~`x3&eO?EgP;@Nmh`{#RCVzdbn#mm~CmnY?N zKQ}6@CV|Oxdp({~;8I6lE!&E2jxNM}L4Vy;``5RgZvqVxZ<;XBt1A{!B!i}io;tYW zdAS6c292uE`?}uiBQX=6rHLbdkIxEcen`cHC3cv2!m{DmO^dW=hv{p;Kfa&`x|msgaix(<7aPTdSb|ZBz28G3^2E8&q_>`|N6X}@R8vziFSu^g z?_bsn_8uL~3O(VW3TBN+BUN1L@01V1guTtAdTAc7@ksvlLb;SR{G9^lzocrqjfdH8 z9%Aq*ysa4=q-2`t!lxf1T#Fta%qh9@WQd3a#a(o)ZINg-oxEWGu1WWT2-NGOwTehAsh<0jq`wqL4Wm~pt?VwV<# zQJi0#k%w$?Vdz5yw7y-DW$k%3HxGS>d|~P7=_yG`K5mj5dma^BfV!he7PGQ)Iep*d zSxbvF`fqm^Sc(N7>yy!HI;1?BKA4qMfyPeoF{ zG!zE?ZzLQ3cKjc61%^c7*xb@{?ba;2e)OlB#Yboc8S(kLDF(b^lUkMCuStuE^U+@Xq==+7AXa+o$MG|CGE$3KYH{ij`pwlHG)VeSIXh_liU&N?ageW z4yLcqmtz!$2aEY$!4o?yCrqB4zvIC?4)NAP+qo0A8ZlV52B>Qzxt*VUS1eV?j7S|y zSK{I3rWo19VdKTTn|mwqjCYu%@6Q}QV8NV6)YrL9>Z~j6XXEr0{h?+F|L()f3G?B> z;S`^Xhfibz6Mjc$`~0}sQe$%|IX2*UdcGIFWZ5ma&gsc!NK z-xVb8Ei$ai3wYf~qxb9PVJfP+uU@@B+Z41%n>RxO`NC{0^78XnPsvy{1~q&x%EhpU z%5cWd@U>sO`WZTC>$kkxR;@O8OBX&PgTr*u2J-Yl*U_t|DT)&J7Bg(f?3-56t^)}W`KTPmgeHy_{265~zmsG_r{Gc#EW&v~HM$~5%8XB5eG?bp>#gLT z8gn4o0oQ`BF>r3PTJIzWqiH@QH_Rd2CPwfrI_QV{3GyA*UUqg~qtG|Rl%DMG8juK}4^*v4OkbK7F?qNdN)5j0) zK@@<=5t1J6Ur#l-1Fkms1)rs%*9-w*UxrA4t1nufSF@vYhHkk!$Lhzt?w9O0oJbnH z?lSX_`)NYYBGTZXBA_YmxCq0%T=o2qI|C*T1gDOGC_*G2QCwfIaq25rVQWuKjhG-t zvUz``{f!Gjm<<3#T7R=_*gskmBs%z^=_bORkGSak8?$A4l$DA>WnP!@eo4W$!Bfq+gPaTT`aHY z4WWsxz;d$?(C{|Yr0M#Pk?pcz`8!9eKNSPs_jce*aKfKZqH@iv?fI9pt}^#m{eE%T=GZe%TfxR$y{9 zxERKe!`|OM6aOvStS(RgxXieZO_vX0GjShF8i|Wefn>L#?IRo1hY5Wjz0Y2c>~nRCH6J z7K(U4$l35L)Z~YSTyhH|;c>WJ$ER)GFOaYixAF>tDO5(WS<#p}4jZl*`$f<-Wq8!W zWRdBiYrJv++tyCQUY*=wM{F!PQBRw=p`oEyO!ym~sHiAR>4*d0GhEy`ZSV8X`UnBcT}PjZG7AH|Nwyn|h3RFMy|2fP%a{ZFO54cDZ!GS6k^R^%P+Y8% zR2Ag8H{ZMqlK_Aa(_;ZHDsCj-Yo-|73wzz31jrp>6K_!@0bqM-XS&Q#sWXncSI$zyy7r-uS zO2L>|zzMmkWY&t7wtjglsLh_BCedOT_XiEbud&|z3ZLhf+pWxL2@!q|N#<|u; z+doosBUt+E>`+8)3bLZb+zolaIZtcW9fX=1q^Em@rB?s|sOaQa8a4*ZhF_52;Ep(a zJ_JiH!JQQm9mUF7MP$V6IT<*bmb6;G+2=idDts=!G5>@2TyGn~Z6~(V{c5zSB#Fsz z!@|UGdLm~Vo;cY4Z6kmUMViBDiq?u?Ca{tmBJ zIgvDg-`g6A+{)|5stPfz2!VyPAuQ}uP?ILVGkUqu5X%H-X&e4__k)W->@EtX@lGr< z59{QqC)XbRE6l|F{=XF7phl*&k=eTK-6~bpBVE7eOMbe!8CII9mkU@D&S9T5#~n2vHpi|HGBksuX`j_iCHR~Dp95lj#`R)$3x>BQXAr*}|u@mEl=^q}{{HvPYQ5ABWiz@*U3gkTd|^f2=IC z8duL!X+_c@SoRxIz%~XgXXkNCcw=%X;7LSHSv#yhQpEYCeSOcNh+6(&)I~lcO-TZ` z?LTD*{az_UBFG+F3%z_YZSh+#Tii|2%Na_lxwjC?i+DRDULav1|e+ z2oNa*VG?UOSX-Y@bsn()uaGngEe-3>Y)Iw8L#m4ve@<6@aVqz9ndL|kh;dLP!w>aCq}GRsP(g>Hjn@aKEAo(M zA0T)kKYLCU>ZeHhV1igPxqkVF+n)avJWBC5{n_-G$4KQ5CYAhE(L*G=ID6pf2StT} zf(#ss$A@Kb6QO&r=jr`th5n^)eYd&Y{b2WZi^2wwr~SR@Jrmg!0{{L{l>jFB!3YWQ zv#M>ITxKmKJD`yJjKyDr_o3R3OxVAeHu$5VmwoRz8N~VJ z<(-qE%|jddVbwGFW&D>Rha!MFVdtF=D+a16fjuGGcA7m5^wJ~G6`%4#1@wrhvpB9< zdR(L;UYMhle(*%YJbFLPXo$hPg>rv*r9mEqjPz)josAdkOw#k{EB&RbZRyP(28j$W z1bYpbU&aV9zqb*kS2W>qW@&Pqhk48^X|u72WjrVEj7o!P1I5GNWTTgz`?+4B*CYS+ zRC291Y2WuGM{3Ty8(Ia0-*y&pI>fASmqbbPGmffscm))}=r8i;s|1+GKq>aEvio0e z8Mrbe>XuZcmd3`(cYVca96uL(hUk2_47H#n3l__S3@-d8QTX1pltmWR6askwc-08% zU(-9h!6quX0Z!6a^K_*4E6trkS*2&@+|O&YlfJa?9hI0TR(yZ_{R1f#*=F>0e}spQ zF`#4m*QF)w9S{gF?vX3V-TNtfU@y4mP@^qZme(Bf^XaCfWrfbge zxyI4rvuA%xwN888fTNmmwcj9?TVmM`^8RZ;8Jwgqf)Fl=H9%24BC&juqyq5;^TztK z&!EFEvKi`Q8ev*u_8HI5k9nrs(xn79JaC{XT+8lG|DQVN>uqTHk86h?MycW4d5gsB zX6qmJf}V#)ija?d+wHLHwfN;qnLUMOQwvavn8L8INJNuOYCS*m7OT&U^ag zgHCDRN(X%QkHzVI#obfQ5g0ocr9XT2?0>O2u4=Ejx*EV!*aZ^b7D%#=a`#tjUrO;T z`T0FGt1JDaKNkK@(u81YKJgvBGePz8L$&wu#e@-`#U*0mpFfKm&DY8G`@=vSUrEf+GNBcJg7bp4xovw{R(o7L|jlrzT-HPs!gOVqz^ ziTVmiGfz6NdjX(O!wmywLH>QlUEc~0v-KT6Arq1Xfb4*Av8q3%$Nd(S`UMg&W3B|P zlE-W7>lbV5Yr0946bOo>#|Vzc58OZ{5mh6_73DunSYrATjE#-^LS-#m47hs9BRdUG zAtbyR>GWDJ=%XV}t8joDsw3a@xHxT22?=;&{!1-GBa^5A>Az5})o>`heh;2`gtKVw5N@_TKAyy_ZQkM>o0v!zf54Fk zwzI2`(J@*r-$PqTqZ!iEYnr6yviyAfr1wux8@G+?dTptwl8P>Zc!1%UTdCTB?e`M@ zOgk@-sQC2MZ*-7Pr9W~5t2Qj}%Fut?KL(YRmvb=%O1BtQP_Claefhgtf zQ0SLvohu11I8JR(RCvq{RUv6G`zEDBXgsJ+Kl)^>qk~6dISUe)c_q2p8L4wSI;!=Z zKQrw-eWTHY{if1=%y0HNSMUL7X}7|J6qK=JYK!W zxpW#YX6#vd2ZgT(D;8*-rNQ^U=Am*()k#QrKYY03J-W+Ax;{|Isb3YCJL%AG6<9{e zJfL@Ue)Y@yI=Pj&FCx;L^$Lb$)}CjzgIG6dF2~LCM&7wGrGsH|)z@{~{DPGx&EnF6f_+JlA#;8g(xxR-PlF2yvJvMg4 zgGsE3kPf!otzKF30C{i7eZ(Vvf~*JEZ3N2fx>h|;5M7==P-X&v`QdNmxqkcQqP*Gb zmPl^rX9>!vK?U<<|FzzUw=66y3Nn!+0@*>DS@Je_) z++ImGAmL+ zH9bAOI<_kyLPdqPmumDj?l-AXN4(bWr#YPdf#3e&&h}m|>QK5nI=pK9CJ>~7kYmC- z-n(S?NRTxQ+F2Oll2IML>xpSaincKlM=Pa1M9=0 zn21?IqUf-Y+t^#DYWHkp@jdQyKR>^@Jk(taHZd_V;TU>}0~Kdy6DliaxkDL#!iSmC zkA#m#*Ep@ky|2qd>BaYXeMcTQ!3KKs{sP|~mXqQFZ6XB+Lj%h-Y;u-7J(3JwkRY4Y z&6sJ}Z;jV1&z}q592D73Nq9N87OJ4tvy6Kq?V&{d+>cqG4qf>ij24U}?w^}b_WKdO ziOKl0+dJX(=;CPl$__T&8!uo=)czXTafI^1PJ*ow0z`bi47n{au;q4 zOt7TEcZiD3UFQ~G;dX(-KTyHah$^fvAs&0EsO_}GOucYyt9158)5}ylj_+GugWy-# zYnDL@^5NtxOwe(v)Q$UD{!HoNP19|rVaqM7B`6L6+6Gq5^eX}n|3Gz z0#aq)&M_4h`~9GFU87IWWOE9BCYr--e5H!O&wi5J>~*#)13RB@_D6x>(4W`%q!Q00 z10k&%RhdV!a>iHki0*I68Gr&2DP8k1nu2P=u`UD>!vjV&p^DTT9E#ap2z@U4XU^C6 z*W+LQ$3`!1v++6NYVJ1EFz8ReO0W_DSg019Ez8}@Fs#1(^b;gRtM1d$LxF5vm3c<# z$Gc+AZ;yA1k@=%NdbB!hDThb8cJOzK!>5S8|Tkg(tCm9({`9{0TW;^HWN z4C4v4Sir`&n#!v_h57k;e#|U<_94Z|x|4L8A;Sw#n}PBe`{a&X+kO#ebncKwAu5No z*$-mAxU-Y2^F8Zw!SbAa*B2MC;9R>d5(bGT-wBoFxo|n!E81HK9{vuAENSrChAi|? z8S=+HkH*+VoOR+{B`6ovNJAv8bdS%E9@Q<_UjLGXN%|fC!2IfzK{1KBZ~PFibIiy_ zS+k)Kkj%i_n`_1@bNh=C@W^XrWo3*00b?d1m(f4^Nbc|$dh>y%8D)f={>1ea7!Iar z%+Hm%9e|dP1ijL-ZL&KlAOwI!)r3F45+EPhePZ{Cil!EpUT;N)WCfY|dLYM@GEzF- z9L|8NH0<%YAactH=81eT(v_ZI4hOf7?4l4n{A(0RUO$}?LIE4!4*K*f#rsZm(_$E6 z{#3rwT!ctx@fJ*cOHLj?sHB-YwQzAUl?*U22<2(L8B4P?O!!m7&0OI3aH<8d z7ovcjZ@fKcCN1s*o$PU}$07y(D*!dGS$_T84^ZE$FY?Rkh|Y9ATpOQ1J-nl4g~HVQ z0UQ6NeoW5!b8(UGiYi^(!-{@y5U;83-6KC6v(KeG-5Cjd9)-3C+ls&2_|4lv1h(Fq zujq@ZtgJj}ldj^Q@HRJT z>Vl09RQh2(aS{jwLMk7Xr_oPyv}E~F#-RChYUNX>y+VrUR}u!XM)2~V>Wr`FO#&(X zOY-u6Osu7gnBO&+%zVzV%zHr}tuWPnvC@B7`59(*hQ=Icp1>(e1Fc8=Ia>g{u)0u{7+tB;RQ$JR>q&5PQr zLyGNSZ2;gh1z9r7WNxZHMSwF|tm^su)_%Oj;M3`rQp4`)Lole)%sq4ND0 z>awXDT4~$feCeQ;mJyi$aY=BibR3gZ&}d@AZ4xAshlF0dX*qIeU7n9HndYLQrl#ha z*Q`;fYk0`oVcu;1AVa+VR@pq}tQ(qsN=w)d0MK*}mOKs&o<)&p*;?g-&&5g#lL*BZ zc`=xOn*jX_AD^R6m89;`U8s$F!(6l>yMOPhQs4mS`c94}8v!BR3+t6GHGdF%Lsp{g zB0Wm_HiKV}UnXF6gEm3AbL(ubqP%?dX;^sFx_g74al=K7rP#GFr4|5Kh@C`!Rff@p zG$&sx!FQ*<-u31axPagx=hlO4)|w*3u6XMwbkVA50u7?Ixk4DmK!Rd@Z}WHUVR^fA z&9fgNkvxOxVwaYRW6xW~U%Y7I8~5m7eRdqMzfZPJ6|g&7sGp8BSklAKI1sfBdVoi~ zBYLXzYph!fMCRlwW=m-;>A5FLD%7zeC;_4u_q1tXW}Ho)L!sgtA)AtK7GnCe z?QMkryDJd|0MLvrFOo9etUF^58Og-DhpLP`y<}v#DDnNg*LqK4o~kPZDI;)i^p%vp ztfYF;sSjNED16S#vlhTG20o1JG{n6;yY}2ZWk_>?Z!GVk{87em$C;lG&69Y|Tg0~~ z%BF;muYYuPIH!qOO@HGL`vFfY@=Kv8-ow8K04rlQXhM)Ea9J|pcvH`SfCrtN(+5ju znJXgj``^r6hs&KZynx0-@S8H^pSYRVg0E*Zq*F&ay1JexL1LtZ9LL`sVG!QoGCuoI zDGVjUI}%38i@x+3rirO;h+;4YJG)!-XUXf77RH!FkqToMk@3=9NiXEKQJ9Yf0Pyj6 zeeZ~9NRRLhPk<0Yjm^N7XF2WNK@8*~?_OXi`A9c6X@1Vu?@j*>h{{FX+3PAdhl2X{ zSPNA%$BnWGSwk#?LXC&{c&HMqYkN2Z^g z_z3-IY;4(>sJ&E`lakg*_#P#dhy(P0h$t5H*nWpHWgO+fIYszkki&SyxZYDGy4!a# zlop~TI$QNO@PY1Re_jy$<;=5Zl1@R3i{A$2wVi18Jl|3b^n9DoDB`()*U4#{TuutdW2mMmhmjrtzN#c7 z(tNh0IsR1ZzxUqDqxFwaME6RMLpLD7Zyzh_GEX~bQt=XTuNU1rQTDXCNpctdzy(B^ zGzh;BI_=Al@DAg1y*mspDbCH6P5Q0(Ird$jGDGOeqAfxaY?2J4kK{T_Yn7arfgtr3Wir zo)y1`rW}OQKZeI}jd>@12Za$cT)pR~%vmitB6KFYeg*&*^ra?BEJ$Jtvv}pTG%CB5 zuIOEL#J%af?CdtD*=Po1*a9pUb*IsiCugC>;Kx) z<^Cyzij3M$O--fMDo{??h8c%2obE_Kn~N2A7>y+5;gu&5}tmg>xy zHxX+;B=LYYRfl|*A9Z*{%ROc8CZgrgP@(i@w*;&@jPk()41I z+BA%O4r=N1@?~>#SAcq^5l{R2J^t-{pE{NREocuaXpbCmQ$MWz&$&l%y~&SO;@o-6?SV28RxM9e+kkimLTn2R=n|ZZQFL-8`FA`i zAimV#yz&LKFN#hy{CHz>?UCJkdL;Kt^VJf;NE-nf%99$tp)6L>uTlPdTx`%A9whh0 zFhW0u#*LK8tEaE0L={o+WOpHWn4g`!p*bsSuR%tJGVtct+|`^UZ>niyv3Aj5ft^}f zdBIFJL!z!w783v*E9G^i*z*1O_3IA&$0cc)sPjWu0)%ilAuDUo|Gal%2__K|y~3)_ zb)Tphp47es4a$br*Yhy)+wAA-I!%T?N}ONt|1(`}TPvM95^@f)w9M(=6tpkWRxjmy z|5B6B`FA#aKHP=tMJ*9vp&-Oz5>~-0A|evoo6yXpe_BazToP;ClvdWKEg>=M`CT!- zBJi#5udC!(@{~N)=N=q9JdnVXEw7hGehL;{WnUY4&GIL8OLXj-1ANm z>V2pM001}bV`D2IO^ddJi<2#eQmukRU6SxT->DdTJhV*fK-2GwquG9zk~1brT6#Jv z(fywv!(FBu!_kOml7UDaj)cav_EUaM{(JZCnTc9i^;^AOg;&?s4wY`3U&>CD>RL5P zO6~di`p#EZSN8)PV~%AXpN;^1{i(Z{HYNo`L9XL?G2?E`@IkoJ|-&PWRDu>&6OyilBxXExxNw^xRxPYq1Bq zrP=MNUA9(~?T)kf&X9sNK0*$j9P#BtxS97BaOPIJ=T1NgzD#K{Jb)^$xc%s8A%^k zb82_+-iXWlXsvKb;-U|~52}wHLaUsd{6KbdI1m5lKUXlmc%KC9b2XTOXm)_>e9|#ml||&_`g!ivC;QfZfpllV+69gZaVcx> z0(^v$JoDFFAKThO#;z|#$ZbJ#G4LNSss2sjuZ>MK5A zZ*X?@P2Z<-(pl9_0p~SfEq8Yb8S@1PB=?1Y!^_qK0s?}V$cX%M2FaPi)T+H^h>P)1 z5$Zr+lBD{{3t}9=g7F1);zG})Ub^5jF6WE1zNhR21fcrfzP|0#DmoDcP7V&6%Sb+T z=eg^g3gb|o9ELkxUgw7s1Tebe94!|3YIWY5FkgTFZt5dnDM?8ynNBoI+E#gtjB|?}Dy(>CXQveA@9B2?GtZ1V zky`~j3fkY_?+BCd>(OI&d3@aaEbjY)^(Q)YIG4+vH8;kXF07F%Uj@ z(1gKY#vI@qDtTeV^>Yn@_J6R-*D#ph+qBWsXmfY>CIr$gbMT^ml2%Px;*1;@2x+Y) ze-IC&Q+K&V!_b=REVAZaJc7U%^X<(af;<8SjBdPj@Y&a+X6 z31yX!A3xT~MQM}u*T3#zXRk=9a)KEje=&4gKFyGj%X%j1q>kvE^V}H*XCx&}aU&UA z+n+wcq5Oyg@I9h8u70KOZc8RihaGC=?bz)?G41~uvDVA1_CMV*R6!D3(2KgFXkQ_6 za$)Tb>z@v+8;^V>;{vruIy=v2Iu5My9q`xL)@$sLQWS327XwP`B3y!DBm{H~?+!-& z*P(ImOZJbCeUNo3Xbq5e6Jdg}u4rS@OXzsTo0BboPBRr|Cr;dTmMnDnfgEbO)A+NP2GkmPK-}hcsL`{TXJqZZN4-8p@UA z#FvN*^d<&?-f(N)!93JU&qQa2MdD?aVYqtN*m3FiLTk2&bgjr)yVX;Sg~#GPxMhz8j{ed+NFAp zc)fsxJunT}1Mjr`lfj{;h1e{aNVo*e4L0b>&r6B#5i#ZQwU6c+x#Bzm0`rx!;EDX)EE zqRu`qL`KnBMYjuh)CX*QgT7Tsym{Q-b<0!uYhZx8z&-}dG`7n|$k^S(n6!ffBojm< z`c#m_LShnPg=t$6skT4oRA%Zt&@BFEd*%v=f+j7qe34uf5cV{#XD5rL=E&J7%S>v$ z#?494uI_5G!pKF6ieaF!qH&o~dPg8T*c?fNx3s%^3N@Y@fjr0Nd%pV!_=Hb~;^v~S;@m#JrppNP%swX3P9l;voZl&f9e2o@)Q`eZK! zyA3piFX{-JFZipa3eCSnPnbeTm%l`e#QHDu}#tA3erOgRi(io9a7B(-jBN5HPIW4y0IHh|tLT zlB$F#kW5w~H*ZmO>f%zDN8CH!bGwQI!{|72?T5hT!{FgiO*XEL>6~_6XOq4~w}1d9 zQMhI;ed6tODxdRlj0|^c|DSs!FAGR~#pc$&?Us`p7E$K8+0N zBR3_bFHbRvho6IEhfBQ5hC5*rM4>ZuKdi$~TWc^;Y7s*1?Rdd$=%WIdT<=HwHQh~i z%zXeDX6WuxA6UQiT56?dV2IoPGu6;vADA>YDtHGZ8ghdrf+;B}6As?-`b8uRiNoMw z{PiHG_0fsq;##IrFnf&s5IG%4h_-fCcB`rpvUv=eyS{8TYxcvuL3ECX3OO}a=~bjI z!6bSkyDLlx{+1O^CW4cbvM;2DfaJm4P`#ztiZ0&nf}q?`Y^HZUO4F>evXb}T17Z#@ z1_?8wW(W)-PurzS>aiiAp{b(9BETG!6`LfZ27kcDH~v1oEAoMQ&i7w?e*RqYx(y0^ zKDJwYKQkd|)^vSTN_HGOsrM3J&#clNNt`W%gL86bgh92cR_Gmp;PDe>`YA6ER$;MO ztZ?k+YP6#BDu`EEJ3BjO6DL(3=Qkh2864p8Z?r~_PS+*51hI0k{rGU71%zEBQIC({LMe2oBRSFn#T-DM9UKUeGjM7`F85!CskjN6yWBPz{GgKo!$Xn-e%z60fJ-up(L45%OE&x#2>&`rlgy3%I zU;BA!KtTp}O@sYJZBS?FWK7$rS>Z%J>Mf&iseUVFXD3-6A-AwKb+o`s{zYHVRe1!= zUPi@ULhK+){jsjj>tLAqBWtBmGmq@0DG7#A+tSG?NzlQf^#CT}d0=Nye^~j9C58z* zerdrErih_}(Y|1_Wsa6SWgd~myW=s@y-Z&-s%x0)=dn$Ys%un$V|K#>_gK`8U zoM)b>PtrBUM2vYCW{v&yz(=@DIu`|8=r*{orQLa-t;v%#i}j$YE(cL+<>tm4)@dOw!ALV(o2CbqMs>XuNbauTD%UDfMyY65*6*wS!{8ilHoirj zkJ)VuFbU6-Zrf?$KrPtqVbRu;U6sJQrw?&-= zW<{NQWuK9wi5Z0~h%h99e^&ZyTFuK(%}v?i0Pr%@@KI&fnJblHGy{pBc_EoN)tdT+k2%<9=8~+MlbZL7v$`MWJNs0H-!a zHQu1pL_p3WWh1v}ft}K~$Y96Tg4}d$js85UFi}O|5Vg#qUQRUe+&w z;M9*iXfB4Kukt|N8)IpfDKkzf+e^&)&C1pT*Q5ZjZ*J1cQEui!K0nymxkgaS{Y9(9 zrZb|WKb!0OV;zX6r(zE3PGe7BoQ`&hjx3VFw$oXAWn&{*712H%5Li~TMf`rw1k${e z%S%jm*QJ?4Gq~gWl2m*Y%%3RT>>s$jxM-m#;jw>nOD7xvqFuJer8>q%dezHrM$D&^&3b>Lh zx(|<7q!a%LGdmB5j0|P6NOQ5XyXB*-9O@h=e@xcA{`~;SaB;d|*1`?n zgorH)ms^u@Mj3IODen$V3vsDDBX>m-XCrpzO!O+8cQ-ddWbFd&W3wyl#$mP!>gwuh zD)fn1RDXGbk@B9}Z7y?wQTw_OV{iC75sGmXv%)_G;LjaOPc)w!`7r<#-{+LD7oO@Iu`+vR^yE!F zabM?a1F~77$B!v}hQVwx%HT>32T|(Cvb1e&B=H(EyUi3G?}dnv^M3Jbm7pfpq&|!T zd?U@g&e0nSvE&i$nm7J-4LP?svOl(~pNIeeV&Czcauf`-ySrOVtwm~q_`%x(xUWM) z%Z-vtv{*zV8;B6GX-<-uutPR3w*7q6%edNFYptqG6cytOpXu^2WyTnN$pC|CMLQQ_ zBzM^1ZM#6X!neK(EyF@(HOqn0Z~%Du+3VT}1fy$%rl+UJf<)uPbsT;DBUAH!{dzJ< z)Yl&g@h}cOGo3S;@uF-*I+J6lkq{rhZEkruOt?Kua%$gu~9}If<&p~G}+?cHJH#CpZ-j&-iV@- zvl{$W?>Yrp*%W*$2!d}oPJer~B1RuQv(S18w@g{!T5LKscPWVnuyp4$@zP{j&{qgP zb>ivk>QN-J(V#@9#>!^|f*b||D0Z72-!VBZO7s|*PvpCziGhJZTePKN<-;*B-#yx> zu*(g%09GW!#Y4KYcThOon+QW9cXLeU2-V&Ozny zawI52t?(C~+(FXQF$!-OQuaPh#>Tb8xa_#{@@6X-| z?aq@@o6|_AHl`nX&VH8V)ww93k)4}+Z4Astsv=q2zh+pwPJE{>)nI4p6IMi9VSUn) zi?U?vpE=AxoPx8&tc|Qm!t-)ad9JsDTt{@1^X+8Qd*iF~}j1yf#LZr~O6 zoBXyPG``b-`bj_ZBC#_nIZYcYoE4xfiMpf1gLPZYP}hTlgI{3A$D8Kiwy$Oj&+6LU z{jIwJ;N=WKV>meXuf2tx0plE#O`pSqix?6OJ-+aMbV-WZ{#c{anQC5}Kryv^TENZ5 zwlD7aXX6f%p(|ID&Dk3>g9yABXXkc+Z;Z8O8Saj{i=GG3U_aG)WV^PQyMAw6QWb&w z_06SKoUnZ&_X$_5*iy~L08JR=oz27tn~Q7?-5NWC^_q=d>cAQA#o@%*Sn@YWhOYQ; z`{m~o+oE=``=C=o3}xW?l;w>0GYi6S`_@<5Ms56%hXByTBm+C(L=sz=&=ULa;{G2~ zR~;2q_jU)7Pze*HUlBpNyQD+umac)JbLdcz6i|^;X$k2VatM(UX6P80VL%!gX6T{e zTf_VQ*82Eo)?%G|@44rkd*a#q*?XD_4}`847mZTEDq2|5^X#fhV%qMCwb8K1Mbs6~ z*-%&4_L6VtUhW7#5U2DVtw+<_o9^GjU8v6G{~fJsEOsMY5KC$=>2>_Oub@DY7Re+c z8+crbt~3u|7x7>`XTZ`}wuhPG5aG|VWUH(1nFD^BR@C6BzIwNp5B{rE1z6Uv-?&uf z&UwVwEk>-E|3S37&XCK$7;WRG5BeyJjj6kTJ6Z+8@P2V@x&MN|3QGaUa!LdqZ`_Z# zLiYFC36?jtgkoyo`{kiI7$cP`O`V2jk_P}dbBZWODOZf^yeBESbdx4wFqPRm^@qqtkhM4>94tEB=vs8RgWDZ7%8dCrsll51`I&!atYas8DK4YA*CBV&cSr&Z7 zXkFt_*YI0d($VZl{?Fqc&&vx~e-L9y?LVor899r8j@5n6^BryDOhD!aGFC#yaRSed zaA*uO1H#Kx*XF_!V$QXR?%w;U4Q=S&ldS5*rLq$$@1#=G}QLKrCin%UWv zaGO+96{2ku@L2ig0^_00k78nCErghVb~RT6|L^dnyC@cs@4wr0CIA~zUu2s$%*^)5 zLtJ37O7OG$ZVDyES&YmhtX34i7ScY~IGHx~)wt1YZMh4B#6L$F85=itW@M~0A)oZ^ zND0Go!Ae0hsM#zmI2Qg@1bVcEQa7#hX{&uiL(|M86Y9_%XRAau?(riofC`4Bbr4d> z09HGc3@?4U5qZSvICjiwGbE+3>SW*7DfX-4E&9n9F!AV<)U=v4A-9jCs)|q|KPOMu zkuo9E%mXPtObmXueEc-Hd;u5v`e7mMEcPqoOS7v7}0YyQ~e42PbzPfC6f| zaI$BJ@RQzezKZ!Dp20{j7u&n*v`p8l_xd9$nbKGLHi&F^8$QW)3-ynMA{1(UgchtB zI?cS85wnYQSSo>zxj91EYW0ontc--D)OW)10+E$fA%!YS%mDrMCp@;W(Ec`-G|b?K z;k0zX_H4nJGyLq5U)GjJ-kwI`4ugyh8#a9}aJvS6FVX4_Qe@zhb@LYR{Xc+dE~9b2 z=fYKqbh&0=?}BU_0kNo7s{%B@Z)I(@*2T2(5ehiw0YJC3w)RYTJWrXN%pf9smWLR` z;Y&?T)5GDKuV1;#FL}~>e@r;hIy$tKX+Wd2GF3+DNyKOL{)Nm3%U;3yT&(j{K(VX& zG08+W!dmx2rY)8v>NmG%d=^_J#lD25?f%i$=26rpvQ?P}era!16=D+`5$gy?hP6TWCequy13 zv9o6=+T5WZL+K+Eb(FBD!J#?w7i8F)3IRWAfGg;`5b~B|U*(p9beBy%j;Y)*;mWvKv1ZcR}@o+ zoE$BvsZA#<#xZ87R4%rmd(T0s4^We(I+$adfVbg9zpnr|}HFj4)Fw%wCqm`yn<;&c(+<a?RU?_I0pbIgi?42@F5W&?1A%qxmW{h%vc%FxhBVMRQr$o4zk+ceb{+d?gu^ zW58q^u}vlGQRdMHg=jrvjgi!-hqvElj;y}~H9h_n6`Mjj?(v-8lb^qS_AGcE*N)0M zTLJ}SMiE#9DmhZ^5T_KJFsP~RWmc?X@(2FfzlcSZ-NgrMA)2>ekd;~BDPW9z8D3~~ z{APn0T2;BiLWUecarA*E1sf6xCLY}$i_v!EwYAj2V}yEmZhwU^94zjomDk@Vs9E3yIP{zfIS8=1!I=|V3>b#`9ApZd zc|~bn$dn!-Vn_k_TKg9+6lKzxK1&HZSFLE#uYpS#gA6PHNK;TE9m!ju~aaOAJV+HPuSv zJK(K3s6W0Ui^zhn=`(nwlM_+Hv+=m@GsY+=hwI{*>--|A08LV~C6v{a4DocYejVhr zk>;;{7kjh4o(_4!*^fl>@N-1hX}r>OQG({n%*|Qkj(GqsYU}G^US3|I&pxiy0~y{m z&zxmHjDm0LZ@D5owTsZYRz_SQUwbbH32Om+25e}1T=c<*^c-W3O^bszzHF&*1?cL+ zXIE3;48^u@iSyqtYcX*|Ho`{Qa@c5}#n_wU+N076{dbYKf;mo|dlZqBFHDy9*DG4L zsy}^2Jw;OHS$D`8+1W482=j11yp4h$*{O-UA{}_KG+}x_4BO0`whv0sdRB5B9TiZa z#axg1HqpP{p)%!FJGNg9N`~R&x5?+Qi+Y^NV3>Sbh$)Q@_1Smlzpbv(rIcgDKVNS` zWRSO3?a=I|XFA+=e7P`VI}ST0EVlN(Ew7u#~c&(F~$giYSt!Q<_w zWUF?|B=78Srw^H6;b6!6*P(4~XDQ*;#ikcK9hQ6|() z5KHB2^7=IwvL$2$uOoL_^X3QdT{E0*mig3vYo321&q*)LBZmBj4L2nx8-3PWGl~?j zW|o&FW4u2WseG~`h<(_87wn<{v9=-fo%64M>gTy1&hz6gFY+M_gHa>vv@OleGWlqt zQ=_43dJJg}ffT22V4zfH_`@d~oG7Q9op>1yRw5}aH`-4sX39^zAfs{zPRK305jMw< z!!DN)SX4un8g#XKP?kryM_A2cnhKWtSDG{uYsfD|o}v=+_>NSQK(1|&4F~d8aPJc_ zWf9O%GKh3!RY8FWVQJy1^T7t2AL3X}y^6!u2qyfd*`6gmfP&V(a z9y*A0WX{r(9pd}c<8e8u9xb7i1=*xYFmfGh!xOygtQfJu5i#G!(pz7F_QB zp#{3Gqfl(OUOWfYdrk`om*Ii}z|Hu}2gRG&v-OwRc}l`|tD72=vs*a>%1?u@#f>Q1 z?}y3R@?lByxYwq!8ZHfAj65~Z3=ahpUs^3jOjTK8cu+Bp*gTN`*70uI1CI9?b!brm zpLNTrhH78=Ylt*gfp#|X9)y8wb9Au~bLY+-(;(Q7>}<>F=GwxTs&hw5ZqHRakNoD) zlg+KsQ5yL%4_iZLQ(zo0iQWX_Z2vaq_rWajPnWV1PRN77Bs!5qW4FBQd}40a%Rn|P zdHa(1?eran>3J0>@y2+EY32)Pep{K*e#;9&9J5m!Yp<64X&V&q89{)>Wx^R35DT5d z_mI4+z2DI9C4x?UuVI4S1lMc)rjV7nzyGM^p06(eo1EbCh`zO@HFsfFs=?JdIbtt5 zjYBMjSzfRy{GyeN0MCG$3q?IDZC-xfqFP!-zUTeoC|g@w%y$;|(``!Fy# zc#aSt$-nRn85x+BqoJUCOHs_9e!my00p=|x@U8XFt= z^3i%$x8MEpIO-sVb8S{w2u}9(sg5^lZLqGPWLpC+^rd;~_w)mSIvC3kyyw@`F6B*= zdc#0d;LLEgOnu;beyUs8N6I|UvXu-%W1F-K-2=PpIDB+#dma07DWeikv3C?!81}+hRJ109lzeoXHeaj(a()ITC@}U~H zfmH%^1!(Q8OxaZY9-@jAQ@>?U}YKAk6GUF!^oqfi(iAxm=` z4Mk+@r)z6!YBC-%NkANbH49ogi*Cs9+(C+2fh;>OWmcoez^CLlY@}B!`zsjI>8Kbw zp6qd%#1%s#!GmYs5AAf3f-NT#(HmBx3%Q3q#L1F3z~Wk&Vv zuEq{YauK`b1YY_$?m7RP7rGnE2Jh02`V?jbP1;4A?*=aIt#Vg{8t_|x+HSF3UYnRG z)}~tB4POv{BIG*e=!EJdh1dRj@ix}R!a^jK8s5Wwn3HK-kjK`BM==-% zYuUK7@GVj9iU><(X=-jxEABFBBaBVnQ`gd>jF*cDXT?%k`eDXT)YgtJ7cS*ZOc2Lw zPoA7rTeM64nz1~syZYIc>xw9&byq(ZwF=cJx9zvqr(relx!<1BPNC5b)6^FS zyMX|=;;>Yb-f?bb(K^a!M{{9;YW7jGjSfIANA60T_;z$q4&KY z|64+O-$Wwq4=qw^dV#)4zD@_L3ir8#vw?#Hibjv#(pwazow4AXk98*4lLDBo`7TtJ zJw0>uA98aeyni*7{?grzERtEFT+zy zg(u9;hAATDO0A0S8HE!qN+Bn-e+ZyF*&lGT@UznGKO4I{k;&~QE%aqQ-O^Jy&7 z5)w6@sX?VRG($?ps^4S|Z<$v|A3vzyJgY`*&?wf0y$^|D@S+xgSF8AlATVm#Z zbZ-u4ON9@t`lR&*S^>KXh{>q=r>Ni_eSvcJ)XiofV3x2ig>yMY8FX6rY7H3>HXoiV1{%$F%}9&%Vw{z z2%}&woF!=@yS#4_xtSb~@Nv@SeMAX|`q&XhW9-~IDI`6s`^hg4rugn+mU$jIV3qPk z>$6>#6Xgk94IA8X7H(DBb1N8EEkpb|kPk4ZYf({X>Z{3;(bE^#c5 zyk`^Y{yLUVG}bn&i{cf0^#_uuF(N}do8~5&_{?bs>^1jhZ{j44T1xKm$=?a5CozGc zuW#N^P)Pg$EGP8r$(@W2O%07(-_iMr#ai?%2Y7rk=g0_X8Zz)Dk+%Ak?Q+$fh}Q6@ zK(A%hE|lw38R&C^(18?>Z?i=a`e*{Ys&gJ%A;&@+>HaXoV?ukBfhy$)sK7h+-d0&E zg81WQ-O*6^c#OTuO?UJA&|F|A-SiNh+Hd|w%k#0vNTzg6E?ytG;H{oHVZE^la|rqD zm=vT}NKLKZK}n9oS4>idb#MJ;M)i9;e3IbF)klny9aTZcrCJvMpBM zDuSg6v!&rhwTAT<@1$~cdZLOCH&weRBM|-JU2rT7HM0z3=h7aFY0#+nIk6@$fY0oJ+0;sIJ4BBbMnE)^@O$S*(4AaljX}DMP!*AJM+dE+`ld zFO@Bxs?fUtM&;!xy?JwZBq1TuHj40FSz9|($(6r)wx8T=!Phi_SRS7))l(|ugWM=C z@-3m-M^x%ay@Dz&)YbcPu}B04?sh4u(HQO@CQ*`?(&8`}-!2pkQ>^=(=Q+$?fY{7# ze4i?XMgA{d;!=Zq4f8%93R>I^>?P2glfi0cSY&&EbiKh1!ou`-&ih%tvx!Q$$KhtU zo`S-gkF~YlmWOCZo}iYNd4f~{y+q!Egpz#5e5~VI-6Loai$`w<%x2b5Ib7rmx!S>Z zrh)**+PFA&ia=jy%@t+uzf>P{6Q2y4Q^!d?OZv>-&Q5;_wR-6lpXU1P*Zv`E+ZVa$ zg(|vxey$45X|nI}t^)T|koJ67@?!?>wM*NznTVZHyMQCjk&CjEUc<`QtE!Hk)#f=l z%(afPzQ5c^4uxdfcTltVuEli(ZTT^1u&z3w(j=WoK15Sq6;xO;VA)XePxC;HR%+h^=M40=Q9WJ{;ZkrCXo&9!a5oK8;BCbK`b>( zZ*9=W4#I%;m=SPx@|Ty7;`>l`WSjMbe%7_@;H;rAEq=uGj0Nz*q^^4%?qhn%otzX& zE+rn}d!AO@99{ZMMcKgg)uc*q&)^p|3A~-6n@Z2%SaOSFlzy$oY_Y7>SaM|3l0f1c z!Ttse;ws|Fz1kTuL{}G|Ioti!L^n4!nVg1S2~I`2oJOJ8M0@ELMZV#ofg^avN{~>B zLhZtz8=Hs}AWd)qn#j-p?;NPBc*q<)juW__jQ2$)N;=18-sa2T-X3r17Kca=Dzbhw zl=az~p@vBNO?6u5>iAj44bRR>)+(JfGH7QfpQ`JasrJteLPAD57UZt{8atmfMT`8d zf!CNzIXQY<4ODcn0AA46)ti7gbi41(-mSv_lLLAA$~Xsa)0=qbSb9y)X)O{R=wDJhs znESD8BJMGF8`VuLm{Qe{OFirF9`6;XrQ*f=gkI$lsGdtg?wJ& zCQW_Aqjeiw{^OF4u;8K4PanK2&wd%bfRa#*4D_B9RVBWngIyBGiq~G?=}b&AGP0$> z0>2CLqDZN}L5};21l=y@hJFVc?N9!#Pizgq$e1srxR2Uq*+T8KkCtTb#nT79F{`|B zTbE|DjfYj_{r-K#u6r?yhP*tN-+i>!ACY6IyxyK?yqA#lH?-vJTv0o z{=LmG8o|!cK{eNTC9pM&hRQkzY`EB5g_E9L;CD(NI#K09oeBzy#og}KOiWDdSIMy; z{&Wi=#^h!%WB+nI%jI>rU@?qL_)vv>XG{0k2WwkH@*MEKtFMo}(-LKThtSWPW9_Id zRDj*tLqY(qDRS{9De1Q=bHvqMZ1NE&C#Uxu^y-f$lBrdwZ#$r=%5tsd!q=oo{}v%~ z8iudPQh*!xod#P(QNt+4D)cTArm+Ji&p2$kIcp^9cAN_a)9zN)6JsCdCC+kf3SM`$ zD+)+lUrIwV*)kEMj-bG92jmNe zcQ#lf?wlTCE_$acQQ{1zEfk!GjxO!>>jj;dnf&&7J~Q#b8u+45T5@@()u##Hxud>7 zIlx50KhMgK?xH-|C#Z;mGt;WgYVDr?M8zpDwKckX8Pm;I+D7G72E4ry+tb(QUxdDK zDxPPZTg}ACXf=up%q1vBRV~EPy@D#4pj*Kw1E!q|N%UefK3HT!{H zXbV3q62$3?e*-4V?06DImaOsWRj8>UTQ5Ez7(Kfz&h|ZPpgOu*_j!->(Uy7j1v^2z zq^(Wn0kXl}eNy~+H!0<}8n4}P)O9w~`c%pIxc(RcYZ&<^SSc9WJ+^Tgs7g51evQ~* zKkD$-Dde*`WU-3e(|2ft?*Up%4S$U98}pjmz=WSas5L!* zeO3^%Mv$%<^&q;qo9ONvOGPL1jnFsp_iBZ%LqM^~lS-u2Un)jIJYjDo8BqCQC`v+3 zq%C}Xs)?}7@LP85nPH6c6aB+@3aMZg)Ch@KYdLf(VXxd5Y6UR($FlBTcik%IG;^V| z8epT$I@c=Kr)5qb$@G(1LE(+`*VyQTX7GiS)qZVm@XF4U2iUDGA?MgO9ep1kC_&Mw z2%TU40jNby5X30iowJk*I6Ys(LWyTCW4ddX7pk7AK4fgd8&7~IVJl7N<8EAC$eKX)!-8Y z0oYT4l5kbo_fxsf)SUW*K%5bXs`jrU^pxdI<&M*_8xzoVztSzZ3@H;;Zr83`-}g=5gGcy>ct`wsAExTCNzaNAs~nn|}xnuTX_5 znY0IkuM~J_oW|V93F-(Jr-$s<`T9dtmd|9urq4GI8H8U)i<}h|q$oI=u1)Pp+KUHh z`YpCS!aB>y19%5ll=a�r#|+)ul-&5`#ECv{1lHkLZau%u|@rG}2O1-$!u(yiY(t zh=kqoDCgHEH+0zrU-`(f8NHfQ$$_`M>Y)%mtp3r*^z>8`or@JMFTxRWu+aI6yhnKEfuh&FHDY>YG%ca?7*`8s^EFD6g?xo$jRdGoT z(6u7Ty-;>j_2Xa^yxjO&n}l3uCww=P6?zw{sOd_YaU1~ediP;-p=$;oC96*M7ZoVU zU`$KdO4P08klESUc0yo`D=nePeto{urQ!7ps0uCo>wP6sYJn`-itOy{2hst!A9*us3bC6FvD8d-1WC}EMUWKC+|)Dbx1fMGKfyp z&h$!@X#T^0pGBWKtSj5i)x7<-vXFzAcUb6q zufR!pN`;8_DNbL(SZE+b&Iyj)H_~8DwH2~Ad zCIaczA?&pZ$GpJU`KERVxw2d27XFSVFlTkh@`Kc$#Db|KYoF)JK6{tT$AO1=^;Tsy7s+-M z6V}|pG;Vo%iM(wIB$G`CAtYK7F7N}LU~RncdwBTAD=67zx%*e+PJit9N717atJcuf z`fG)NfQ>TMq)kCWO6PamT2g#@zo%cGokt4GZ9J-ZHHgsh}2FcrsY`XfH@I~}oJ6^fqTitnJ>f>~}0>O3; z?&KWI>rggEYmu(Er5aW`zIkXrTa?LJ2gYTG< z0$PKy{weP?;BdI2z#!_c5VU~SAg2_#3fC~oLpL2GHNNfJK;KNo@Ec46B{IE08 zu;8OiZD^rP)k0iW&d!0EFOsR%$InZ1d%oe0E3VhxBb!Mk@DCZcaYX_v&w{Wl``UEm zpBT#1sly}r^9~kQ{cfRt?Qe^1p0it9jolGXdRC6)$wnDgyJOj^w<(qwPq?uUpXX*E z?J2P2048Tr%B`!QC{hKKb8Mue9pAuZ;99Jf*m>j0D?2-Vd!%go+gau^>rOe}*~W}u zf`vhTx7zvyZZ!4{@#!$IYLkG160~~=0uz2u*MM2WgJ0@oL#QfF_7WwX-YMF4LBqdk zP{Js}zp9pRb!N{t{W)`dgp{CKanSr6N4%_ z3aC^x=PN=Jr6-vkxFSCf@HTDFgl*tt!PU($QW@szz{xmUgPCLY*^Udcrr!=i)7yE= z)`%-b8=DI>0V4CvwKCY251f2sX88r2$hY_GB!@uMy2T)4vjPqOHb8&}wu}9AEmE`) zRMa^TbHICzR+TgV2~~B*=}--6lKE|6tamo}<_nd>&Dk)8GqC#(jxqBl(6)sjc~J3i zv2kniudwhf82`SA5kc?bL0{6X{lh~^9cWRT$3nZf816Dkn~-(*pG#xeil_4g^yX`%hJ9 zMF~c%wR5?kfQj~4>|yw=kV2xDAI$g_q-uL-a7srYsYXH zl^Mp*cG#~6H+mY_yObI2w@Zg_ZREgE{Ab z^SD!gD({*7r;A=K5MR?m5&W$IDkhb={VD-ZH2o$h(}=sDezS{+A|AFnsQUh~Sc6_S zsp)5`SH?Ck+GXqIvrcUGw^39;f?CNSlwGmgjdH5mb9(?;O{9qweB}LS=VOvS=;IwM zm1RpyOR?YP%$q}VD{l!#t(l*1Qo~<2o+qdl+8*990(KrPbFdm8Y$MjKn*_vWZL_+{ z*8}E2>z}4tx}8GRp(H23qrM5GO9npTifGc1BcPH zapH)D;^QNGwumj-o}a=z`+wl9cR6E1?~f+6SGwaf$DW2@XBwQY&`{Io8#+d1hCu?L zUo*^9R8;)VO@PmjgbgV+76KG4ucSD@*x93UWmfMJbPxL>1|cmj;x_8_Zw@L~f*IU( zq=OjfjWW>HwfP>AEu{tu*f6MOMg!-ew1~pe(hpNk7oz`<9b&wQAqsdBrRmu>t|)oj!^9xnlU&v657 zYHptKo=LW@BqL+JU}q;a_dd+(xjMSa9kUR8v?-ZgnPGi{5EI&)qRq{b2D2_@aGO*m zJ6O$(dk8tgjo6|{53+~)rmC*~{vLba3SZQ^|K^3RL`}rP07UvA$!ws*GdIV2d!{9X zi9m4;zd-WuO=hu#;5YDN*7Cy%wpp_`QJi*`^;_QoJcsHRc!dNPJnJmsQr768y5t@7Fs!@GAndT$Tn#_+d>pL zrFIr2-Nr2F_Pa;Q--V#~pT`a4pX?$&O93IjkFABBQB`(V(p#Jh1ebH3v5V!S3(8CD*X5jN{_)Ca zY6^hove@_5!2h&L$oa?5k8_uIMXGDX-`sxwR^^kLHCq;QcHBjZaGW#VS*iNit?oOl zDfO*RdPmTME^-fN>TlytoIu_4z5UrsGHR9+UiP(V08+N^X9XBt5VW5i7JPK9D{Z6h zd<58d;H$G(;wx80`@q+CaChgoC(mwDO+@z=3RNAjO~=ff_I{75E}t_^b1Yf*Ozf_3 zvk97@oq-ao918y0(G_*7V{yJbB0lLNTCU?FbRiAKp`}8m;Q+o2^TE*fqTNDKCb0GQ zuz^Ds$0nwWM6g?4Ht~yhF%Rz#l-yC@Ehg>>KcQ6~t$c=ME7Y3{SH9gBuJ!O}n*qa_ z3Q;q`zJZyTy_f73vYQ!wqoAM=b%d4PNtCo0KN2%4H*A-8w&?`P$ryIgX>7{mV+Th}qt<;`u*$9G9f#64GqCCRK?(~&DbB({{BMvG! zl?(H;iC-k`4LtZX5k|2n?*3{kWB59jl}e$S_DTD;yp?7GwQx1bWMA-W;3{Sq`Ei2FqA z)GwGx)sR#O?)>2Gjr}m;t!hHnVx=o$cwxHK>Vg7G6ZTSD0~B`otLPR)`XJvsRaLFb zeQ_ZR{|X==%>^IZi_0EZPxBpzn-Q!$ zAe?p{AGgG8qgfuRyUAz2xTysFGPF8dr1ME*tjSelcizix1^p#6>2H=h*ZriE?>2$( zMAtR+(kRVP27$_FdUhiuQ?uy)IIiqDKmSNH6&88P$T;U!-;&z`2xM5>Z?!~C@0&Nd zv>ea3H|u3oS!8|re9(3e%NMr1f1;A~HY%kF#B`d9_r0RRGAljjo+2metJn@K|*>)q{W9kR6bTc@6IiInG(1%+HJxMHiwDn+K*rVe+ie z2DRcvSlUgXvggCl^ zCH4LJEy!{HE@q*ce|=!0L>4#KA-y-YeB4MvLXzg()J|QmZoN8~#zHb#O*{78yD$k^ z-8JGYPM26;zwg_49=6YCEK^kwR9{@~9571IBGRK~mZ4XNDzV&D%#WVx7K()#N#)M# za+jRcu&KCxBbe5v7L46pjUw|*{&m7ljwt842l04yZ27vw?BRPn-k@wtrr>>>qC-Fb zU9(A}Y_gTkPw#TeMn~!|0>`zmc`^o`Pk?Xf;Qwmlc7Mo}jbjj}x=fPX+uslKTxgNj zKFbI@S|1oV1Xbcd?(UXb7|a8cY7cZOf#~95t-RD2d3*d)F2{)q$z&6X>@qMYF|K?7 z?+TR{6|J0&ilru%3;Qy&rs7i8shD3=g#PX2=LIh{9y&_yjKKa5_P?NZ?! z>x;`l;*lPN^YhSOQR4DgN78=P4;UYB*Y>UA?xi#x=C3`pG5~|`nh^>KM*{u%;tNea zRmv4nn|CpPTBA)m*%wKs*`^Q6A5i6bMy?OD6&>Z=T^)+`G=><@2dNu^iW|^*flG0* zc-Wc<0j3d*7z=P-z%i_S7;vDvrSky#j9tz(X(Bq&&} z%A#ct{YA7acZ@24HuK3mV1&e`w#;z1ql*#U+R?)Q@o3NS^?uLyb*xo!iRf?j2qO+* z;fdJU#@`<(kqQ#9?rvGVvy;mjm$X4aQP*jSp-{BoitmjVy*1&cK(8(LiEq_zu9>Jn z^$H=$u{`TP062*+y9sml32=Mus*cqduN{q9RIHHU0$pJ>CA9I;%QkAw!&K36(b4vm z7j<1t?8c$YdKqQ6IHx+&GcxXL<$o^SyB(*huWndfAAf2jrU#<-_lc>+D^&@MeYn)1 zV(6&UffB!>_KmP1>}cU#q1J_CUUl{L7s6JKq(Dc<>7jw4^4Ge$f6PoxH;nc5?KrqU zCcZS%?9eP)6OvDdt)>Yb>G=fB>E}oCKT7PmUu*iAn+CNeAt_zl+}bMV@9V8mYB+%H zz_;9Xz6vJdxwYRi;8u@ER8B*3o94cK`}Spawnfy}$H#SLZS`0KlDpj7`%Jrc+)fST zU0bO5<6)_$kqoxRy;48TC#xQT9Pxj&zm}ec_qAkuNRyx}cE48rbAM=+w3OucRtYFu zYpayJzpsB7ydvwxK7tb9=tR>k6opwjhMypl*JhJQ`%A|S%-Wxb0c2eY_+$ffCQW3DVnITf@gmlK)VXCIcM2I+LvO@%K-1qC)ZlUgXYFz$mI> zUB6)(vfb<7l3DAnw~b#ldGR4%)Bqqr`F3r{CtIiLaQRP;E}`r(El`|CIItmfxY=r6@E>Wv9t8?GU`OYi zJ!$V-q1g1&&WZlN7BMzo;6A3nolS(q5`BG>D>(5UhnI#J;9O%=N(6Q?c}matge8&l z+S0>33tI!X85V$|XA$hslbwCjgFAp|v{aEa(^-)e!N&92#0M||obDD{9+HJO9f?j+ z!4ZZ78Z*YXe=5e4;2GdZ^7=JR+C@$wVl6Z7(e>!TQy0J<(j|+0)KY!?rVBN}Y78 z)v6i(OAg2jD_OUAx~fR`@?#cCENC;QNui zwLV^Y(oC1DFBPE0FaN8#OhS>efb_Qu&m~6w_I$xx zP$NSAYeF9zQ$8|Lp_00iLz@(PES~Sm!av^u|+Y$;w=M!mz z>+!5eEr8q8EuOzpo2;{5btEdO>DK91;CMI-$cXq}XfhR9EpKBo*)~ zCv8TOT|b9i7MNl#%P(w7B5)$U?tayb?)$IIjGOenlqb4h!J}SuPIpB|)3>{^umV zBlb9*QvH13_vNpy>@O=RSs@nT_%>-QGobaj2fViN6`Ux)`Saf19T4Jr9Jk$xaU@PLm_p(Q#@L~K)jgN9obUfmuDUdj)0331L3XRk zh!1@8q2Il5)!Cxo?`8m_X6wsVo&{K3p78&&Ma%tHCtc>p=}B>B05~3_Vd*aS*s$Dd z?bCx^X!2ywJlrN(qDR&KZ0dV`MUM+n_thYFH9b%YejmgBT`6gErA(sQjppmlxVl%-yfzo zDNLhS9%gzrEhScyxHDA$*Cl!UkJGho5;*+q6px*R8@E@M{Lh*m4VxpZv$KXv&?Gxz z)0h5twrJoMTw926x&OviSa*$^orFb|zD(F}-7zxacfH&JSU!v9jtnXB)R)jZe{`lB zF79Ay!Xb(?ofqQ&jW7z5?z(t8<6)W+`YD}*#kexTYoNzH9}tjWPeH7tQz_4Iy0UN#Bvr{>mo8m-5H2IsRg-WEDd zYLJlYo^|f3f!no3eu-tGJ`bJ2P0_v?XK8ZdxxDat>TgAZbB*z-$5)%PeUTEe5czSX zyfz+zl)b(XVrwc!On_9+9DTpc@?P}3?l?LaQ7H$%*PUzr8~qZpXerSXGed#<#P&rt zn7LEqV&Px6c|hiU3K%fDrP@rQ0!6X*JPco;w7V&rnApVP8fOGqtggTy_#YWn0DP}%?Z|CJaVFIUoUgP7ZhoZn9 zzL0U>5N`_(SNq>1r6^!DFGlMRkKp;7eBQjXL3<^i+|@z``TLvi>T60fxKDkTRnu7lrGMqG^h97?$ zBJLlwdb%+tq$N^@uu@n6`%20%^F6!1x)eYGqe;MQe*Wju!65>xV0c2(v1iFa`c1{yyInkaq39t8J6mfLM3%)=Ge@WN!DW9gE=gUC+s<;NQ+>2yig@woFvH0cqOTxY!uAJ{$v^?y5r;EF ze}!W^9jMmMic(U8e^aaJq-J-_nR+iamdAYdau$Eh%1uk1R^z{%aqdw$eCxEq9Bx7P z0!r{H1pNGFYZ?=_qv|E>H}>y{^d> zUZ00}OZ7zXt(-ez-^DQLJNhQ!CN>%ZJW5%DXA%Sg`~V~TNR*z^Bxt$o@oGyZ(O!7~ zaasUNYuUKrpH}(*MXQAmlHE!!Gx>W1>!8&z$5HO8#AO4ID}3&qv!<)76wvLWoI&Yb;)-%c9l zwWXs-w&xiB(*%MfNM4?LC+TzQ_i_YqhGovQid+Weas72%9iOD&@IQ5}<&rfoOF(+UmR-eGN3qkPw`8kx{tLow-9QJEM- zBuXvZ-+6K68S^(ID{1m?*5M?-ufh-QLWeFiNi2i*l#MEEGROhw@-s@HnD83?A z*(0)J)Wf6_{-L3wtz4yO^H%pI@y&+-a&mG2KTuNjy6As@gzI!c_J008j=ubY0s;bp zLPA0U0s;b}A`;?+Um*br32_w>F+~YcNj_h&(~v33gF*am*ocXnIKNn4kwn`>?I$uK zlG|)ASdi4Zl?^<~A1O5AQ^UhXcIvpL!Suu`i#Ou}1G5;MhP`}GaF`OU{h|eyX(+bh z*%MUpf)zF$gLRp{Jn65tbpG1yU2DlF26dNrZGz&_D3gp%KTF|-qeS~oH{5jp*R>{W zfA`c<+(gEg7hFUEN7?KmEmf#^%o%zOYKcHoV4b|V+(>O)s- z&AxR2+V>3yZW^{cRB&+#|LV$hcp6zT;yvb*Wi28p-J~n9Uuw8F$znKtKeg8x#Z)X%GZykoq3HKR(pUz2EO!&$FL7 zf1LN-XRW>VTKl|bJ|%AWPq3n?;)>Bi0&3?}8Htk&(mRgU@}5ovzUrkob=5$-m3)2A z=8J&)bg4tvy7%I}{DunEaItD4o$={MKJ3*I+23h#Fo#716gXkV0wnC+EYC(izw}U` zJT#q#9o2$1Ov;6lLq|uK`^hmT*Ib=y#Jyf+;OwOf9m}5PPOT+$3q76n+N*p$<|`Da z((@7N@j3i$kP5#)c{H6SR^(*RlMmt6KI9IfLCK_GBB@gUtxVv0IjpRO^BCv|;-iUJ zqLsWg0tP)Y$d@!PeiHx%2geZRlFGJ|?Qagd^%bEKF|4@Pu1TAtEel5v1U|lHYUDmC z`3a4zV_9yxDHBuii#z$%n>}cw>_#z*(cB8yQb;Y?PPPzNJprl8Z^opB7-}gM8mVr9 z+WjT|6k1att@07@r+Yyf74(63Y3C}KJMs0Ol}Il5Vmzwh4!^IikVr(pYMt_(@Jopt zT^pgG%cV=1U!X&Kg8dT1wK#c)WW(-8e_36x2 z_ISEIa~Zv9_??vr`Ux*%a$kx|9eu}m!Umo1N*eI)t#1sUlVMNV256}B8$*1zrLpuC zmARz5_^?@d!7E!`Dzc<5hG8nQZH&BE0y4xEJ(;p;>LioKg9@c$m1Ho@WJeQCi)<|9 zIx{<}HFZ*97EfbUNNfnZRucnLc@(z6ma9 zMYqTx=}kj@6xfxMy30$~U4%hjJ3_lQQ|5bN))rUMQW!`zv+Bq@Ze~k?1$oEphagsP zFC|JTtv1yj7Z94q=T-sb7A&S?GgOC! zr4bx(;@0_tZ+wzYO49qlC^{e%uQ&%E^)==dl&>S^gOOjKVKyY>4Y_~JB49OB$ z{k%=1{p+LX=i}L-`AXnB9^*kg8G}pI(Y3jWaS{?+Zngc7?(iLG%&;qQQK>b>kjH&| z6pC|jzhglwR_Cri3z753+e^_T%a(NXUe5Sa<)cIltWHCCp$bIvF+=>j!LxP>ggy*3 zOnR!esWcM$db&9D{4IwT4sCt`NVmSor_W3@c!lzKeeh7Qb)-cNm?Ux%&CL8#k963$ zAsicg)n%=UYuqUXURZ!`81`+H7LPb4-BL&&=S`L>KQni3zp3UZ+jw^k6JBd=N+0$C zDh)Rl%UijD*ZduWI%IsGGFRgpMxa;LDeaF;ZLxk<3-%rA^bHyDmBg0AT_or%0NMF$ zfozSqnBrTr>(t)w;z_jrU_8z}N$#%z>tQS}#+*9Ne=B|hs4HV)=_BR5PS62>jsO7u zxv^XW0KggHZU=GZbhEdcq%d-xKyEa?_RUU4jAK5ho9C*lbS!gj^bDR*O}XPqLl6f; zxq2PTK<7k;`tbCJay@nwK65kyq}$2Nok_6rRkIiBtBlQ)#r7~yJgo$kxzX(J7I!wS zsY3ndw^jxh)m|SYW~V!zY#ZmWgf45-&9G+IZ7SEyRBw9=bCu(K_t5G*Qi_U+*A?ls zcF&z*(->%=t%p~ZZ);l0s0qdy(!UxB5iiPo8#Ix{ST)UNYmq1#Z6Ojz>IBN?bY~4D zkVyEX7C;?ZLK9*S+vw0gnfItxFG61Pork->NJ+sG!E0GYkXaW%glpYo8SC1VX1w7c zq46L$0t{RSIig!#)HC($^V)*h@CutCx(7NO3>+xvZ)IOo2u3RpZw0LLAgIlZ24VOa*ZIy;soJy3!hWbriCRb$eX^9t%OVUebY$@VO0yG47;*y4q}wX zpMJhtB;aSu!I{PlX*?C-s~%F(SJg^vl6Z`Rzc5RTp;({x?qgl{;JvDQ`B!R};@*y^ zS@(1&?=dxwgZuB$JCA0YFwj5Z-K5zOApP>Cn2dXq!XO4KX{B_=V>`N!P1OS1f%#aZ z7i5U_9^TmnC&bY^-MvqZN(K*O#85uscGW+OeT8T47`{T6Ue#-8(qOuWl6t24ZoxyzJ^^Es^O_X(1LN43A zo_C6x^+OqOUIgc-3SGQ>D_jXa+M6YqD2$}6wn+I8 zu!<7(TPa3q4?8?Wf)$^8VZKn^?GS}_hs+{xE?4evcpxkff_2yKKfp^gKUyA;E|KUT zYQ32gku|CLV4ttSy-1?H;=9EW4yws&!uAZH+otw3qwu7miS3E^q{<96A06ZTtN9Nd z#@Ji8ScON^%?nID3Q8rcT{TvT<_+1`v$wsN-tn%-O__RP2*5UWl_Pb;*SjFY%LfN@ z%Inpz{VS$V_4wGNLgPS+|Ra&siCMjBqxrq`Wmn)BMG&@04u z+l={lnaugpnps%=dDfl^BZJyZzWL|I%wVr>A%z3n6-Z!pfOiO8=IY0#q8#H@lRP9iQ|p2Ql& z;8FQOlg$4fYr2+F96HEHPbvV!*q}G&=b|xRS%ess?#IoyNgx`Vp zqfhMat;EJeH&oFX)6ZPN&C<`)acKfY6n6{pe%wG>T7LOp|fc zl+KqQ8JJHV8TdHJPneq0q==hJeNQ@8s4zWrc6l*;VYxdSy0jg>u)KUjU|VpD|8oh2 zqO6sKPL^x4ODWNIyo%W2l?uTT7H7gS!fP(^>Ue^(Ql2A(bl-R$_>HvI*PEzIJB~l* zE)r-*96PFex`-nENBy%jkr|~G6j7Ek961FG*ss@a)$MEw#uyM)=jYWrlG{B$A}uem z6P9w*e@M}z*IL)}bw^Tb_ zetj)SSOJn{p&^ zwux!z5dDkcqUD?Qn6igAdPC58uGbOWh=I!uUthe5XVf$rppLmMicGZ}N@I$7b__DF zvA7n~R7bhA$|!L69^E(W%@dvC>AG)uvENq*B4Qgy#s)*{$0kF0r|YVScF^pf$8E-Z zEph&U%9iB1OakS*3xbcyR$%V=W+mrM$qgg$o}3OHO;cXo40`SCb2?+VPo7|mXQnO? zR_mzI!%x*WJ>Md2gQu|Jn7D-A+HT=SDNLUONm+c>`LeiptH(gQUw5Xoxoz0nvZQXc`G-7(Vb#($v1oexu8y>6l%#;m^(~nR^+e`!M!SwO)i^ zD1E;(x z4GDQN6(LV4EOu0E#WhAM_)2B0uA4vGqtU?WgVm1d@M3sg9zvh1!!AVazAY z=9)9ESsPbaer`)G%VA?Gdm2%aPjTPm!ZyL{_kr0fI*#*fCcKj zMtcm5vcHX??ucYQIJ}}wr5#^Tuy0oW@Gjr@phu;s(hZ7cy)tbdGm1=raF|io#YfzA zf?K+H-h#GphDa|l9S?M<`cYYr#!;)Ra9&VX%eR-Z+Xb~~q(@nu^He{5PbG0lPSQ6a z_Dh$p6AJk7b4uC!coI6hf(eqKPpEaq%FH6TlZ8H42J>#DXpAOA`>{H4kxqSmTa($F z#uwUwPx-1bF+Bth4i=+!%Kj=Qip&v+;*nf%?;iw5z}e*VrP zdI`%#!}xDmV3)95G?3rJ^lK42YathwXM%EhLs3^o$2hx<03ZNX(EvakeD<;Ne*ofC BpRWJ_ literal 42200 zcmb5T1#nzTleQ^lW@ct)W+sc7nVDrVGc#L^mMmswXraZD#TGMf*w_B%&iuQ7%=Qx< z(Wfda^R3K072T(mWI@5ufPkQYfUqi`fg^-0aNkT^TH#jT|klOk9-yo5Pdpr7lA-AfR9G z-ww@OjTk)bZ21H|?QDtM&757V9PIf?m>HNzh|KIw989h3E%-^)R3+%SNCX97_#Dj5 ztxU{#{zjqywVue+&eqXPn=ik;mzXSa<;%RGT zZ^KVw>FVmp!^r62;lbd+%HZH^!N|zr+ef)dPzkNA4IQ&QSuYCNIuHVvGn3&iY z|GECF+QrVw_J14kmo2@CgPo(1tCg|sf6J+ny_Mbn*3B>5-#h)cY71vmQ``UO{#Q|0 zn3z}@Ef`IWT#e}6t;{?~|0@4yAOH8=a{rIrnmQYK{9jivGco?MAtCz5JI}uwm4!s$ zZ&&^*p8((QBAPgx{oV_`sgbLh01FcnH$4*pI66C6o0+&Wnz3*ju`sjIvlttjnR2qwGjXsmo0@UZ8*y{8vv8TvbD1-7aG3rH z{a;c3iTa=W+vQjDySQ4J5dBVVi~@Xriuot~f3N=I;&)a4 zIfbC0pne}~K>vEd0RaKIn7RJSrOU6AXkUBFWtRiV|H2^g>bB62DD2g`AIu&q1lA>F z7HC<2E2Qf)j@Gr5Gt`G4xrxg3%JyhEAai1X+(LG%MZ3mBu*UD@n4XBTt{2?AX=g#9 zY8qN8{wZS`ciMElMwd4~OK_uc70yTLArli^$V~imigg5+GN7Wo&Ve&00>C2*63rtH z9MOJzLI>8KslG*^7eBV7u?h8s5#EB>Ow(gy-`T63PteTwe97M4e*N*>+;UWyii*r6 zI2zf0X7$S*R_>0Ma?0J&*;7n|NAX?AUBB_bOywf1$J@u0C9Bcr4Vz&b}&a1D`*#~yJlXrD6X-G#I zJkkuO?Cf~rngK)gzVRHCHk%JF-NuFZOS_+k>0CKD_i0o)nNt7hgkDA_KHK^9uDnnprag6NsRmVbG~I^t(8 zqQYaHN`hqcQbqBLIJk^>#QS6vFZQuEg62#exBaE;cx!u%K?AeqD_o{`#7{bNDUz$t z<%&X0qbN0WVRZ<4caq`7Pg4BWgX8z>sCb4^WN_G8L;j~#u#KfpjVPba8nw9?OVhtj zn3;%=h274{q;|6jpg6iYmBnn+mK|ffx7lZ`vZ!*a6n#Xq`yF(B4p>2ZU|RUxTtWl% z2^W{k&`x5dHgf-F!$s^fVKvvmBac>qawwM`4Q5Q0ULg)=uYAj zhVC3HJ_a>q3-rDI@Fm5CUye@b#Oxs08F-P4r}=4dXlMo=^3Px_kO1?EpYp9tlRe|~ z-jEV1=;(uhLexXufwv7I#Ef-n{E^;8vjp+X>qRA`cc5^H0VZl+ODf`_H8>~Wwx#9D zFR32}m%eKgg&wJ%QyxBAKYXiXg>8m#aNiDzU_O(MwTj0fC}C99*`}d5-ArGd32KgT z@}3?=YF=4q`BZ}IoF~@GAuAtYgxnd#fGe_z5}-A3@9nzV5M8*>+l>+s31K+U)i8?j z)vgmBGd1@oR8xR>LQ1>(nmL7+Y9DEi^#5qf2fRK$rAuL7+K6mNl|yw+0^Po5cNQ9wu`=#SMd5Oa*02zRZ>6ACJ_>a(2&B(}{)Qsi>y#HWySG>DToA z?AWd-vffM_Uj{4kmPJT%v?GRe>Sdh)g}^_kJEM zUjv>t4#QuLyx6ki%aSi@J^O2m=1jS|2o&vmI-ZaB2u(chI+j1ydgL7JhX>|bmH|6} z0n5gSaKIRdzog@X>ARixeMeQ<>W{T!LxH>iUTnUYgDr-zEd@f!vLYvT=V!`3>Z3T0J_f?dAYFtJ70_db>@ijJ+O6C zk=9(ef*k%+wJ!&+i?0W{^!s|u=s9rf z@9URJA6*COBJ`1ej`_83%9G_e-0y$K6UUNxZRFp z+t*XH4mrM7!Yz@B_Yr_EbN7=<=OuP1>$>heQ<$=#wtnat{?_!D{N0YZ$9n{xg{f_S z1Pg~Rvrx{lAO_#I_A@p^A1|MLHgG;0Qqls*jFHa z<}A4}&aohzy74;=tL|E<^gTZmCq#eB-|y=j8}VtSHx@D{A|j)Jj= z{qS83N8`M0og!+DzvE(JRqK%2s&FDm9Xi=ukvqS4L3=_WOptNX)N1M+V!2Nlz9ojj z2l^+N`+_w1w|H9x!?&F1FVl)vLosmgXLYs;hQrlnCjI%WpoOTKWHy0sbXN`myZ~nU z==pMMk+*?4vJgJpF(X2aTiNnQ$ruAJZoKM!kK0FZlVP}UM!snutRAkLf^@_H{YpTN znCO{N8L^FR2fMjuC^tmc$_4EaX{HGy>tJ_>H!G-?9kw@%i-AVW>d4i#vxKqu%f67! zi>FnTgXDw#wb&W;ZhAfY+B2sM&NMaKU^)}e8qD#*7cJwlxsY3wB?}#5cKmWl)OlIW z97$DN7lH)L(bRsy5{^8!Q4QhXuKAmXv2br_*^dOS4@4#kNLIX51Br(RfZ_)d=f@+q zzMJ#;j+Jy`_uHN#iIKHpL;|Cc76GxGUNjlMNArY#sX7Y>7CZEJJlUE=%CV?wbV)#; zFPl@RBT&3=@L}DLF;p;)D!1fuSPj13e+oXFhs8ub;U2cWg;LN`IkvS+3vVtqwVcV3tWcR5HF5TOjJAcPV654cWfOr0$sI{J}RXt`Uxlu82 zE~dbHqJZySmhSRBl^2we!@Rc5%eaNj{oZy45a`-7ALyNzTKTfiGM@RypRy!oXMIR6|^FQDCF+k!9tXcBDUpoCX4mv)}5=GDq4C@*#EYN4Y+<2 z{Kl@i7}87^_RVg>#M|QshF!I*eN9}Zww>*aS@L8gKVma$X+3I4({L@$dq-7GmvEZ5 zsJHO<-lsc-M%2)j+k$J-E7zUCObtz+7H*x6$MmxEzVWB{W32XhxMf3!ABYpwdN*^9 zQL*>r(|UVvj;;5ZHOb(I(Q~A_*KBv4_x zhx2VY={#rBU+4BLfy9q5uIazd$-hF8XbF+*0E-UXtuM=B5vrULYK;%y(0#i+VQ~nAhf<7EckEOxW zGTiDN?c#QT`WZGR%at%~jBR+2Lb+r%tT3A_n>b7Gsr1^Ed=$;((=ig%+EevIjb`k; zuaz%I3==KL7Ls0!$`q z2SKD_#2*+!6czA%hy=KC@_&fD3{w7sV~Dse-S_Z=78PtdcE-edjF% z#%f4}|2-Nd@q5&c^B+r*e{8E|e~lJ;@?Ak+L0^qcTuWyk!wpjU9VD+ad>*8v{E@5< z`;h}`gl94jiYy@ry(h)muE2>~UR-(+&p+|9fEpdg7W}gba39xLCyb~wLtvGkH?9YO ziC%y3)PeSypK^7k`sM;0r0+oDU|>(UL^01Zv~>I;6`izn;@H_drzQ{2m;sinMHLdm zz|tOP>N;fDnZc7=bZuIx+Dlzn*&Owgnx)g*!c%Kpu0wTxw4zT8M-1mj1!=#fB36pG zM2PSo<|3G=T4JDr|4@u2h7s;Hl)Xvycljl#iFKX!-5A^cs5g zPfZyoDA_V|A4C370xMA1cAh>#*)-}S`1qp{Q?gKevxjHCpyE7T2B)bABN0LiTMTOh zC9JINrx?~>VxfY+G@u$G{eg`HE~`x8--)s$c?L&@m;))5MYch*!OLa0Y<~i8kcgwT zO!U}A-9_C^PTxn(viBGE6{!~fjg#wAf%HD+APyUJ!f$auLVQJ%#~C9OBiR2TZW@Qp z?KMa<*qHdA{-peYnx*z{7q&~=F$ZY|NOovJ#N+dmh$LtvXe23^WN2AF{%s*5vLyGx zN(??U#PpGmcMTBoLOrTW1+jK7{Wzv!3E0<*NHFqBJ*pywWCnj0QlMpZD1?aoVII10 z_J`Z=so#DTph6FtQ45j&;3`7J){qMo`iD|fkwWeZF{;1hBEiUeQ;HD(;W$W%OFsNx z*C9%lJbcJ7e~_UJ5n;8jjthezB?9r)E2Ur~=`=Eg()Y)RS-9goT zgVxt5$v6~Ah!2(M8xGX*Z%vPhIxSz#rk_{H?7o8U4VWNv{okGx25W0s< z>nC{-2R1#VN28g~RSj1!Y6mz(qXpo+tx=&$eq&OlXlL?!n*7$W;>5tVvLk-+gaACr zcd-!0a^cOMH^0{#!^a7`rF^F(2rE?OwgQN;Sfqe)2&=tBKA>ig4A8baikGm<3)x)E z5LPJ09*m)OGFmty^=jin=Ij86irDY|4&I7f={=q0p+a+0XxW0EEv>$~)m4o&agaAx zhAJM;Tz;ONgns&Y_V455J;TxlUTXJNt4{uTn+BYiV&2Oqu6JEt+-mDx-9+nKx(Vcp z{gGhD-&}LYT~0CGecW=A@b&z+=-oJX6f02c?cTbte|+Iz)n8;>&GAt)fWNwct}OM-T}?Q_1ypv|P4GQe&s(W%EwUwY<=Lx!;!I+CE0?*e{mY$i3-is@@% z7cJX-v%ad09!Wl(BO~mVVTA0@7eolV%-TDhWAh8`d)wPEtXb!i4~s5a0FB>)o0S`; z8kVm{Ke=YL#ONtlV#P&V4*FojWt=6wVj2?aeWTNV?L6KI&KJ!w?6wM}K|TjA&4eve zgsonrC~D(Ix9(^K#s9cxP+M7QcRYyxa^r*pYV?6tGlcs*R|(mf8OuDKc5HN<`RLAu z9f-t4_so>>qt+9TF|2?>5OI4mKV6`J2B(XV9;4jLC_Sq8FVi*qhpKvB3^C;(&rZ$(ttW0bd z)_u!slocF!60urVEA()1_b+`A4Lm#ay7RU45#%mPx9fI^Li=dGS#_jRaw5^)R|W9i zIHvw^Yk%gfN0ioc_)=?u3x0ab1%ocD*4!MF<*XrD>8Jh7CYgQ<2FT#Z$y~ml)>`x^ zIWb76ypUQ%_%BD`O?mrF&`AJ3t_Ta4bJhxvl)a2MV3jNw5U zX5FjAxaY#l4&Id@c~*q=g|9FWE9^Jsf?-~nV6~rECt*u1^e_gRq(Vu;xVZ;%D07iK zZqs=gLJ1>)vsi?p=@g7Vo5(|30Sf3FRV#!Q6^_xzdJe~~SILBzSgR6utsvKuR-Yma z9E56AV9=CpWcBF3)SF9sOck7ro6lt@iw;W27{ zV6cMS+ZN!<0@v84X^R?FS+9#l)@V?lE_U;itrFGTS0#^+sC0sg&x#N*ev?=lNy@K{ z&bnAm^3y&!F&Ud$R3Kn%GF}`>iUc%$FVn9+F^S~KUV2@qBJiVgG6rPjdq-BrlXLJj zwJc;yRa~e0Z-w`tv!-$^yaGH0X7uJp-!%MpRgBjvo7%q!4=pW;y4@i-JF1;L>6~_S zS4zin-V4d+KDbM>Uucn^6};pvCT?Z$DAe&NP@no~jEu2c7PS~#+#SoQ4S@TU`;@|s zyKDDr)veq#bY3d+T6NC$^Ab+b!sFL%;@9TiG;|s&0lf3Qk8|SpO9SrFy$JLv3f?nosj{Lbn z0{q({;S?S);r5jR_uYe!D?B|pfsS7TK4w0;(u@v>ai!1U%&N0Memaa4si8qKrvR49E=mAwE9hPMHroWVM_bvJkr4bfF}Y9tp^C zqag9q7s}=CQY|x(|Mi?pc?ANFbA#Xu&@m=~I@HrMo-g@_YdcE5UX~xho zg>l96+hO0&XMiC`)aGQE$KH@-t7lg1(psy(mgGz=;gh#y8+W_-rSQ<`_h(+=SwyPp zRnad=oPZqw-&YsrJYV0=4-|v=gT1w}dBm^N-Y^Cb9=&Hr^4@rlw*3p0bc1)C=|VOyVn!F5?rS4vhvWXI z-!lRkIH~hLpbi5(Ul^WYDK5n>EC4%}0`Z#)gammMdqz_6dfzxVd#qnZN|_{*JNity z+T$D~0pWAT_v8zwUVP8DFVyxjgbh-T18IWk7av@j1vgF{+He;HWqZ;qJ$bAx`$3FP zu=5MWtCm2q4;{GJZ|4$y0q`Ho9eM#?c!j9fR?hf)!?(d>XWn%@mWx+Yo@~L{=&TIN z`p)o#+Yi@KcQ!`y-5uVLh~F-00>VFfI-KSZcUt1#EvOU?E+HQ$H5^_Z_8mHp!tX3F z?YF5?0^;A#tjYU7QYnZn!h5H@eMFCv6dyqGGENaRy+9c^FMGZo^~{VUZ@r#F2)rIR zy+-lgPjRjhl0+c}ewX;rb0#&=BAvR=HoG#Pg#S3)+K8}x@vHRQGJam!=Fsb;H_aXC za9|t41(4lBczhq{1QT}XnNsW8p6>9KXZP3~mGz@cmAjWX+E{*&y_FHP$a!}pUbv+3 zaDRvH$}c`$KOdQJVa3uZesqBwWcT;%w|&pL37$EAawQ|3QZ*Sp5yP-bNAHOh5L^%7 z?H;avS97V3Q%`3bv{H@+_;M;>3TvApX-9-uAZ2v`tP3W@bE@(yz4<}wIvUGEb;AN)%($axw*ak;9{oH+iW`5{NY=n}hSxLP~hULy?Kn`jnyK zNkl0Xr3hLLT8$A>Ej|wkzTDawdJZH;Z#qBj1K4{bjG2edKcW9*TPGLPk?s(+fj$6$(23!h>B_@KpJ=mS05T zIv0fng+(8M<-7Xm%AkjJTDQt|Zqh}HQmWFOjgXo~bfw;t!qmLR_nM1aMYnDM2jbiN zfRFQNl4zPxzR-uRaL+KY4{PvpZGV74^oKX)`^4a7Qcobc#4+6ai`b*>E}f!BzSa`0 z@7WAshIV}uVeaJQu7fk+Wry+OzOGK7^GWpeAhhgMIXRe8??+3A(2T9%^0H3nn!0YcwSQc(S62fZxFW-6V;Ft{hD>JoXV{S z0{j-JWjp6k0#L7)@@wxd<-J)DY+P07hQSdKbt{uXWCHmDnP(D)iFyT)idbn}L#e6i zFtP}I^E^_gM`cS5xpJOw(+uXE72gM4I+E2~96hN@t?Oy?2@ayK4v*@DY5+&^d#hC5 z=v{B89fH38IXmwRf{Kt|PeE5qdQ!Wz=Oo9-Fu55m@;FDkq*FvwL{nU>QaK!$*m7!}zDJq$qc7`Y(Kp|4TmZR1KMYGx$Lxq0`_el{?#bcU zv5ksz$160jMB(h4k)_Hj3XAe#5!(%BVp$VFKNgTe3i=J&mDrie|BxR!8_n6=Zop$`O-_Xb+1|jqh?!ir7OI&BofN(}$hj;L5R^ zRi*%s4&6E~uCS}v*A}5x5;B{3k8J76!Xhw_)4lM5mA5@h*uS&NveZ8;0{zN^EDO3- zRTH}dtUV+#LLhK+o1TnNNgk_qDLbGcSzXFoA~4Z`%&#UcTJnS2yHWS>c+7*tE$k@r zHx;tp)_Xh4k?iOM{tlkg@cp(3NL?(cKTC|t#Wo}0xuq#UxC<3GKw-VM;gG3uaSQMu zZAo-X;9nuMUFJum#1g?^R1#qY+eAeoOHx@-KpscAUt?`Cy$C=Fc@6X?Kv-_knidGZu=hfCV-Toef#esHuUe6Quwih~G^;t!jsq4Gt%fS4^r zi~?N6B0|Z5<1;d1If~y1_{eSAzO|klKNZ{AkhsDv8?7}!Eo)iAE(!X52s|6{J%uV| z{pLAHN&8T#eQcZ^zqm=h^BC$&ORbU!mHclu;2H3#F|WFXjXLHC}b{V zF1*?=Zjc+t6{?%{?E0pWq(_B%xn%Kh&JSnvk-=~#Zp0&^Bcda+CM&!)1vaBbIZEgT zJ-EKlF~IL=Re3xh8YDebeC0CLi4|n@o1L#AMybI@ZeUm#?XZ4aLpzAY(o-TWVEz9< zxf*)!&sPDl6TDRNF&TV5ehVSqFi3g2Q6vyOj}FpOuRyabHu9aGR$DN!%il+>BNwUStzm9}v?p7w9&Dv8QbQAWLg2fL#l6vxb z^7=FI6$Q#<7a~3r;MGld0~Ru^h&r}w z@;6@}6EmWIb8>1#llfR9M?RS}UYQ}IewAoFE;(QK1m*88Azh744xPVOa<`+;%Z(vL z-GgZiSy-4rirz&iCy_!ez96gAC@++hyb;yCpM|`T(O_-+;*}t}W zb+*eiqjy%8$)%kRBl}vxdO9Khr9Q#;Ym|X=8D4a&aSXo=#fsi!s4={8`dh-%QpS1S zd}Z|gk`%4H2v1GxJ$#_UnQo)HZt9q}Tds|)qb}`A9O%;7Qt7#y`J%w-hc*kQI}AD za}Q)=qcMNkj8)}fOt)Qe>`n~dYZy6U84-dTaLI!Mi#igouj_1QXT4O+^z%`OLd-UI8O1tskZr0xPd4r5@fVlOejLtRCA>FR z0*hMBf&5ft6oDpp8v->`?!ZW~idwLP2eb_HnSRjBjN<~2yt0qH;uhbVTRnfZGex+|Nr*Vi-3{}Pq4mUU_$mi z#exlDK)x6ut?NT#Y)M`~u=6q^-J|^G-jZOmkNEnmO8c;A@AT^g%ny?^ABALVq~k@~ zt}pDvo->ruLW~}U!1SMt zYD;~vu4+v;!LP=@sK5_aIZ3o2I4{mzMGm`5SMtsMG|VuaCClwI4Xz%g$xy9kEHaJc z&v5#tkiDgma!T|)uL!NoI46lpEnDA;=VxjWu(Nt3%ED;(sLHpaw1NPS6?)ts>gr?j zcVBeIL-050&C)j-f$vl!=Q++(n*r=g*o|h)=Lr1+t!XyMi_^=OW9ETr^@h>>uI&(= zN7-X%fHy-7{`m8JT-|M z=p18J^@LvLLRfR0fYr$9wU0`0OA-ZH&u<*l_7ZSU24f#G@5}E`1|CGk)rufCK{1mP zENtd5>9a?E1h{RpdPaLFEie&k{iK~<;B;*xV%b~Nx%@n=fK}GAOaA zf5428|vdsBIrx0I1N-Uvrme?Ntd@Dd~?=RrHS232a5%Xj?7YW0h znRtK4#r9||*~IoFQz+&iW+EhTmGF2_RaM=ZyJcgpS*L1d=RG-}%3bYu!(|t~3A^=b zKT&<1iJr4%X^r1hw1^IF!QKk|d1vEPxlW)%yI<|tYRBttMf=U<0XO~hupejU7{!Gl z`Uw(BLBH$$?ZspAxRw*!^Y@Q@U(VdvfLsNPU>~Ihh=&{H_uCsw2>edZM#(S)4;Aut z%SUwoq(ad!1Z`}-7&@;xo|G?wCXCt_mO0QxPJg%pxnh!K^B1#1 ziS@_;?EWnp*PJIY>;U%P-muZ7{;w55Z{>V{%CirLYV7oftDq-KwuVRKQm<*cyt11( zj}8Quz2klWPft~|A_sL)kc!@owwa90{kH>xN-U>3Y#$Dh zYu;jgf1gVXkWaZVau<|LeqkWVxiFJIDVf~DKu~jGCeJCEyuv{CB4sI;+(?*O@_R#? zzGU`Ve;PQo{ z{?eI)EKjys!8DUOh7wrEJ+k{0BlIr`D7b$mND%!?LS*b83G}21nmK>y%o&uR z+pJ)U9EwjxmLgO5gT8_>DawUv#-ZFUV-hk|$_6i~Qu8E~?~Na)ELiQPgWn48P+102 zxzr8uR5%;qysy*^^?p6|yl*?&0k@4nm&ui{$iHKU|0g!_zhe*ljh%)hMW^rw{b?go zj0@F_!QBxVC^8hvKQJEFBS(7BOuA59rAtGx)zU5FLkG5-iEO_-T<}Ok=?1<%*Ly@M zG7~E$ABbilS_zaqB^-PjHv*JA{mRpl@2@zqSA95e}3(t&9sf{>18%>d9nbuST5WC ztka4Oo2G_0iE*rB)K9?KcD?pOIS9RNVp-nIRL8kow}?VFrB4rF1bg&M5cvufI-bXF z;?c19Gn2LNe4A?Rr0$x8&pED?X}`9ebY#A1BkXD5qGPl{7jgd1=)!f(IIfh~EGb1= zg+e-$%&aj5!%`~qr%J|QwcLH&&<%Im--aIC-ER{5+tPTl|LP}gqQCtVNB*yVqEGnS z(j!U#YH19qKP?@fWzsfY@I&B=Ctvh9S)QkWs=h_4l*Y@BqI5d-VzNZnBbq)lWnC1V z^qk#fMqSF^B1J$8qV&5|Rf=$?00xnDoMR-;fN?#Rc+XDcH@Nm9v|;c4p2&Xohhy&W z-MChHEP*Lid-$X8r@!K{!uh|!tCp6l`E72ixFQjzDnOE}-*m07;xE?)I z$M^W+zFjMhs7?6K@x#e$)>nEV_LUh-AJD{%=8!4`z=H+KJIN6&&!)@3mv-Maf zz0Ps?Cuiq`uxnC^WL6iIqLMoIxn&;3WVr!hO;jxh_&=!gl6MSmhhN!pn3u!grd9vc~XVZ6Gt6zI+!{7hfGc&q_0Eg7Y~v}!jHtYn~R$7sU^srRH8<`b=4}keN__lMl+|Am1`d z7uk&Buv1UaEGw-FG)8k_nIMj6iz#!Xn0(6Ic@x8C3Stam46>UCbhY`3T)Wjv_Pg^^ zAFUIPmHQ%buH$NE@Pmpo5!tT?^n?;YkwQl@M>0p>OB$~9cUSvlZx?mf2a`xKE9Kh zk`dAnlM?Uk6YU94P@nCKvi}ezC#3oHFk2&}vKB+Fv=QD^=qsV<$DapA0j%}kcC~1vc zAzp%UCri&GP|i~CBU> zq}Rjbj#{I0GOyx$+JD=yuO@HYD#EoBn;V+{=R>;YYMv6U`s zbEG$~Eo9(F9_e^nW^-albI-BpU_@-)bdqYKV7;aVPX+aE>ClD)}MmuO5CXuolc!gx`|~>ODO0v&M1=g48qaga!a+ zcs9jS6#o#A+`M?Vc2qSlSXF*YY*A?e!Rx2rglep`#z#6p)eCtl;-=CrR##;C+4Iiojb0HVj))iz@fNoQC4ynz{=|SI+>Ejj zK4i&}B@t-d)6Fp|7jL@=(HPAkRP*yzUg-yD6B!MBAIFYes^!c?>^>kuJMrV7Cr03n zf92{s&_y8lMU#g<=!coD?|BYd4m2s>-t=LsG6ljNFz|4a3B)o(9Gks1QE74(s|_>7 z)0Eok=a3Gu!JVpkmjX$F5R%6AvkCWKC?Fs5{>C993S5-NitlH((W zVn&gO4_tItJH+;%KBuo}GWkZCKB7%uQP3eb{77`Z(Ts2;+BR(EK@{gpgNZU`5Q&5% zzZ|-RXW7NE2_>ia1k!BQ5aPh`ghD8zI#{T0Nc*fA$fim)r}?Q>qHp<>n_$}tx-7hk z*OyF}3dNfnpMhm<6tO?1`YHSB)I?@6W!;B zz=LhLP!i8-{wUE)Dyhhq6(Q!Pq@XaThQ4(CU3i9+6{VuXRnQdjri{SlaGL&PTdpc& z&DB2r^=!+wH8|sE%#c0D%`lU1o%+2VSe=$`K08W#g}Hl@k%V_hgk(!2Fo@g)VcxQx z%(pBAm5DHLTj?h7Caoyn!Q)IC_`sM$L|ak3nWQ+>CqyjeY`wVB1j5dDP4J8z;dP-F zjT;8OG8NI~xg8e{iUx;YMiH@caX98L?Qro;?lQ-qK6*oX^fTx5!!F=4hFt_rqw(P9 zvG35OSo-4K%Eo$Fd&06JLe8#jOJmfIF!tL+vfp}gwmgC0aa@{WsXv})&s@UL`GyJ8 zIJWLYd%xBom0tQ=B8AitVTXbXpsgqw)}tsPSySmzLA7M41rG7NT++oe=3w*+KH8&I zz_E|&7Kwn10K;lrI*PYK$@Uk9NbIesT^dJj-%~M&5Suc4XStqRQm`` zLmNhggl8#Wn3RM28j-;V7jf2KZ04rDntZ) zGUOh(2_2Xb`2l^m8^+h>^EL>TL?Jx#gFIRo0OkHuN=rEqR*7+EKSOiaEd~b6LL-qG z3@S{T+)n>h7_2SVf$#)St8hIh@-~X^!XvT)Xd)l~H{eI)g`E_Ae~>BDj3M9TK3@#$ zPQH|5Vo{*pXyp4sN>VH0c3C`$2}Kw|@Sn?pSH_>dvEtuqMpBXO}R@NW5! zE(?l!E#h#c|fg!eF2Sm7z$j4rhqQ_^(%AV>6!Zu@m` zDch8pPMp5hjgrZE)3Zf@PeNm4dGV|Qo*;X^j5LPACI@E5taLiObos^d3_ zt7y(gLCK|$Xbq+&h44H>F#C#V;;{k`8FNBs5Rsv0Iz^d$FfYGCBl&W(0W#E00yc6TUEL{ zi(G(gS5ye<(`y(5yo$4?5R?jb)?+0W{hgDr->|n3h=yU50Ggv0Z+O(Py08#}MRHht zQTcNm&@gI-6M_!t9SS~!BrY?S9}AnArLiqJz7|0>ZMdOr@D3Qh&}rL`zIzr=ju?~3 zJ^d0=e7k%@(UM_pJP8+TAMj#DoTe@nd{U||cP-4!%FRpoJ|Ykuj&kz{~fjH!#lE;R*h3^Ee-yC6K z3cgXQzZBZI1wYre`Q%l|rPN7DyGJ=3XFKL`gO*o-^@g-GZfn=ea0y}US39jN3xOg* zz~rRLU^CjvKY)gUXbamu=c5H8^>Il0ajOxbC$X{NYh%oo(cSU}YZIJo2=hnG!i-3S ziZEG#kMuGGvmA+|TyPr3knDj{6+j?v3>)?IZo;08DWOXGf!KjX@`Le0JLif7jU>|% zr5HjKjEG|HMabq6mVa-y5=gwP>#1vVlPZsxbv}=TiARAcmC6^%-~X zGcez0$y|^TWXhE3Cc*;LCTLVq|_;Ur4E%GT>JK&NFqfZUJ z)40zmO;C&?GMI3z=#(BWi@d}<4s?P_Ddp@0uRMas6As`2Q`^}@f5AAkHYXm4{PbPfGGZQdK+*y- zR!$*F2CJoA8950TK&7*(Pb(wsQK?AdToijf+}lk9NZ_+*g|Z-Gnb`?lQy;QrP`YA` zZZKUdqLw3JV+>^i_ye(yesViVp;&8<15+#SGG}2h6P0UNp;@^6nehKZ)jbAx60Cv3 zk8L{}+u7K*wXtp6ww-K}jsLN2+qP}<*6umyeebPbP1RISSND9F4^z+6Pj`F2YaK!* zVuWx`z7h0RFw=t;)m{>~m#Pw>#0y!9;2}qwPHMkONqG-q&58 zz$ojDtPiaEBP8!3@4pL7s8IDC+t|J zg3A!*6V4?YC}!CTVk>+mQ?#_XjPba$LXqn|o)(5IenUjQ8F6h-3WN&nfEgy2e7N!G zPIuoGHZ48Ab{B}ahP`9`jxNGg>W)=}ngIvvO3)v4NrJC+G)^pE3KeLsKD_F*fY}M8 z#b(L}1m~@(pl)$+H#sa(IIoJKsqvs;nU$27&|OG8#S@T$b@Z)}(0mjpd@+)AGuQA( zK`%H6+>X|aeN2N5>SzHxEhs=?5joaY+Y9BCsJts3V)=v1N)! z8I<(}Vk2`QW+KoFnBBY<-{#C@5`0EN$(?e>9sFU8{5TiBV3PO~#+)99k&@E58W2j% z47mVZZu2u#v>yO=nW#nD3EG(SCNn}#Be{C#+e*u1DL$-^pg3V^ZereLc;^p14-##` zh}D;^+rv>zc7&)w)tl(Jh~M8g^OJg>iFGFc*f4kxN(<^9HqVDN)-;NqH@E!lq*UBc z=r=0Phrd=Sli>kOitz~)%%eoe)cMY<4o=bxCM&`|k{0X~R&dmgI^37wf-A3opg5%f zZ)OmY1xY``BFTCKn*4();*N~8MK_>GKsr>b5y(p+RXd>1=l~oi1RFQngM=5(O2!1T zvwrF~KyYEGW&%r1p92!or!Xy+x1kZ)yA_Iy6pk-0{LBHV9PQ6+ao-eTWuz@9ED16f z8YQyeWvz5ca{H40pfD^Kgq_y8r+5~tQG=7Sr2sTFVlM|; zX;uNuuyA?MfpnI_AQ8{>JQL^|VNaImh){a}3}O#oo+eawI4eEwo+bzJpc1D5Du}HA zLVjp+R5Bd7L~w#U)*l7Ii8uWM(U*E1tqhoBZn*m(-faCSCis@vwQNM_Dy)50h_ywI zYSw+1KVx~#zf6*Oob6V3%H&9Vcr?&UmtK0*d&xoJxqAw9>M^5xciKXgNQGral@Km~ zaY4uhXZ1KUgHka+4h2fdcMAFq$Q1UOv3TI1S!tgTWFb{n0@o|L%3Kem*>gBl`R&RB zmaW${8I{O^jxq|xbhZq6c)qQ283xIQDN)$--U4i!#~AWnm00y6kEBheo=mzHrWQ@L zCNJ$xP>Iyj3(A$a!pcD!(S#P^$~7S@@iZp;dJsu?{soxMUJER7Sg}b^XHEuHy3BwU>Tdh;<3( zWS|?p>aYttwEya_62(C9zS!62h~(Rd3;T)4pH1Z%{M2C0w9C8u`D@3g`Go%dq)SU@ zJK}0E2e(b|nLqC6{#JjEx9fY?!~XXZL7>||&(eWzDPeAl1Y4hqJD=6{x8`*#ApJH3 zUl67Hu6K;(>-G^pa{u|_A<{i2(rpfP?@2Hc_;~*FYR~4YEn_CGu*c6}8bm~O8+`MZ zS>TN<@YVtFRs*EFmn+b>Es!i%?uIUN2aPT2Wp<4erv+XwcQ1FZSKO`-S4NL{?o@e= z4t@jcw^=OrMjITPQ^lTR*5U(-B1~T7#n16JIMb+xx&UVPs*k5Hi)4BkMD{AhBQB@T zkz~{A)GQCVsSv9An@TsKAgFO%kct5(Z3l;xB+huk|9uQBPXR7`b8I!0}q9cK_H z_!(A>B;Jv%HJ`2C8pU5 z=(_sdaV7d5M4<>*TM6g5kU9UBuUG#$EU9;8`{c8yh5PN zm!xWFvgHC@d@#Q~Y^qM6)UHCajU-;DnrsP)DApYk$S)Q=yhkY2LKGbQQVjfj#_NYN z#Uxj2)iy1_t-T8xXbV$lS2|MHSLL{1G%F-A`Fehy-(@(kB0u-90IfryU*|GUp{)b$ zQdMfKBn529nKv4N|L8DT+NVxh0`#c`6PwIIJ%2B>#&{xv9z2h;MQ+JaM6Eu2`Kq8A zkXn=fSJj|tPOs@IFk5$40rqp+s3B`Rw)O*>WS>c&N#Egp@8Md`lEg;way*5fu>eh`jpelK@QSvuyo|M&;648j}wrvk7Pq8Id-ZF!<2VGCLWuV2OrxlOtYnyp)=|I$))ZScq4{-L|6y<})s?l^?67 zL?%~Bb6iJxXC`UNp#+0Y`|xdXC$_wY1ZCEJ!8wTk);veKB@T(u^YM zh1YqUF;+Sw;dhH$M{R9v%bD#DZVH1bbqQ;EzevwWI;)p!sk1<<`qCk}1UCvSMM`m( z1jPng$1y5Nz4v=F`L;ZC*a^7_ zxd}b92t8wFVU(csy2?gr>D5=rOvRNxWn+|N%10^RGquycXY+;hX|Y^-@Z5WRqRuJUE8q~t+SwX zay_=)qhm*y1f;N_G{aW-60KPVEYgW+O-b-{_KfAXC@qcU=5wfa51EZCWNQznxc}K? ziEZQo-7oT2S~SG)*c>BoTJ*%YhKw3CXNwT9NZ*nJA^l>LQa5k;?F(~H^!q4iy`Ms) z>*1~r-cxrlmu}rD?D^6&=Gpp}EALQlrHR{Q+x97$W-PM0gfsWkB`#^zxB!imGdk(e z0?W2`AwIUU=c8swKf?3(g?Z~%_ZM6uAwj?D)2BI8!6yG`G%-u$L@6x$9~f>~)hc5> zpc}SzWfkWAOrs{oKS)_c7u=fNT~tJOJVaM=fVC{7Ue@}KqzI`}NVn-Q0C8aoLe z=|3GZe6(JpW8UG*EuPY~zW4c-d==|!MFU}FX)-Kdv@1DxUn>Ny~6uVd2!+{3KiksnOR8&Mm?O^9&Xc$LdZCm+DTm zS-qPT%eM%jYwqqmG%taT5X^y+RzE6t0n0el(K~Z{s_0@jPe2W8Nhr7#PmQdB^4)9d zAqa{X8o$CA$2rU!5o(Vu^%}XrXikv?^-G1JuH@6>Rdg{-E8VJs?((|V5^c-RLe#p8 zwIAu*;Gab4571NtOV=&*>nJxPjx8t4_ zU#7>@i_`!|C$?&nulxF4FE{;JZkQFeiAj+QvP$gJ;+s5jOQli|S9CLsXz?!SHNlN* zawOJcd~LdRK1%qlL{h zt2@KA3bf6+v0HJ3;_0oPs;TRb`;lq{0_a$ihTcPW8<2Sm=e-v=y7vFl&LCS8BpxMvy;7_U5-`AGzVq zmmLBRS9PESJMIaC)irUGyCD@cP_^>SvjjSzq#hM!K8)j2Oxen#( zxrDinq>W2Sw~9HG8*~kSj3^AXq;z*;8D;<}k{0P3L}7hT|5GE>h|MR!@ew!@T8&n# zrr{A#9RN<=5t8N9k+YxHw`i|B08@mkg@lG!fLa@H9puRfU661?>W-m}i|dUWPvaZ< zdl$d)tx|jriUHR!fISrhM3Z5%gij%wi{_zp z;8ilRZ3^l|gItLS8wnKqF13(Mr=V=B;1H6vY62N`4pE<*z_;IogfdT=_Pw!LM=rpMJav4Qn7Qh+9<|Q!{v;18Jr`oTXX1uSs6aD zbM3<=3o(wCPI1C-56-uY`Z6r4Lw>h3Dp*2ls??2ch3P^XYV>5HXT${7RZ#0vAF{og z*Sa0>nC_^-@wFs}Wgug$2GNbw>O8Exb6q$c)n$;iGW7bdccWTf`QSN$A6PI?Q)hTS z>0m?C9*Lh*&q`FwMxM8?@tr^PQ8bgg|0gYBcL^1O=%sHK}#XMr2_21OrT)N2n+6$35?L%+5` zr8mg4bJSP%;mWlW>*SnWnond?q?NXQ9kM_(^hOolCynwHK%r)c8x!RG@o@GmQ6}me zP#l~OEvgDZ$eP1>YE8)J(zx=@a-}@7)T$bFI8FWAcy=-*+%>mz-reWD7 zM(|#{qblck?f1~_Ow2q+%-82M0s6R zA1Gs46Vl;#k?xQTPZ!Iuc=tBz#+TmVa?Rf6k8ldOU+4UYIu2a8K&&)^f784`#-|vl zW~kr$5hkCym^Ek_XMQQtN~VC`jb)s);10l0nCLevzz2ET9~L3}t9Gs+TCa0bs+Yg+ z*QhBkVU`O;>5^SiBd!V}%vbFIljg3uK(4^Qbt9sSzMdes5a0I0*(_T7r3N=~WSCfQ z{XP`!TC)nv*QWEx+i606A}{|_0{xjow9O9t3A}k8rL}Zq7CXp)JdU7tTNYl#qZPXfm#VVihzJ?+x>|w zQ+z}lb6&yzW~x5xNwKo<&44L0rdUzm%rf^3I=wPe+;x_xbU&xdC3Joi%-oC%U`gbH zB<6QS1lPX$oE&D6VK~oByy;*IYE4{dQfO0OKBC_w@w0(3U;S{*?)tYF6_ibKA2OD4 z*11bDtJhEYZe;e4L+zkPF!D@;=~|38dW;ExnG2@XIJJZ!Ss7+(_SVI{4jo6m_Mod? zZ-i8D#!%pVo;5%2si5spDFn#+ZY$Db0!*o;Y1OmTG!>q$O2)s$LYUZsiPuhGCNU|1<*LBuu?O1O%Mz8UoIGM?|Ur;p(~8gr=m%x@*Kw3WDj zuIpB-X|&xuwB0uMJTcWK!+!gI7(J(dg-u7T0!1VW%o;~FR4OgVx3jNp=B$fPFDG-(%LkK0&d|jwVmwPp zQc*>N=X!*EHAJopKX~=C^jaJ?-m0=agOzeFPa=pI{){=hFXV(W$>XvMsv0m&U{i&uV04yie}E8)la&X<9GaQq43@N*Qh2V(g9 zUvE7=ttpasyKV7QsIliLX)XYC$);-Y#GK|oiG7|vyjoEr8MNQD^_yOuQlShB&DZDg zlva+Pz4r6xILBmgkDI^NebRoExn2GA0B4Yfupe*GiF7zAbNgRV}4gN5=H&CTjNedLnm==To8jcB}K++kqK4?R%QlQ_Nb+P zBbGh}5Jp4PJ|)3~))W!}J$U9HhCBbnQ4k;Pc)#Bph@wTVMP(CI_Mxy$;`!{ENNw2S6_ zc@cTYNNpn`?j9|9C14NKVA0eOIq5CbSBTwI=@lBt8MXIZ0 zj=Al>{ua3v4zvig2t2(HJ^j4n#i^F8Ico~(?_VZGs<vU9bZ zM_jXW}YXM+-*Mx{=8U-8!j?Erzj3Og1x(@Sd(9?Vb$v zQ$J6XM!VWU;en3`5tKpnBv7f7yq?xYcFL<8~h(qb5W-&wWpw@F~}+YX;6 z$)ww%s>n@wvhrs`Fho>3`ItH_-VvH!6luNk^GDijNw;`+!2JZacC|L;VMj`9 zOk0S2w5ps%S@ur^2`1(U2Qg`FLm)~$jg9Vh89F5oY|fNftiqWDoT0HHz9YP^VePW; zER8G)O-hu66ee~8R%2XPTnF4Ni3o{X30TSnoE;pd9Kno-gn{=Mg9K)KwmtjK=oV)t z?*!A@BZV)uJqoi!?{a^=z2T#3C??7RIeG$tgMN1%? zfgvTB6c$*8VUb}F@lX>OcAZ8#R>h%4FF&f$pTL{lLg$3!8RD5-0)!0ogkyV>hd<%4 zN}j36VZQ-W{kn$mfKhD^PVoc_iY7X2%)+yfQ-zR{9n@BZ~aVn361E%2YK6nn=(YPee<$2ZQSvXu8shB@j({DR)Kn%|NxY z5z1~0{z@LS8X`1tN?R;;u0TbwPrf>G*B?SyNO)m6;b3rBC*T|eMpt>ky(SQ$@L{IG zvtT;ZY&Y!MX-$m{39)nyVA5Imn!pe=&X5?zxj-)==Twm4@LGU|!hrIH0Wer-82)HnX#UtxIzzzn z23UexXIQvu1JvFEJIX7O5Pto7?kI59FDUMjVF>v7(UQZ+dL*zxV>s~sCwTkmh>EK!Q=o|v75LHVL8wwS^};2gwSPn| zNJ#OLIkLe$f>_b}h=g>pSP(Kpdd&sNgN5Klm!$P6F!x`A93X&T_gU~0KyVQVK#e3b zvDWwo`N1S&Nub_<_b7veA=9{jNEpb?s}&PVE6{k=fDrE{%sJ|d(dpZ_&Ztc6GJ=69 zXWq6cg2#dRJN!x$4F+_8sEYouhz$Y}OY)aCCQAML&KnBrTz%)1u{ZPfoK7_ z!NigOaWA3pqXybP01Q?*ndEubEan2zG6cHi#9jm1-0Njx7m|{wCypRfvS98H2w;(Y zF~M*az($WpaK!^GD(rm0=TWZ_jVZq@zCs+jL2_fPu@*6=+ zq4J6BGI&B`W;0SPSSm3O8+2s@!M5SvOLLp??btfdv;GIuTj&;YPZr#*7zh3d3XYfa zd-G6MH`Uae$9<`2#GqG0)~MTvaVJ9m@G8i9tJ3v{BU2u{5Jj%Io8#snijynGys+Cz zw6`zMH&M^K+uJW`Z{Ex}I|?So6?2z2RpA_Qy9~JcbxcK=5M#E*$=olLwVc$%DGTh- z`++|}GSG*UYg8hd5Ej^lCJ&0^E8s;<@X4v!ZS>7NkeIJ@1=zh*|C}E7oeDxoR+?I# zp()Rz$38*9a*9J@pek`O(9Q>k{Pj85_kLT51KKRZyLy#OHDR+NlLkwGo%6dGbt?6P zSffnG$*5^T{u0b0!Cp0i$XL|XGFq~5ihhv6z7pQ=u2rojbRWySHYv>FoB96P^QM*97LDFkx`zjc-?V>MDar z5UY5`1Squxn0^`-1Jij?3D*dwn*K&iG3m>W$5s`r-x@<5&1zI;;I^?C7BUDAu@=QC zWwL|lG9nvC8?hM5#$iI@M9pdjLq)^R#1>4Wb7Lchu|-j=#bY%RMc6Xgy&e$Y7^vok zqZxe%7%hnNHbx=*c;o|=$h+bp5h2?8UGU^Iy__-FEL(k;9ur_-SE1F9QsQHF^6G1? z)^a}X4T4q?1Gh?h#S-Zw*D1!on*5>$&ZHNn3soW#`Zz2aU_V12SmvfB_|E~pO{h4{ z7YDvSSnmcLyU}rSTn<3~ZW2SL(W#=bS%>&xs{t-t+t+r`iqt6EF{zp6PxuA$C`%Lq z+Agk1-=G@%5danq7R?JWq!~7vO1GHFDI=9J0{0-1GSA|IfQYY!dm@?^7MZ=6u?ol( zx5k1Ese(a<&OX51CDliV&c>%TF68-mqOIewfh<{f2)XyOmnx6qP{unJWe!UsA%T{N zLA?eOLfMFO$})+rkCmmhm!f^HJ>tk1AHw(&E)!Lca5T89jpAiNmc!fQn3dMJB3_yU zt}}s*6>~PGHm!2}bLV}aBKK9cBp)pD&jPp5S|Z=t$gN^P_}Ca8A$A%C=`annBu61K zpw6ko=Lczq+yi(U)cqxy+|pA(sy`c#J(_w;1^5(itcV68sITWktuQnX8kC4eWxrjB zaiK#XK*@lM8Yv#7cJ@YcU~3SjOK=}e6}dP7axW5p2@G=!s>=sHrL3gj!&_xwMyl6N zlFf?PT}zH*|Ho_K@aO@tT?B)|EWicb>iLe(*O;Z=UDm`}8H43em$Hot)s(uS(og`A zgOSE)kvtRg$Xj%rO4BCDox3rg?Uj=l*rpl}evg*_!N+!8!d#P#``*L$2zDZE1}jAJ za##6{#YwqAN8YkT;+FE0VW)e7N~wR@gsK;!`IruIq(Wsg%nwZ0$EDU zbM`1n+sETYhi|%{v*~Np=^If2pdh_B;rVSTE9G^fQL1%WrdZjKtizt#P}Z302+i7s zv;OD({dT{@Q-cs}!U}gdjRadGUDm`kLakLIR5Be+6|1x22!Xmms}2fhAzhX3WKahq zoN==6(2BMbaNH;MV$mEtpQW;{cE}e#2~(sx?$3xdy~e!DX`5=e$v(@)03ydl6Rjv_ zeoe<-9FLtplLeEhSpM6`Nd!qu=9kZuza2uX`iSu#o0`n6uyhEu6_nEoC_f|7x}&@?GS&fp zL*y&-V{pvY{@Y|UsGF$m!Ai`G>f5H8`d?cuxlhAB4!qU2?5bNAss1jgH?yj)S<6~I z6bL6Iq|0KAWQ~2Q;c6%SD0GESQ-K+WIXABrv}x{TX)7dj28afT2J#Fw$0m8j#5Atl z3qR(7hM=qj#~qexDso9s`g^bZ9bZOOnZA=U$mYytct{J03W*9&Vdt!CHBGWwoBc7- z&*0YEido88#;~iSS+tq|w2YUtjA>Ci%y9(&P*pLzP|9);jiy@Z{5iJi&ep*nd#fAJ z=&{dBNKnPTInPC~?pVzn;FQ`fTi6QKuR{sEKZMgCjIPbfz99$_d*N!xYS`e*q#2jj z8O}9%B-JWnKuLpAmDsNv3Pi2bT(Bs83Y5miR0oyDFpZ{Ktv-n+d)fnxpZ)-@+QuT7 zh6=^T0>UKy1)mv#ihI0z9jN1$$&tdCZjFq;8I||DOAC?zWS}2krhagTSL?A>Ul=}q< zMpI|m3@SUqtKd(Xx8*+As>XwfY&_w(3lvz9HinM&qc{yFUMbm2KtWhxo}`r1P((p( z0)HDH1x-rQ#Qt9BT$Jq^=mGnR^(xp5K4}Bv0h?hH#<+ZPM2Dz%i^`1>ej{@ho33zw ziz!#09bM~=QD$$ES&Ulx3q(jqNJnT>TJ1&#eMQCdlsUC9Nyy8lbxzlWPEwl^8lBsq z;c=prc;>{!T|4Wlj|hzQugid9wmt{6OF7%^j7(^oh6EU~?8J?g4z@pZl&ml5n=yKms+%>sIZSEKc#+QD`ky8B6JUZkd59Im=vw zTY_7Bgf_MWWoycNt02~~ojFE}5;?bX7DBy>c=mNAbRISh94-BEwsme2^Fs4N^NNSZ zbWE7Pn`pyzaH242uzD+@GEeLlBeiWb$5UPC-R$UdRFxhE}uUjKGDRmr?S=T-8lr5)=T4(h@J*Rf8tr%zcG;-dR zor9&fspO~UG+hO5!8zTv>m(Mk`kdADORc z3QY*E`V^|X@@woqc^a+d1jkR4k3sVw_HyXr5J64DaV z66?~1HlYBmy>?l9)mNCfvV~uti{zRaJXPga1(Lyl|M2Nwq|2a;u~${29i)sN-z`6; zj{{63cBk0?^=RnO$fqS&L*Al^zAHc9!2DKDb>!gE)QH(j{g36gzKgVw9f^yoicEAW zeRnesOUMJz%$3AA1Z*S?2LF~KGG{ebGA-&=h z)@)C`9(&H}T=KskFf#KbJYi4ij`NyXgNEK}>{-l>$@pl%+*IAYR<|r3MbJZ zG*m0X0c`E$YG}TP-C7k>Q-qL7ci@DrZT|}DzV9r3X3r0g#f2;~uO+Z#v3wA)I*m35 zhxiuX1RO~YS5zyu6eXsqUP4FzNX(MDl{T~SGi=;|FzS*rC-SGn zC&b?d#@CwEMVO$shNT`lAp7 zhys$gG9#tC8uz?8FYo3$P#u+NKs-P^KwiJQJDo0V)>oEp^oTwzAaM)LmNsBn1u{W+ z2Q6xx5}Q^yw(-OrT*@^94ju|WAH71`(uHFsGz;;id<--&l~BT>=IWO!)@5MtBn#X? z>xz^FTtVjQ<58@z6ZREtL6jRaA!QMoSjR|!HT|DN1lQr0Qn`W>dMtUx!Fyn8Kq`RF zw;?O&L}5OAGAWfxgHpgCHGtLp(e*#y{Tqv9@(DGR;ghVYfM|ME?a{Tt)xY_)ZK&|D zM)iM&MA!EK{p|^^A&*vZ`Vee8*Q7$sqsBc{khP6Bo=u%SeJEmz-$%GkX^ALGBX z{R&E{&)4^HXN1)U|2xxTNAH?a?Tth)?Z5GR7w?ZOr3Dv_!0Z42!RZ#89kQ*Mx@Cpb z@t?Yy7HWKVZ~G*_PJQ;V-kt(gOekzz*(`1Po@;TYeL7;N(2ScnTa{)sfrp?P*t6o^&|!pi%uhNygTqcX4aN`|#1?b}Ahb-#H=UsZi9{ zv}--H?aHHTJ5wM=AUGh<9%Z1LyQih^%rpS24`MUaseO^Xa{5XJN~_Y|W^bC(oI7_Y zU~$tlKEckr{?qi9b7YC-LYJOpy6}CueQfCrVBy5da2L0MbvRpqW#o5oV3#GA-Kn3e zt*}5}YJ)~XMg7dEh`pqxq7o?;^P{YP61k1E=#M@^)IcGCnK?|fp&9lqGTbwK#EK-< zq3MY(n0l0_Y7FGOnZNW;FS~Avft&X;=&s&NNX6sMY1EMlYxB za9!{YMyMcI5ER7hD3-`RnZTG@BbnAhDqm^SiJ)cF;yj@VV|Z5;r%IaABA*mRWF3ZN z2Bg%PLV>xkQjyLBdnFD^b_3(zURE`T%&*lcQXgP4OG8s*WM!>$E3>4aWF zod)yMre0TJ{C7gsz{^7ncy{ia(6B`o#A&zfNb036zqL;eKHbV|QvgqgefxDKWdA#0 z(d*kt+xLaBJ2yp1%WU^43o!k4quaCm8dgL&-rU|8%+Ag4qxq51{k!s*y}6ZQ%(~f& zVt-gqeXfC?=9iYkYs3=h<)X0|rqFHq7IRP?%i!QW=9iLBCPS**yk+Yjn5|WBN+=S! zNxT-Gv+=`W`QM+MsaeZ#2_s z#5JK!avY}X?o^{3t7)_EE11NjtVUj-jgRtaV(Mj>;)w2ke_j+oD+)$46R0t-EN(%Y zsuZvMEQ>aaWX_<8=|^K08KF|J}U`v}k;%6Xc&4|7p6nfqL9@a#)D1xz8x zFSRpZpr?+%4Hg7$R>yx8c@r!eD){_du2--W8WDn>Q!_Vd4NwYF3R23dQ93`^QzSdE z(LM;-txAc?e9$od;Lxi~1$;CgT7E6xJzZJW{+>{G`)1O5UG?>$D1|8xEYCYLQg|%& zwbd%G3q;9f({+GNorkocR!9j!dxD#kMKS)&7RQnslK=pT`6KIR(_d{H!}uq>r9QMowAduYTt?HWh9z&gl+9n_h8|V}7BcBpFK?FC1;0kZ8+J8Y#Sg zjx`ZG+v(fEPRnS>IM_{P8Fl*RGo|974K{Nj|L_7$`goT#Q92m&Ynqa*&vr_qv30VQ zvHj@Ywha6&vZkG3f1dtCvYUFpBg?d5YMu9N6wa`HPO@2Z*C{l|tR}&>Bt9lUDEN=8 zA|d?krm_JlPqLWE*+_;4{;3y$?{Ldn#ih6Ns`&-+@hm7>>97;7T;fpBO0qiG@$O7qsVc=!q_w{F zLHy=jbw;%CPRZtwEt>RrEydR75YkBl*@oackUSt?vX?N0tQNDw0wT3F&3kgYQ0Bmn z`LS&F&^J8AV0hmznikRLw_xqB3~t+232IO9OTF8NRe}P2E(3=}R6OEl0{BTg_eMA$ zdPw?O=lOZ!IbmAjyCqqYZxZX|1ov!LRbY>(wJ^VgtanNopAvmu+!NjT6$O>)U2peL3o(+B4jB zm%y{QOotk`JJ>gf?M)G2gV(*9(}ZUn#>Q&hj{d4G?!m7hEjDuen8Bs$yit7S= z`cwX?#egMihE98{8MCU8ufp&5=XZC#-wQj_B`xL#<99!c*i60kZwzFMc7wlOL^F@y*W{$jEr%bza(nc3}JXX);R?i<9plvmY|n3c!VGK=86X4;wN;pijj^I6F=+W(va5BM=IvTqOXD|Mg0s^vW78$3wC;T==8T++gqB)i5^z#X7l>K7XAQ0-wpI3hxY~`M{RPrSvxfZ|F zU_*jzBw=q#l87c10X_5GBkZOD`q!b$(un>!_|gjFJeXAvPK_ybMFt(lz5F?;=|KER zDP-t^?1Z~?5h%;RDD+U{PbDZ*lBm#xX5(PyInKp&S!n6?RTYT?ucd4$TlvSlfM@jM z>z^fu>bYGMRa!hg>|aVUc5iaxGTW_;OO8VsMq3KLJr|o%{)}E>YC&pQ>1tUJ8%aaX z-GNkE>6059?)|HzKUbHZ_6U3oiG=XwqC>D$_6Bm5bP5y0G#5qrMD$*g5KF3HrS~TT za6-URB%oNK{}~+;GfA9MOd1hlkG_sDPkcIGlGhp$O20kAnEA&H=``HrW~F?)rt&>6tKxyopa|+Xqrt|f@M~7ht)#X|w3i#-k#CJkdLW5&5O@&+xyA+b=0CI_=c>%H)@cK}A60vVN z(KB*gcsJUZbD9fJW4uXL{Ac_*{ymDY?@6*{dWw-!3c=8jtbap!P8dlWU-y*Xmi7HI zqrNKa@izi4(aA^Hl;0?e$Z^uj)^B(QTE2j7L8worsJaAn$;&xzmSGB6e;OOaEK&(*T9r}7f?+t$Eme^ z^2)PqlvFt}ja185tdxhWo;7S-Hy?SfPsYaSbGzN|#|pDckm zJ3jy!m`R`O_xy%l50ed1(raP=Ho?iekXtN*h-9F8kZ0PKT9JPUi|} z=Wn;uCjmp^_n?u%01>&y3qWL4z*EgX;T3ZqgeJekzfz5Bc=Tr8 zewrJd#*(5A#)ueLl{j=XvXDiAt5(!#=+|Zk4$i?w3zutnoQNj_ktic$fsf)~%n8W=Kiacb@-u{gx%N24o%t4u?dhusH`Jmi(5M?{v6 z4JDQ*F4kIZ^azBux_{k0GC&#TMWP2o|1;`YsROM(x(2vffU3UB1_#D} zTO%ABi}u(M9DFo91Pk-d5t!4gDV#irGSBgX31DuyAP>$se_1NGdHNl=jWckAxiw3< z-|wlmsrH#%f-+oB`*_t1ez0x&KG)!BQoDEGxHnP}RPBG>Gdx^^myP+ZUE3w&h6UjJ zRkmDosFm+TKqQ5hpDAJ%;CMR^-lj=v^Z2!Xyqw-|TBm$+>4gyiT_g+)Lf9(HQ!DfAIf3p*U6#F3-Xp2xtNt z=D#Nt13>}(JNex`-^Nx42?*%#g#`ozq+;%3V`yt&VeQ1={J&dzdt0*z1vzne7#tWN zARu^22@xeAAmG1e#y3KN009B<>o*Al0U?n}iU_K>XJ6*<{FZO)16L45mY~|0(V&uZ#U`dy!s8+)rhXfdZ{Wn?QU=3>qM|3x`CXP!8joTG z3*Fn^!kz75=iTrA<;~5)&GJ4u^-QyKIdygM(s`Zr`ZY1(>)_}(ha^(zJ?jPvRUj_` z99~WY{-Zn;&A${!38=vGZ$mA0&)FHZ_wm(Ll_NYiYKbCP0xSTeMZzf`#cp2fwd13t z_Wn%|#u=#+9mSoIvgh%9Sq#0~h}eLIL#ziGNfB)pAc5 zK8=GS1*YaX^!&j`O5us2quQLv5uO}IP z4|jJ;!m8?ONIm)FKtbU6Xjx{e%!Za0jtrTXV?;te%A(~e9c6moha+hRj+`%!?{8xM zk9#}*i_1$OdemrH$O85Vm#u2O?k>OYuFF7xdNs9K*Y_8DQ&h>k-)GlcJ}VLr zh*-B4-WEaEWji!kKuMB|s((sKO5r8aK`mKqZ0rs!1_nm-G5_cN#4dv``@0)g4qHQc zfy@~`aBN)MYWJAukF1U60*(O@sur%^K0Q1LyKM+f+KzGd+wqwuBp;pWq@@L&H|7}-2NCb<2}4e|D?Iz6aV3j)bAbl`lBCLU z$oOtIK9Cnfw71g-@jGCcB6{%n)s+?Zj~9pJ@~_)rz3)?#qW;?}!3{L8UT~i;D|)9lygd^q0V$8kAWE7b+l>;iw*AY#1bI`&%$V zDAMG7Akr2Q;xPrFKnXA^AjO|9RQ@%8)YR12*w_w;Uy&#W^^!9}CPzQ*#O^Zex-A*{ zgt1}bN1zGfh8?NWi0HMX6rI%iqS*_m$0jz#@3cn~Xio%?A;uK^PvmM82ehIB?Isik z5bw`{&iAJyTa=M= zRyh{-<2yJ8va_?}PFDu>Js}&e_2o7;Hlo|xnHEU%S6Bl{6n-09EzQ$eMKhP9O()1$h??4avwYnsAgqVs{hJzjTN#R0S#sdS%6|TvStSv z2czz2_+C)U&Sk<8%9&6zv+*FW8j&&0w7Q? zrf8wG|5KTDMm4!@+Yov)1dt-VBOqO*NeAh@sk9gZgbtx2(jr}2=tV*kP^!`dK>`X2 z2uQC=hoJN#-8VS*`8=NEx$oXN#`h;*=3INP{f#x&T>J6$CJ;pfJ(A-ZiEwopcg@$* zRRm4UW4w<#QfS}L7Y{UYWAE{CvQF~@047%|fmV@?=4Vn%&P1!pnW zva7AOA{+KRSjzWXs0fYP;(d2XIrUKc|J+e!~SXD=7Rt|ZWegP2hwB!v}X z98=DcS$Pna6sJWoPw?xv=6CS?9(+(>X-i|7Jvt4Af#%tZ@?wQ#sDTx{YK3Jno~;&4 zZ9|3Lo?(w9nj|$GZB)RwJ+bp zERY~mp(RqWu!ME%{$^@;3Cd*~C!n{X#IN@7(|&qy^s8)4M_k!)b zJ;gz58fNZ9IkB3aB8wpQ+}P=N?dyW8?qc)ZPBGtbDX?4m0Jc>nDrc0pUgrK@N$y!p zEj&cvcxxJ4*uxNf0y&ubvT1h!`rdnV@JuYSez%+LStN@Ua`G$_Ahi)hNZuYx!82ah zV?Wx{&pp8046D4>du~ufLh(FSsOvd#QF(R!wMGI#s@6A$u#=(e0be8--$0G}-6|fx zoI-}`uxgaXaX5x*5O5j2kOLX|U}c0m8(Vk?02I+-+o(pJ7;q?i={v=467(O;t5~N% zfM6AN^kvA&$%&Sh7RH2OlUxf~%peGFUhWPxMT!v1NlLC7Hdo?8qJ?F-Il%a`ub)H` z*S>mHo&!-<>FVN_3XQJ=7l)MGBHhNxnEuj@Ml5uapF8UyZAAKWfR&Hyl?`h?ax)>PF%Ta(^hUW2n%O?E-->8_=y}-Nyo;+JJJftW^J_39{ z|FR#S>g5(~mMg^%7xsRBja&Z~lki}n|G;qzKW$rN{65lz@)?&TKL(epJ?d#FwG9N} z`8|6p{Z;Z+?O8%Y0QZ?Mi?{#D=Tcp!(%mXSY7mwBgQXh`j3H(nYsS}C;-oM|xW%(Y z0E@ks13mU{{HGpy#=YiaV%5ZG;2@)Vo^OZ(-%uV8DwE^gX)jcevc42=zeXe?%!+Tr z>~1eFN$BL{bk95E^rg@{99|-IoQ$N7)0eaA4i7!sMiO#n!yH6HXIUuUCo6qgDomCl z!v)VN(P!F>!8q z^n$F^tyj*+pO``-jB6BtX2_*(P>ya10!~T5lffH*DAaArP4qs}YIlWQBAV6NFIz#D zVlVP_Lu32Su)$fiuc?{Y9I215?`|z}HNB0Oq8eYhPf7`sc%LeVO(m3wJwt)VZ^Lyy z2KW*M$tHk4s`1IC>x=-a=QO$VG)^aKt+`wY@iR_oVJ4`CsEph3T`ldwD{!F97tT@1 zptWkQol2HNT(Rl9uCebCJ4VH#)e zXhj`vl#K5NUF^oEmcY4o{Pg8~{C>%=B>+LOxs+bBAt}lBbi-r%(@vgY9Fhi(tL)A+ z`!caSvXye$#N$&~Rx$TNiD&{!>L@MS0X3*w;gKMg9Z&#c#IrXgdU?7DH;NUVo%CAO zy68r*T*Kidw@7`%dUTK;gZLsHt@KCDsU2wTtjqK;?tv9>2~d>M>tLbk?h zuVpj$;+RI!&~Hlt30BHTIZ%fN@}-4^a3r2~ z?CQ-NX&7gFK^0LCifG7#Mc(bxtBGEirBGAQ$w9^42C{GN`JX4VAJIX>85D$yN1|Eq z2Mf<y_Y&q{hjF%B@ZJ>2rk|kO%_U*mX|3PY zO!B<>wAO>3zuiz0lmBSVnE2@fGcqrX5gWijf+n9bNcUf_TJS`SxD1y+>Q%|8h}|g; z9w+OL?yR+D{qVDuaw#FJKF&R^WDDk$(n%{yh$sT# zApTvLH0y)BvX+2zuLS2c7d!<~a4E4*7>-5S?Kw>SIlm2C2713vdJw@gc=Y}gsFyP@ zXnB*~{r*&l9MJCr!8-mhYHqeus>!4Kx~EUst?SiimYOu$Ib=?Jj0s__EqP06JuM4X zLnY~{5=5+Q=cJ;}O5yn@W8TeO0_MH$6dJ+2)c7%N#aiiBEde9G{C#Kvj!x*Q4aYQb z{8W6e9+n@>T?o3qlR>f5Bm!sDp9VY_S0z`m8%x_rHK4RQZc%3pdD0Uj%&FLBQ(ZxX z+V%{S=io+WEi3sJY1x^*Caa?&Q{Zop<2?X<#cMvKv(nun`>rjFDdPvHIjCl&gA$7f zrTEWmx0{XB*ahCIpg&PT@CYc?LJK%kC9&FSH?7(Fpixr+`ol58oR<`+o&W@sG5A`x ztpR$W6SFQ<*T`PzQJ-{uB#tOl_@vI?z9qE7H$uEz{TVJx*r;~oofIXPGZ(*~vN2Uq zUQw|P+MAynCgTAw*IkRzwp)^IW+@e=-=I!DamM2Ve8%HTV3{ThR2|pgSZ=H7C+{A) z8o;fk7m6hog?5uo6;vzW&zkD5(M6R8zvFMG5y-=(V1Q4_9}b0H2&?FnfOL1 zFEEM}j6cr+1-s($wVj`z4-OhmKUpmb#EZBLNh(xOp~%gd4G#}DU$364#I*{gU5plv z0m3-kRq0S#*?U4lLb3hM=tR&FeLqc;#8rVqUi&Xu0${cykLUsg^{_Klis$TcXU#1w z>xMF@2ZGzD9XmOl*5rdrI@DF-9S1c#F)t>94GI)gi$e{-jqGH?58~QC=tX#cFT+Y{ z&~lM*7Kdh2j^ZsqB%_7Hh*u`mpB~;?UP4YZiH}jr2rd{1G6=(W|4pnOmdw5TuyXLj$ST zarjf}(iemk<^kfC^{Q|#%}e8?3$fX@t>+t9?Dh29kwJEN!sqY`$AM#&b|Oa}I)$h( zD0c=&GDb)9`c@Vp+myUz(~%(m>yVjzxS+lS@D_mf_ z%*SGh-57~bX>jRPsOoqKv2WNyJ?;a|y~i@$G4+yVwS0vNEeRcATVV7Xl?<{9hc95V zdfpN;Dqb4(RxcpP4&R34X`byMM_wezMWw8W6el=~gP!mG5m(7!vFK@Q>!D7BQOs;d zQ7RtvnY^Z$sB!YHmhQF|gaP?QG?Yjw^v{0#ZwCCAAI?FgV4>izPs+J2rUx3Tx+;}Q zHevc&xOmk6H<)2-IscbDgcroi8*vZf`EPiPzgwrLo4qFt@&7O{CN_Uf z&&5jreZ1eXE>iN7MMOmO$8_k&FH?bg4D9jn=3Gs6C6oPttdcs`21pQo`aRC=Y zrwh`>bm#*Ad`Q1rjGeO^#0&N#Wxrd@#T*LZ1DRv1^wbdM-Ltl7 z^lWM-BSBu4d)gl@YR}%Xc66rLpAY9{PZO*R$dfYTNKZupEGge8p>pq>aoA7;UH%;r)?{Kjpu{&dZtx(+-kOWZe{BFM2y_iN|Zr^ zM0#&?Re9?AL=xwhN|XVuWL-&D!I#CZoaP#L$gFU9nP8Ga+yd{n18%IH(`Ms&0s+*nYk4ap$E0roc+Rq*GHQRBupQ-2_bO3u>y?)PERP ztmzl{P61xllxQ$qO0rCLlZeILZ-!89?DK)+#B4Q(wlAY%>boZ;duWP9{wT+x%8*A+aZIM@Tj9*on zpK+L<^_jCt#_`aSoAY!ezS(AIla>whHt&jAK+;0z7EIhMCa7G$K9dHaaL%u9QLOd9 z@be>Cdk-E)QTfoxlfQd%jEHBtM9@Dmiv^bs_34L0gyY%?N+~!?>^+xP4tJIyMd|&Y zhs=YmR))_j*Hv>Ex?h@q#^0W?i1D6`nsks+8+oxG-*GFBR!7lEx}}lsF_%KZ!9YUi zt+|ywbpaB`8eWHJ)gd^YX|~_;%z9&b@aZW^hG*_fXFf9riVOcwgJw=p`|2^p|=iQ&^%w(8>g8kIhzblXYcLV~MNEPI1tX_3WL(%p)cU@mcN0n!cS_P(4tn>(dq7=rr@g71B9aV~@UITyHG>F8%@Q&B0~sT?XOq)k zJlbGS528A4-^M;pgm&Hos#5w%u(>r`z=Dr;e z_&rX&U}fS(98q4C<#R*!exk`vcD^{ixryXGk~-(q2*+ZUo&A=o04Xg|0q!`){wA69 zl<9C4jR=b?#58CD%f{#^VQzJ^4_XEShO(c)ZyiYk?;Q&|L~Z20MJl5OL=@J6r%?ig z##S=rnoJ(+`Ibw=mK`zo*4N&~Ru?XkBmy1nrfWteIJ~ z3~-*L6tNfUj6$qKjEG+6y`#*K#X}pqJk*ERhL;*&ffx@Gu%>D&PaMP6CDSj#^IWJB zgc(&-)LMr0UR}O4Cl~hOT~R5!QswQU1yComQ{#5oK#|he;-u%jVDpC+S+Yo@n6hEU z4_QF&#RIH#*A4A>`jyLJj<%b`gz-vh?9OG)6hL=;dAfxwX*Pl>?eQk4xFPd=TRql^ zdvFc1DlJNE<&{`gz7YaTaF>+H29(LxvJpc^>q%c=5}JS^z;FF9#>x1NK>gUyf#DEzfrAN zs!P=-yvtTCF?^L2&za<5fhI3O4|~3xBT3S+xk)&6M^k=HkzFg+puN;C9-9_v;IdSU z>o%%VW7ue80=doZ5_Lzj=8nKj!2*nAYsO{B1-W~0<5c70XJP2lnkeR>rd;>a9#>Ka z3`tb-6R~02Z%sB0udL^`jTI}fO4!UbtK<(RPJ~O#qenWtTiI1((jvp-O6#*Gv2QwU zF*ng$la?toDyCIM|NzrVo}DyDN($h4gR^Yhm>o@}9!pVXgszdd=Kj5o&p`B@318vD&?@1%o6? zAy&=Dacfm3mns$S%5++TET^C;N7B(R@Hu&E)81weL9%WYHjwTh?cW|3S{(sqk}f?d z*FI5W@)wtWp9D_+ekJ41>w9$LLSEi|#F+e?YM$wS;u*c9_s=aGI3$B)!qO+=pLC%x z2U=;9%x^sCTt5p3G6W7RC1#uWXtlR~Hl#KUi#WSkAz1Eqyj5iTyoj?s^3zy?z^ziD zaYlR>FM{?qjv_FHLuG;a)jM`&4A=K0Gs2Xa<&r1Rj^HGG!#FL+Vd`yiaTpm?~;woMCx`-9dWwQ=cAwjMn{a;$S7p zreZWFLeWxJLO*7tem)^vu}X0zB)D!dEEj@;opuTs#jov?%=6`qgu}u`GO`R?s`W%! zRn_ayz;uMc@061#@Kf;%XYkz$oQ0_8x##$){la6Y1Ks;S)CU0zE%=$lp1ZoIFcy(_bt0hDtyBgbeI&k~BqpyKC@ocx<5 z$*$s?FE-X|HOL52PcS#_TYsszH8yGK*0Xad(`r}Gtm-@HqTwrL_08l* zYuzpb0013TW5sJ4x@tl$5LdVz4Dm5?O-Kq`Nq$_5n53~%(V^~Y(<{{)N-ha z$$&=JgXJm9T%ME}74F+XPg?A64t?$KGv$eT@8>+h9}KySKFG!7!}1~%jvuZ2W_Ml{ zzsZ@Lj^;yTRKj~aRj?+a8U>z-oNz3z&5E~A2xss?PbN0&8lLlntI^D105o54*W;D< zw~0w)6Y224LWme;!!pR0iFNa;fV*sb(`FbdjA>_To_S9WmTBJrT64&rmxnE@Z ztoA*=Dxi*n@qKkY!ck`UcG)^Z>kPRd`%_gEt&idq-FU>chh2AsAMf+}k zEdeRlzQHQv!sqO>gfEz8O?n<;S%%7VBL1_IkL`sUH|ED!W`b}plKK>|)J2H{0DQu| zNGcAH8t~I8<>3mx;RgT!7tbH<_AhXOyTH8s-C=)at(A|q*x&&G7tgOO0{{Tv0`r1g z^wd9CKaTLT4K4vQJ^;YL1ptu!5@!q#06@UJyx^|(e{|!2W&Zi{ne-Q?5D5SPb#wLl zU)%75_)mA!Kk+f_pKq%Fs`8Ts_`^>Age~gt?&)vl^4E<2Bm@4imp_sJ8?ir|&Hq$q z;x{^fHlP2g4mjoSoBOjF{b!vEJNgqEztQ>Coc^=Yg}3{O@ZTu?G^_uq%FExV{4}rs zQu*=CUc~<4Krig;Pi+21px&QzG From 5c776e2392f8a6575934e5db287d09aff6b6cf1b Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Sun, 25 Jan 2015 22:31:11 +0100 Subject: [PATCH 606/652] Revert "FMUv1: Disable stack checking" This reverts commit e62c8d73678f87b9f6cab1ad3a33c8911277a8a8. --- nuttx-configs/px4fmu-v1/nsh/defconfig | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig index 539634e3da..a467fa605e 100644 --- a/nuttx-configs/px4fmu-v1/nsh/defconfig +++ b/nuttx-configs/px4fmu-v1/nsh/defconfig @@ -92,7 +92,7 @@ CONFIG_ARCH_HAVE_MPU=y # # CONFIG_ARMV7M_TOOLCHAIN_BUILDROOT is not set CONFIG_ARMV7M_TOOLCHAIN_GNU_EABI=y -CONFIG_ARMV7M_STACKCHECK=n +CONFIG_ARMV7M_STACKCHECK=y CONFIG_SERIAL_TERMIOS=y # From 18d756dd599f93abcd5dc89f323a5df77384ceac Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 26 Jan 2015 08:59:19 +0100 Subject: [PATCH 607/652] USB startup: Ensure that we are not talking to the peripheral too soon. Startup does not take longer due to smart rearrangement of launch calls --- ROMFS/px4fmu_common/init.d/rcS | 26 +++++--------------------- src/systemcmds/nshterm/module.mk | 2 +- src/systemcmds/nshterm/nshterm.c | 6 ++++++ 3 files changed, 12 insertions(+), 22 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 2c9387ff06..4d337171a9 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -5,6 +5,11 @@ # NOTE: COMMENT LINES ARE REMOVED BEFORE STORED IN ROMFS. # +# +# Start CDC/ACM serial driver +# +sercon + # # Default to auto-start mode. # @@ -43,29 +48,8 @@ else fi unset FRC -# if this is an APM build then there will be a rc.APM script -# from an EXTERNAL_SCRIPTS build option -if [ -f /etc/init.d/rc.APM ] -then - if sercon - then - echo "[i] USB interface connected" - fi - - echo "[i] Running rc.APM" - # if APM startup is successful then nsh will exit - sh /etc/init.d/rc.APM -fi - if [ $MODE == autostart ] then - echo "[i] AUTOSTART mode" - - # - # Start CDC/ACM serial driver - # - sercon - # Try to get an USB console nshterm /dev/ttyACM0 & diff --git a/src/systemcmds/nshterm/module.mk b/src/systemcmds/nshterm/module.mk index a12bc369e8..4e27105723 100644 --- a/src/systemcmds/nshterm/module.mk +++ b/src/systemcmds/nshterm/module.mk @@ -38,7 +38,7 @@ MODULE_COMMAND = nshterm SRCS = nshterm.c -MODULE_STACKSIZE = 1600 +MODULE_STACKSIZE = 1500 MAXOPTIMIZATION = -Os diff --git a/src/systemcmds/nshterm/nshterm.c b/src/systemcmds/nshterm/nshterm.c index ceaea35b63..50547a5626 100644 --- a/src/systemcmds/nshterm/nshterm.c +++ b/src/systemcmds/nshterm/nshterm.c @@ -50,6 +50,7 @@ #include #include #include +#include #include @@ -67,6 +68,11 @@ nshterm_main(int argc, char *argv[]) int armed_fd = orb_subscribe(ORB_ID(actuator_armed)); struct actuator_armed_s armed; + /* back off 800 ms to avoid running into the USB setup timing */ + while (hrt_absolute_time() < 800U * 1000U) { + usleep(50000); + } + /* try to bring up the console - stop doing so if the system gets armed */ while (true) { From 9cc94fcb2dde1a591c20e008ca59d1f876c2070a Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Mon, 26 Jan 2015 13:51:34 +0100 Subject: [PATCH 608/652] mc_pos_control: Protect against NaN and Inf setpoints --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) 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 3b631e2cea..962dc6d4a0 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -726,11 +726,18 @@ MulticopterPositionControl::control_auto(float dt) reset_alt_sp(); } + //Poll position setpoint bool updated; orb_check(_pos_sp_triplet_sub, &updated); - if (updated) { orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet); + + //Make sure that the position setpoint is valid + if (!isfinite(_pos_sp_triplet.current.lat) || + !isfinite(_pos_sp_triplet.current.lon) || + !isfinite(_pos_sp_triplet.current.alt)) { + _pos_sp_triplet.current.valid = false; + } } if (_pos_sp_triplet.current.valid) { From 9c627255ccc980270fe56b6c4ddeb494e1ce0f50 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Mon, 26 Jan 2015 14:22:36 +0100 Subject: [PATCH 609/652] MPU6000: Increase gyro offset tolerance to 7 dps --- src/drivers/mpu6000/mpu6000.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 168b34ea91..81d156af1d 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -921,18 +921,18 @@ MPU6000::gyro_self_test() if (self_test()) return 1; - /* evaluate gyro offsets, complain if offset -> zero or larger than 6 dps */ - if (fabsf(_gyro_scale.x_offset) > 0.1f || fabsf(_gyro_scale.x_offset) < 0.000001f) + /* evaluate gyro offsets, complain if offset -> zero or larger than 7 dps */ + if (fabsf(_gyro_scale.x_offset) > 0.12f || fabsf(_gyro_scale.x_offset) < 0.000001f) return 1; if (fabsf(_gyro_scale.x_scale - 1.0f) > 0.3f) return 1; - if (fabsf(_gyro_scale.y_offset) > 0.1f || fabsf(_gyro_scale.y_offset) < 0.000001f) + if (fabsf(_gyro_scale.y_offset) > 0.12f || fabsf(_gyro_scale.y_offset) < 0.000001f) return 1; if (fabsf(_gyro_scale.y_scale - 1.0f) > 0.3f) return 1; - if (fabsf(_gyro_scale.z_offset) > 0.1f || fabsf(_gyro_scale.z_offset) < 0.000001f) + if (fabsf(_gyro_scale.z_offset) > 0.12f || fabsf(_gyro_scale.z_offset) < 0.000001f) return 1; if (fabsf(_gyro_scale.z_scale - 1.0f) > 0.3f) return 1; @@ -941,6 +941,7 @@ MPU6000::gyro_self_test() } + /* perform a self-test comparison to factory trim values. This takes about 200ms and will return OK if the current values are within 14% From b8fade7dcfa98d0963c7f23529168baa64f7fc1a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 26 Jan 2015 20:04:56 +0100 Subject: [PATCH 610/652] MPU6K: Improve gyro self test to allow more realistic deviations from nominal state --- src/drivers/mpu6000/mpu6000.cpp | 31 ++++++++++++++++++++++--------- 1 file changed, 22 insertions(+), 9 deletions(-) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 81d156af1d..1c35bae2d2 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -921,22 +921,35 @@ MPU6000::gyro_self_test() if (self_test()) return 1; - /* evaluate gyro offsets, complain if offset -> zero or larger than 7 dps */ - if (fabsf(_gyro_scale.x_offset) > 0.12f || fabsf(_gyro_scale.x_offset) < 0.000001f) - return 1; - if (fabsf(_gyro_scale.x_scale - 1.0f) > 0.3f) + const float max_offset = 0.34f; + const float max_scale = 0.3f; + + /* evaluate gyro offsets, complain if offset -> zero or larger than 20 dps. */ + if (fabsf(_gyro_scale.x_offset) > max_offset) return 1; - if (fabsf(_gyro_scale.y_offset) > 0.12f || fabsf(_gyro_scale.y_offset) < 0.000001f) - return 1; - if (fabsf(_gyro_scale.y_scale - 1.0f) > 0.3f) + /* evaluate gyro scale, complain if off by more than 30% */ + if (fabsf(_gyro_scale.x_scale - 1.0f) > max_scale) return 1; - if (fabsf(_gyro_scale.z_offset) > 0.12f || fabsf(_gyro_scale.z_offset) < 0.000001f) + if (fabsf(_gyro_scale.y_offset) > max_offset) return 1; - if (fabsf(_gyro_scale.z_scale - 1.0f) > 0.3f) + if (fabsf(_gyro_scale.y_scale - 1.0f) > max_scale) return 1; + if (fabsf(_gyro_scale.z_offset) > max_offset) + return 1; + if (fabsf(_gyro_scale.z_scale - 1.0f) > max_scale) + return 1; + + /* check if all scales are zero */ + if ((fabsf(_gyro_scale.x_offset) < 0.000001f) && + (fabsf(_gyro_scale.y_offset) < 0.000001f) && + (fabsf(_gyro_scale.z_offset) < 0.000001f)) { + /* if all are zero, this device is not calibrated */ + return 1; + } + return 0; } From 52f3fe1d9af53e5f1dd40714aa177fcc4c5e0047 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 26 Jan 2015 20:23:42 +0100 Subject: [PATCH 611/652] Added more docs to offset as suggested by @velocoderaptor, thanks! --- src/drivers/mpu6000/mpu6000.cpp | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 1c35bae2d2..2be7582442 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -921,7 +921,17 @@ MPU6000::gyro_self_test() if (self_test()) return 1; + /* + * Maximum deviation of 20 degrees, according to + * http://www.invensense.com/mems/gyro/documents/PS-MPU-6000A-00v3.4.pdf + * Section 6.1, initial ZRO tolerance + */ const float max_offset = 0.34f; + /* 30% scale error is chosen to catch completely faulty units but + * to let some slight scale error pass. Requires a rate table or correlation + * with mag rotations + data fit to + * calibrate properly and is not done by default. + */ const float max_scale = 0.3f; /* evaluate gyro offsets, complain if offset -> zero or larger than 20 dps. */ From e95e09628484875cf65686561eef8434195df96b Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Wed, 28 Jan 2015 09:31:42 +0100 Subject: [PATCH 612/652] added references --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 6 ++++++ 1 file changed, 6 insertions(+) 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 962dc6d4a0..cad6cf3aea 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -35,6 +35,12 @@ * @file mc_pos_control_main.cpp * Multicopter position controller. * + * Original publication for the desired attitude generation: + * Daniel Mellinger and Vijay Kumar. Minimum Snap Trajectory Generation and Control for Quadrotors. + * Int. Conf. on Robotics and Automation, Shanghai, China, May 2011 + * + * Also inspired by https://pixhawk.org/firmware/apps/fw_pos_control_l1 + * * The controller has two loops: P loop for position error and PID loop for velocity error. * Output of velocity controller is thrust vector that splitted to thrust direction * (i.e. rotation matrix for multicopter orientation) and thrust module (i.e. multicopter thrust itself). From 97471bf0aae5c069fe37f063f2a8b5eae08755c9 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Wed, 28 Jan 2015 09:31:42 +0100 Subject: [PATCH 613/652] added references --- src/modules/mc_att_control/mc_att_control_main.cpp | 4 ++++ 1 file changed, 4 insertions(+) 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 a094ed2c60..c828f1f942 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -35,6 +35,10 @@ * @file mc_att_control_main.cpp * Multicopter attitude controller. * + * Publication for the desired attitude tracking: + * Daniel Mellinger and Vijay Kumar. Minimum Snap Trajectory Generation and Control for Quadrotors. + * Int. Conf. on Robotics and Automation, Shanghai, China, May 2011. + * * @author Tobias Naegeli * @author Lorenz Meier * @author Anton Babushkin From 5444972347606333dd61206011263e4a2e5d83e8 Mon Sep 17 00:00:00 2001 From: hauptmech Date: Mon, 19 Jan 2015 16:50:52 +1300 Subject: [PATCH 614/652] Add call to access the mcu unique id. Expose via the 'ver' command. This is prep for verifying calibration parameters against the hardware they were gathered on. --- src/modules/systemlib/mcu_version.c | 11 +++++++++-- src/modules/systemlib/mcu_version.h | 11 +++++++++++ src/systemcmds/ver/ver.c | 14 +++++++++++++- 3 files changed, 33 insertions(+), 3 deletions(-) diff --git a/src/modules/systemlib/mcu_version.c b/src/modules/systemlib/mcu_version.c index 4bcf957848..24f4e42076 100644 --- a/src/modules/systemlib/mcu_version.c +++ b/src/modules/systemlib/mcu_version.c @@ -47,7 +47,8 @@ #ifdef CONFIG_ARCH_CHIP_STM32 #include -#define DBGMCU_IDCODE 0xE0042000 +#define DBGMCU_IDCODE 0xE0042000 //STM DocID018909 Rev 8 Sect 38.18 (MCU device ID code) +#define UNIQUE_ID 0x1FFF7A10 //STM DocID018909 Rev 8 Sect 39.1 (Unique device ID Register) #define STM32F40x_41x 0x413 #define STM32F42x_43x 0x419 @@ -57,7 +58,13 @@ #endif - +/** Copy the 96bit MCU Unique ID into the provided pointer */ +void mcu_unique_id(uint32_t *uid_96_bit) +{ + uid_96_bit[0] = getreg32(UNIQUE_ID); + uid_96_bit[1] = getreg32(UNIQUE_ID+4); + uid_96_bit[2] = getreg32(UNIQUE_ID+8); +} int mcu_version(char* rev, char** revstr) { diff --git a/src/modules/systemlib/mcu_version.h b/src/modules/systemlib/mcu_version.h index 1b3d0aba9b..c8a0bf5cda 100644 --- a/src/modules/systemlib/mcu_version.h +++ b/src/modules/systemlib/mcu_version.h @@ -33,6 +33,8 @@ #pragma once +#include + /* magic numbers from reference manual */ enum MCU_REV { MCU_REV_STM32F4_REV_A = 0x1000, @@ -42,6 +44,15 @@ enum MCU_REV { MCU_REV_STM32F4_REV_3 = 0x2001 }; + +/** + * Reports the microcontroller unique id. + * + * This ID is guaranteed to be unique for every mcu. + * @param uid_96_bit A uint32_t[3] array to copy the data to. + */ +__EXPORT void mcu_unique_id(uint32_t *uid_96_bit); + /** * Reports the microcontroller version of the main CPU. * diff --git a/src/systemcmds/ver/ver.c b/src/systemcmds/ver/ver.c index 2ead3e6323..087eb52e3d 100644 --- a/src/systemcmds/ver/ver.c +++ b/src/systemcmds/ver/ver.c @@ -54,6 +54,7 @@ static const char sz_ver_bdate_str[] = "bdate"; static const char sz_ver_gcc_str[] = "gcc"; static const char sz_ver_all_str[] = "all"; static const char mcu_ver_str[] = "mcu"; +static const char mcu_uid_str[] = "uid"; static void usage(const char *reason) { @@ -61,7 +62,7 @@ static void usage(const char *reason) printf("%s\n", reason); } - printf("usage: ver {hw|hwcmp|git|bdate|gcc|all|mcu}\n\n"); + printf("usage: ver {hw|hwcmp|git|bdate|gcc|all|mcu|uid}\n\n"); } __EXPORT int ver_main(int argc, char *argv[]); @@ -141,6 +142,17 @@ int ver_main(int argc, char *argv[]) ret = 0; } + if (show_all || !strncmp(argv[1], mcu_uid_str, sizeof(mcu_uid_str))) { + uint32_t uid[3]; + + mcu_unique_id(uid); + + printf("UID: %X:%X:%X \n",uid[0],uid[1],uid[2]); + + ret = 0; + } + + if (ret == 1) { errx(1, "unknown command.\n"); } From 21d3dcb1905432e349f70012ac0a4f79e5e7935a Mon Sep 17 00:00:00 2001 From: hauptmech Date: Wed, 28 Jan 2015 15:40:44 +1300 Subject: [PATCH 615/652] Fix config utility to work with all devices of each type, not just the primary one. --- src/systemcmds/config/config.c | 70 +++++++++++++++++----------------- 1 file changed, 35 insertions(+), 35 deletions(-) diff --git a/src/systemcmds/config/config.c b/src/systemcmds/config/config.c index 077bc47c90..9f13edb184 100644 --- a/src/systemcmds/config/config.c +++ b/src/systemcmds/config/config.c @@ -36,7 +36,7 @@ * @author Lorenz Meier * @author Julian Oes * - * config tool. + * config tool. Takes the device name as the first parameter. */ #include @@ -71,18 +71,18 @@ int config_main(int argc, char *argv[]) { if (argc >= 2) { - if (!strcmp(argv[1], "gyro")) { - do_gyro(argc - 2, argv + 2); - } else if (!strcmp(argv[1], "accel")) { - do_accel(argc - 2, argv + 2); - } else if (!strcmp(argv[1], "mag")) { - do_mag(argc - 2, argv + 2); + if (!strncmp(argv[1], "/dev/gyro",9)) { + do_gyro(argc - 1, argv + 1); + } else if (!strncmp(argv[1], "/dev/accel",10)) { + do_accel(argc - 1, argv + 1); + } else if (!strncmp(argv[1], "/dev/mag",8)) { + do_mag(argc - 1, argv + 1); } else { do_device(argc - 1, argv + 1); } } - errx(1, "expected a command, try 'gyro', 'accel', 'mag'"); + errx(1, "expected a device, try '/dev/gyro', '/dev/accel', '/dev/mag'"); } static void @@ -133,41 +133,41 @@ do_gyro(int argc, char *argv[]) { int fd; - fd = open(GYRO_DEVICE_PATH, 0); + fd = open(argv[0], 0); if (fd < 0) { - warn("%s", GYRO_DEVICE_PATH); + warn("%s", argv[0]); errx(1, "FATAL: no gyro found"); } else { int ret; - if (argc == 2 && !strcmp(argv[0], "sampling")) { + if (argc == 3 && !strcmp(argv[1], "sampling")) { /* set the gyro internal sampling rate up to at least i Hz */ - ret = ioctl(fd, GYROIOCSSAMPLERATE, strtoul(argv[1], NULL, 0)); + ret = ioctl(fd, GYROIOCSSAMPLERATE, strtoul(argv[2], NULL, 0)); if (ret) errx(ret,"sampling rate could not be set"); - } else if (argc == 2 && !strcmp(argv[0], "rate")) { + } else if (argc == 3 && !strcmp(argv[1], "rate")) { /* set the driver to poll at i Hz */ - ret = ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[1], NULL, 0)); + ret = ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[2], NULL, 0)); if (ret) errx(ret,"pollrate could not be set"); - } else if (argc == 2 && !strcmp(argv[0], "range")) { + } else if (argc == 3 && !strcmp(argv[1], "range")) { /* set the range to i dps */ - ret = ioctl(fd, GYROIOCSRANGE, strtoul(argv[1], NULL, 0)); + ret = ioctl(fd, GYROIOCSRANGE, strtoul(argv[2], NULL, 0)); if (ret) errx(ret,"range could not be set"); - } else if (argc == 1 && !strcmp(argv[0], "check")) { + } else if (argc == 2 && !strcmp(argv[1], "check")) { ret = ioctl(fd, GYROIOCSELFTEST, 0); if (ret) { @@ -206,41 +206,41 @@ do_mag(int argc, char *argv[]) { int fd; - fd = open(MAG_DEVICE_PATH, 0); + fd = open(argv[0], 0); if (fd < 0) { - warn("%s", MAG_DEVICE_PATH); + warn("%s", argv[0]); errx(1, "FATAL: no magnetometer found"); } else { int ret; - if (argc == 2 && !strcmp(argv[0], "sampling")) { + if (argc == 3 && !strcmp(argv[1], "sampling")) { /* set the mag internal sampling rate up to at least i Hz */ - ret = ioctl(fd, MAGIOCSSAMPLERATE, strtoul(argv[1], NULL, 0)); + ret = ioctl(fd, MAGIOCSSAMPLERATE, strtoul(argv[2], NULL, 0)); if (ret) errx(ret,"sampling rate could not be set"); - } else if (argc == 2 && !strcmp(argv[0], "rate")) { + } else if (argc == 3 && !strcmp(argv[1], "rate")) { /* set the driver to poll at i Hz */ - ret = ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[1], NULL, 0)); + ret = ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[2], NULL, 0)); if (ret) errx(ret,"pollrate could not be set"); - } else if (argc == 2 && !strcmp(argv[0], "range")) { + } else if (argc == 3 && !strcmp(argv[1], "range")) { /* set the range to i G */ - ret = ioctl(fd, MAGIOCSRANGE, strtoul(argv[1], NULL, 0)); + ret = ioctl(fd, MAGIOCSRANGE, strtoul(argv[2], NULL, 0)); if (ret) errx(ret,"range could not be set"); - } else if(argc == 1 && !strcmp(argv[0], "check")) { + } else if(argc == 2 && !strcmp(argv[1], "check")) { ret = ioctl(fd, MAGIOCSELFTEST, 0); if (ret) { @@ -282,41 +282,41 @@ do_accel(int argc, char *argv[]) { int fd; - fd = open(ACCEL_DEVICE_PATH, 0); + fd = open(argv[0], 0); if (fd < 0) { - warn("%s", ACCEL_DEVICE_PATH); + warn("%s", argv[0]); errx(1, "FATAL: no accelerometer found"); } else { int ret; - if (argc == 2 && !strcmp(argv[0], "sampling")) { + if (argc == 3 && !strcmp(argv[1], "sampling")) { /* set the accel internal sampling rate up to at least i Hz */ - ret = ioctl(fd, ACCELIOCSSAMPLERATE, strtoul(argv[1], NULL, 0)); + ret = ioctl(fd, ACCELIOCSSAMPLERATE, strtoul(argv[2], NULL, 0)); if (ret) errx(ret,"sampling rate could not be set"); - } else if (argc == 2 && !strcmp(argv[0], "rate")) { + } else if (argc == 3 && !strcmp(argv[1], "rate")) { /* set the driver to poll at i Hz */ - ret = ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[1], NULL, 0)); + ret = ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[2], NULL, 0)); if (ret) errx(ret,"pollrate could not be set"); - } else if (argc == 2 && !strcmp(argv[0], "range")) { + } else if (argc == 3 && !strcmp(argv[1], "range")) { /* set the range to i G */ - ret = ioctl(fd, ACCELIOCSRANGE, strtoul(argv[1], NULL, 0)); + ret = ioctl(fd, ACCELIOCSRANGE, strtoul(argv[2], NULL, 0)); if (ret) errx(ret,"range could not be set"); - } else if(argc == 1 && !strcmp(argv[0], "check")) { + } else if(argc == 2 && !strcmp(argv[1], "check")) { ret = ioctl(fd, ACCELIOCSELFTEST, 0); if (ret) { From a3b2c99801a2cdc780cce77e04dd2d924c2c0560 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Tue, 27 Jan 2015 12:37:35 -0500 Subject: [PATCH 616/652] A huge developer time saver, J="" make archives This gives warnings about -j1 being forced some places, but it still successfully builds all archvies in parallel. The resulting archives have been tested on the board. It is disabled by default so no functional change unless someone adds J="" or J=8 in front of the make archvies. --- Makefile | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/Makefile b/Makefile index 64c468d31b..b178597056 100644 --- a/Makefile +++ b/Makefile @@ -169,16 +169,18 @@ ifneq ($(filter archives,$(MAKECMDGOALS)),) .NOTPARALLEL: endif +J?=1 + $(ARCHIVE_DIR)%.export: board = $(notdir $(basename $@)) $(ARCHIVE_DIR)%.export: configuration = nsh $(NUTTX_ARCHIVES): $(ARCHIVE_DIR)%.export: $(NUTTX_SRC) @$(ECHO) %% Configuring NuttX for $(board) $(Q) (cd $(NUTTX_SRC) && $(RMDIR) nuttx-export) - $(Q) $(MAKE) -r -j1 -C $(NUTTX_SRC) -r $(MQUIET) distclean + $(Q) $(MAKE) -r -j$(J) -C $(NUTTX_SRC) -r $(MQUIET) distclean $(Q) (cd $(NUTTX_SRC)/configs && $(COPYDIR) $(PX4_BASE)nuttx-configs/$(board) .) $(Q) (cd $(NUTTX_SRC)tools && ./configure.sh $(board)/$(configuration)) @$(ECHO) %% Exporting NuttX for $(board) - $(Q) $(MAKE) -r -j1 -C $(NUTTX_SRC) -r $(MQUIET) CONFIG_ARCH_BOARD=$(board) export + $(Q) $(MAKE) -r -j$(J) -C $(NUTTX_SRC) -r $(MQUIET) CONFIG_ARCH_BOARD=$(board) export $(Q) $(MKDIR) -p $(dir $@) $(Q) $(COPY) $(NUTTX_SRC)nuttx-export.zip $@ $(Q) (cd $(NUTTX_SRC)/configs && $(RMDIR) $(board)) @@ -195,11 +197,11 @@ BOARD = $(BOARDS) menuconfig: $(NUTTX_SRC) @$(ECHO) %% Configuring NuttX for $(BOARD) $(Q) (cd $(NUTTX_SRC) && $(RMDIR) nuttx-export) - $(Q) $(MAKE) -r -j1 -C $(NUTTX_SRC) -r $(MQUIET) distclean + $(Q) $(MAKE) -r -j$(J) -C $(NUTTX_SRC) -r $(MQUIET) distclean $(Q) (cd $(NUTTX_SRC)/configs && $(COPYDIR) $(PX4_BASE)nuttx-configs/$(BOARD) .) $(Q) (cd $(NUTTX_SRC)tools && ./configure.sh $(BOARD)/nsh) @$(ECHO) %% Running menuconfig for $(BOARD) - $(Q) $(MAKE) -r -j1 -C $(NUTTX_SRC) -r $(MQUIET) menuconfig + $(Q) $(MAKE) -r -j$(J) -C $(NUTTX_SRC) -r $(MQUIET) menuconfig @$(ECHO) %% Saving configuration file $(Q)$(COPY) $(NUTTX_SRC).config $(PX4_BASE)nuttx-configs/$(BOARD)/nsh/defconfig else From 27b2701340648e2fde1992b175abfa591e0eee01 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 28 Jan 2015 09:48:53 +0100 Subject: [PATCH 617/652] temporarily re-enable stack checking, disable some modules to make firmware fit --- makefiles/config_px4fmu-v2_default.mk | 10 +++++----- nuttx-configs/px4fmu-v2/nsh/defconfig | 2 +- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index 3abebd82fa..76457216bd 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -37,11 +37,11 @@ MODULES += drivers/hil # MODULES += drivers/hott/hott_sensors # MODULES += drivers/blinkm MODULES += drivers/airspeed -MODULES += drivers/ets_airspeed +# MODULES += drivers/ets_airspeed MODULES += drivers/meas_airspeed -MODULES += drivers/frsky_telemetry +# MODULES += drivers/frsky_telemetry MODULES += modules/sensors -MODULES += drivers/mkblctrl +# MODULES += drivers/mkblctrl MODULES += drivers/px4flow # @@ -70,7 +70,7 @@ MODULES += modules/commander MODULES += modules/navigator MODULES += modules/mavlink MODULES += modules/gpio_led -MODULES += modules/uavcan +# MODULES += modules/uavcan MODULES += modules/land_detector # @@ -120,7 +120,7 @@ MODULES += lib/launchdetection # # OBC challenge # -MODULES += modules/bottle_drop +# MODULES += modules/bottle_drop # # Demo apps diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index dedebdfa03..9030a1f022 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -117,7 +117,7 @@ CONFIG_ARCH_HAVE_MPU=y # # CONFIG_ARMV7M_TOOLCHAIN_BUILDROOT is not set CONFIG_ARMV7M_TOOLCHAIN_GNU_EABI=y -CONFIG_ARMV7M_STACKCHECK=n +CONFIG_ARMV7M_STACKCHECK=y CONFIG_SERIAL_TERMIOS=y CONFIG_SDIO_DMA=y CONFIG_SDIO_DMAPRIO=0x00010000 From a25dbb3c8e795b90869c480ae5c62c84b8424b3c Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Wed, 28 Jan 2015 11:19:52 +0100 Subject: [PATCH 618/652] Add a compiler coloring tool to highlight warning and errors in the output. --- Tools/make_color.sh | 36 ++++++++++++++++++++++++++++++++++++ 1 file changed, 36 insertions(+) create mode 100755 Tools/make_color.sh diff --git a/Tools/make_color.sh b/Tools/make_color.sh new file mode 100755 index 0000000000..81316a9328 --- /dev/null +++ b/Tools/make_color.sh @@ -0,0 +1,36 @@ +#!/bin/sh +# make_color.sh +# +# Author: Simon Wilks (simon@uaventure.com) +# +# A compiler color coder. +# +# To invoke this script everytime you run make simply create the alias: +# +# alias make='/Tools/make_color.sh' +# +# Color codes: +# +# white "\033[1,37m" +# yellow "\033[1,33m" +# green "\033[1,32m" +# blue "\033[1,34m" +# cyan "\033[1,36m" +# red "\033[1,31m" +# magenta "\033[1,35m" +# black "\033[1,30m" +# darkwhite "\033[0,37m" +# darkyellow "\033[0,33m" +# darkgreen "\033[0,32m" +# darkblue "\033[0,34m" +# darkcyan "\033[0,36m" +# darkred "\033[0,31m" +# darkmagenta "\033[0,35m" +# off "\033[0,0m" +# +OFF="\o033[0m" +WARN="\o033[1;33m" +ERROR="\o033[1;31m" +INFO="\o033[0;37m" + +make ${@} 2>&1 | sed "s/make\[[0-9]\].*/$INFO & $OFF/;s/.*: warning: .*/$WARN & $OFF/;s/.*: error: .*/$ERROR & $OFF/" From 6606b5636434c2198ba76221fab58e943eecb253 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Thu, 29 Jan 2015 04:49:39 -1000 Subject: [PATCH 619/652] Updated NuttX submodule with memcpy fix, disabled run time stack checking and added modules back in --- NuttX | 2 +- makefiles/config_px4fmu-v2_default.mk | 10 +++++----- nuttx-configs/px4fmu-v1/nsh/defconfig | 2 +- nuttx-configs/px4fmu-v2/nsh/defconfig | 2 +- nuttx-configs/px4io-v1/nsh/defconfig | 2 +- nuttx-configs/px4io-v2/nsh/defconfig | 2 +- 6 files changed, 10 insertions(+), 10 deletions(-) diff --git a/NuttX b/NuttX index e4c914e261..e1e56f2254 160000 --- a/NuttX +++ b/NuttX @@ -1 +1 @@ -Subproject commit e4c914e261d2647e44d05222afa7aa3cc90d3c67 +Subproject commit e1e56f2254559c1e71e97d423cb282ec82feece4 diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index 76457216bd..3abebd82fa 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -37,11 +37,11 @@ MODULES += drivers/hil # MODULES += drivers/hott/hott_sensors # MODULES += drivers/blinkm MODULES += drivers/airspeed -# MODULES += drivers/ets_airspeed +MODULES += drivers/ets_airspeed MODULES += drivers/meas_airspeed -# MODULES += drivers/frsky_telemetry +MODULES += drivers/frsky_telemetry MODULES += modules/sensors -# MODULES += drivers/mkblctrl +MODULES += drivers/mkblctrl MODULES += drivers/px4flow # @@ -70,7 +70,7 @@ MODULES += modules/commander MODULES += modules/navigator MODULES += modules/mavlink MODULES += modules/gpio_led -# MODULES += modules/uavcan +MODULES += modules/uavcan MODULES += modules/land_detector # @@ -120,7 +120,7 @@ MODULES += lib/launchdetection # # OBC challenge # -# MODULES += modules/bottle_drop +MODULES += modules/bottle_drop # # Demo apps diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig index a467fa605e..539634e3da 100644 --- a/nuttx-configs/px4fmu-v1/nsh/defconfig +++ b/nuttx-configs/px4fmu-v1/nsh/defconfig @@ -92,7 +92,7 @@ CONFIG_ARCH_HAVE_MPU=y # # CONFIG_ARMV7M_TOOLCHAIN_BUILDROOT is not set CONFIG_ARMV7M_TOOLCHAIN_GNU_EABI=y -CONFIG_ARMV7M_STACKCHECK=y +CONFIG_ARMV7M_STACKCHECK=n CONFIG_SERIAL_TERMIOS=y # diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index 9030a1f022..dedebdfa03 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -117,7 +117,7 @@ CONFIG_ARCH_HAVE_MPU=y # # CONFIG_ARMV7M_TOOLCHAIN_BUILDROOT is not set CONFIG_ARMV7M_TOOLCHAIN_GNU_EABI=y -CONFIG_ARMV7M_STACKCHECK=y +CONFIG_ARMV7M_STACKCHECK=n CONFIG_SERIAL_TERMIOS=y CONFIG_SDIO_DMA=y CONFIG_SDIO_DMAPRIO=0x00010000 diff --git a/nuttx-configs/px4io-v1/nsh/defconfig b/nuttx-configs/px4io-v1/nsh/defconfig index 7c76be7ec0..385b8d6a8f 100755 --- a/nuttx-configs/px4io-v1/nsh/defconfig +++ b/nuttx-configs/px4io-v1/nsh/defconfig @@ -93,7 +93,7 @@ CONFIG_ARCH_DMA=y CONFIG_ARCH_MATH_H=y CONFIG_ARMV7M_CMNVECTOR=y -# CONFIG_ARMV7M_STACKCHECK is not set +CONFIG_ARMV7M_STACKCHECK=n # # JTAG Enable settings (by default JTAG-DP and SW-DP are disabled): diff --git a/nuttx-configs/px4io-v2/nsh/defconfig b/nuttx-configs/px4io-v2/nsh/defconfig index 02b51e3d73..a9b8477948 100755 --- a/nuttx-configs/px4io-v2/nsh/defconfig +++ b/nuttx-configs/px4io-v2/nsh/defconfig @@ -89,7 +89,7 @@ CONFIG_ARCH_DMA=y CONFIG_ARCH_MATH_H=y CONFIG_ARMV7M_CMNVECTOR=y - +CONFIG_ARMV7M_STACKCHECK=n # # JTAG Enable settings (by default JTAG-DP and SW-DP are disabled): # From 2c644715002516ebe998bd952aa97baf9b80d64f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 29 Jan 2015 16:31:21 +0100 Subject: [PATCH 620/652] Updated NuttX submodule --- NuttX | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/NuttX b/NuttX index e1e56f2254..37cbc3e8ae 160000 --- a/NuttX +++ b/NuttX @@ -1 +1 @@ -Subproject commit e1e56f2254559c1e71e97d423cb282ec82feece4 +Subproject commit 37cbc3e8ae6b75e9b7e79996d30fe8ed0beb9704 From 8de411619a0ce05cc9f34f5a9f756908dbd21db8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 24 Jan 2015 15:50:53 +0100 Subject: [PATCH 621/652] Initial stab at supporting multiple publications on the same base name and auto-enumeration of additional publications. --- ROMFS/px4fmu_test/init.d/rcS | 7 + src/drivers/drv_orb_dev.h | 9 +- src/modules/uORB/Publication.cpp | 2 +- src/modules/uORB/Publication.hpp | 2 +- src/modules/uORB/Subscription.cpp | 2 +- src/modules/uORB/Subscription.hpp | 2 +- src/modules/uORB/module.mk | 5 +- src/modules/uORB/objects_common.cpp | 2 +- src/modules/uORB/uORB.cpp | 270 ++++++++++++++++++++-------- src/modules/uORB/uORB.h | 65 ++++++- 10 files changed, 278 insertions(+), 88 deletions(-) diff --git a/ROMFS/px4fmu_test/init.d/rcS b/ROMFS/px4fmu_test/init.d/rcS index 40b3500e0e..4b9a9b68a0 100644 --- a/ROMFS/px4fmu_test/init.d/rcS +++ b/ROMFS/px4fmu_test/init.d/rcS @@ -97,6 +97,13 @@ else set unit_test_failure_list "${unit_test_failure_list} commander_tests" fi +if uorb test +then +else + set unit_test_failure 1 + set unit_test_failure_list "${unit_test_failure_list} uorb_tests" +fi + if hmc5883 -I start then # This is an FMUv3 diff --git a/src/drivers/drv_orb_dev.h b/src/drivers/drv_orb_dev.h index 713618545d..e0b16fa5cd 100644 --- a/src/drivers/drv_orb_dev.h +++ b/src/drivers/drv_orb_dev.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -35,7 +35,9 @@ #define _DRV_UORB_H /** - * @file uORB published object driver. + * @file drv_orb_dev.h + * + * uORB published object driver. */ #include @@ -84,4 +86,7 @@ /** Get the global advertiser handle for the topic */ #define ORBIOCGADVERTISER _ORBIOC(13) +/** Get the priority for the topic */ +#define ORBIOCGPRIORITY _ORBIOC(14) + #endif /* _DRV_UORB_H */ diff --git a/src/modules/uORB/Publication.cpp b/src/modules/uORB/Publication.cpp index 71757e1f42..d33f930602 100644 --- a/src/modules/uORB/Publication.cpp +++ b/src/modules/uORB/Publication.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions diff --git a/src/modules/uORB/Publication.hpp b/src/modules/uORB/Publication.hpp index 8650b3df89..b64559734c 100644 --- a/src/modules/uORB/Publication.hpp +++ b/src/modules/uORB/Publication.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions diff --git a/src/modules/uORB/Subscription.cpp b/src/modules/uORB/Subscription.cpp index 44b6debc7e..8884e5a3a1 100644 --- a/src/modules/uORB/Subscription.cpp +++ b/src/modules/uORB/Subscription.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions diff --git a/src/modules/uORB/Subscription.hpp b/src/modules/uORB/Subscription.hpp index 34e9a83e03..75cf362546 100644 --- a/src/modules/uORB/Subscription.hpp +++ b/src/modules/uORB/Subscription.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions diff --git a/src/modules/uORB/module.mk b/src/modules/uORB/module.mk index 9385ce253c..71ad09130c 100644 --- a/src/modules/uORB/module.mk +++ b/src/modules/uORB/module.mk @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. +# Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions @@ -37,8 +37,7 @@ MODULE_COMMAND = uorb -# XXX probably excessive, 2048 should be sufficient -MODULE_STACKSIZE = 4096 +MODULE_STACKSIZE = 2048 SRCS = uORB.cpp \ objects_common.cpp \ diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 78fdf4de75..20a9bcc43c 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012-2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions diff --git a/src/modules/uORB/uORB.cpp b/src/modules/uORB/uORB.cpp index 3373aac837..cfea12f044 100644 --- a/src/modules/uORB/uORB.cpp +++ b/src/modules/uORB/uORB.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (Cc) 2012-2015 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -51,6 +51,7 @@ #include #include #include +#include #include #include @@ -68,6 +69,7 @@ namespace { +/* internal use only */ static const unsigned orb_maxpath = 64; /* oddly, ERROR is not defined for c++ */ @@ -81,17 +83,30 @@ enum Flavor { PARAM }; +struct orb_advertdata { + const struct orb_metadata *meta; + int *instance; + int priority; +}; + int -node_mkpath(char *buf, Flavor f, const struct orb_metadata *meta) +node_mkpath(char *buf, Flavor f, const struct orb_metadata *meta, int *instance = nullptr) { unsigned len; - len = snprintf(buf, orb_maxpath, "/%s/%s", - (f == PUBSUB) ? "obj" : "param", - meta->o_name); + unsigned index = 0; - if (len >= orb_maxpath) + if (instance != nullptr) { + index = *instance; + } + + len = snprintf(buf, orb_maxpath, "/%s/%s%d", + (f == PUBSUB) ? "obj" : "param", + meta->o_name, index); + + if (len >= orb_maxpath) { return -ENAMETOOLONG; + } return OK; } @@ -104,7 +119,7 @@ node_mkpath(char *buf, Flavor f, const struct orb_metadata *meta) class ORBDevNode : public device::CDev { public: - ORBDevNode(const struct orb_metadata *meta, const char *name, const char *path); + ORBDevNode(const struct orb_metadata *meta, const char *name, const char *path, int priority); ~ORBDevNode(); virtual int open(struct file *filp); @@ -126,6 +141,7 @@ private: struct hrt_call update_call; /**< deferred wakeup call if update_period is nonzero */ void *poll_priv; /**< saved copy of fds->f_priv while poll is active */ bool update_reported; /**< true if we have reported the update via poll/check */ + int priority; /**< priority of publisher */ }; const struct orb_metadata *_meta; /**< object metadata information */ @@ -133,6 +149,7 @@ private: hrt_abstime _last_update; /**< time the object was last updated */ volatile unsigned _generation; /**< object generation count */ pid_t _publisher; /**< if nonzero, current publisher */ + const int _priority; /**< priority of topic */ SubscriberData *filp_to_sd(struct file *filp) { SubscriberData *sd = (SubscriberData *)(filp->f_priv); @@ -160,13 +177,14 @@ private: bool appears_updated(SubscriberData *sd); }; -ORBDevNode::ORBDevNode(const struct orb_metadata *meta, const char *name, const char *path) : +ORBDevNode::ORBDevNode(const struct orb_metadata *meta, const char *name, const char *path, int priority) : CDev(name, path), _meta(meta), _data(nullptr), _last_update(0), _generation(0), - _publisher(0) + _publisher(0), + _priority(priority) { // enable debug() calls _debug_enabled = true; @@ -176,6 +194,7 @@ ORBDevNode::~ORBDevNode() { if (_data != nullptr) delete[] _data; + } int @@ -225,6 +244,9 @@ ORBDevNode::open(struct file *filp) /* default to no pending update */ sd->generation = _generation; + /* set priority */ + sd->priority = _priority; + filp->f_priv = (void *)sd; ret = CDev::open(filp); @@ -283,6 +305,9 @@ ORBDevNode::read(struct file *filp, char *buffer, size_t buflen) /* track the last generation that the file has seen */ sd->generation = _generation; + /* set priority */ + sd->priority = _priority; + /* * Clear the flag that indicates that an update has been reported, as * we have just collected it. @@ -364,6 +389,10 @@ ORBDevNode::ioctl(struct file *filp, int cmd, unsigned long arg) *(uintptr_t *)arg = (uintptr_t)this; return OK; + case ORBIOCGPRIORITY: + *(int *)arg = sd->priority; + return OK; + default: /* give it to the superclass */ return CDev::ioctl(filp, cmd, arg); @@ -556,40 +585,73 @@ ORBDevMaster::ioctl(struct file *filp, int cmd, unsigned long arg) switch (cmd) { case ORBIOCADVERTISE: { - const struct orb_metadata *meta = (const struct orb_metadata *)arg; + const struct orb_advertdata *adv = (const struct orb_advertdata *)arg; + const struct orb_metadata *meta = adv->meta; const char *objname; + const char *devpath; char nodepath[orb_maxpath]; ORBDevNode *node; /* construct a path to the node - this also checks the node name */ - ret = node_mkpath(nodepath, _flavor, meta); + ret = node_mkpath(nodepath, _flavor, meta, adv->instance); - if (ret != OK) + if (ret != OK) { return ret; + } /* driver wants a permanent copy of the node name, so make one here */ objname = strdup(meta->o_name); - if (objname == nullptr) + if (objname == nullptr) { return -ENOMEM; - - /* construct the new node */ - node = new ORBDevNode(meta, objname, nodepath); - - /* initialise the node - this may fail if e.g. a node with this name already exists */ - if (node != nullptr) - ret = node->init(); - - /* if we didn't get a device, that's bad */ - if (node == nullptr) - return -ENOMEM; - - /* if init failed, discard the node and its name */ - if (ret != OK) { - delete node; - free((void *)objname); } + /* ensure that only one advertiser runs through this critical section */ + lock(); + + ret = ERROR; + + /* try for topic groups */ + const unsigned max_group_tries = (adv->instance != nullptr) ? ORB_MULTI_MAX_INSTANCES : 1; + unsigned group_tries = 0; + do { + /* if path is modifyable change try index */ + if (adv->instance != nullptr) { + /* replace the number at the end of the string */ + nodepath[strlen(nodepath) - 1] = '0' + group_tries; + *(adv->instance) = group_tries; + } + + /* driver wants a permanent copy of the path, so make one here */ + devpath = strdup(nodepath); + + /* construct the new node */ + node = new ORBDevNode(meta, objname, devpath, adv->priority); + + /* if we didn't get a device, that's bad */ + if (node == nullptr) { + unlock(); + return -ENOMEM; + } + + /* initialise the node - this may fail if e.g. a node with this name already exists */ + ret = node->init(); + + /* if init failed, discard the node and its name */ + if (ret != OK) { + delete node; + free((void *)objname); + } + + } while (ret != OK && (group_tries++ < max_group_tries)); + + if (group_tries >= max_group_tries) { + ret = -ENOMEM; + } + + /* the file handle for the driver has been created, unlock */ + unlock(); + return ret; } @@ -614,6 +676,7 @@ struct orb_test { }; ORB_DEFINE(orb_test, struct orb_test); +ORB_DEFINE(orb_multitest, struct orb_test); int test_fail(const char *fmt, ...) @@ -643,8 +706,6 @@ test_note(const char *fmt, ...) return OK; } -ORB_DECLARE(sensor_combined); - int test() { @@ -700,48 +761,65 @@ test() orb_unsubscribe(sfd); close(pfd); -#if 0 - /* this is a hacky test that exploits the sensors app to test rate-limiting */ + /* this routine tests the multi-topic support */ + test_note("try multi-topic support"); - sfd = orb_subscribe(ORB_ID(sensor_combined)); + int instance0; + int pfd0 = orb_advertise_multi(ORB_ID(orb_multitest), &t, &instance0, ORB_PRIO_MAX); - hrt_abstime start, end; - unsigned count; + test_note("advertised"); + usleep(300000); - start = hrt_absolute_time(); - count = 0; + int instance1; + int pfd1 = orb_advertise_multi(ORB_ID(orb_multitest), &t, &instance1, ORB_PRIO_MIN); - do { - orb_check(sfd, &updated); + if (instance0 != 0) + return test_fail("mult. id0: %d", instance0); - if (updated) { - orb_copy(ORB_ID(sensor_combined), sfd, nullptr); - count++; - } - } while (count < 100); + if (instance1 != 1) + return test_fail("mult. id1: %d", instance1); - end = hrt_absolute_time(); - test_note("full-speed, 100 updates in %llu", end - start); + t.val = 103; + if (OK != orb_publish(ORB_ID(orb_multitest), pfd0, &t)) + return test_fail("mult. pub0 fail"); - orb_set_interval(sfd, 10); + test_note("published"); + usleep(300000); - start = hrt_absolute_time(); - count = 0; + t.val = 203; + if (OK != orb_publish(ORB_ID(orb_multitest), pfd1, &t)) + return test_fail("mult. pub1 fail"); - do { - orb_check(sfd, &updated); + /* subscribe to both topics and ensure valid data is received */ + int sfd0 = orb_subscribe_multi(ORB_ID(orb_multitest), 0); - if (updated) { - orb_copy(ORB_ID(sensor_combined), sfd, nullptr); - count++; - } - } while (count < 100); + if (OK != orb_copy(ORB_ID(orb_multitest), sfd0, &t)) + return test_fail("sub #0 copy failed: %d", errno); - end = hrt_absolute_time(); - test_note("100Hz, 100 updates in %llu", end - start); + if (t.val != 103) + return test_fail("sub #0 val. mismatch: %d", t.val); - orb_unsubscribe(sfd); -#endif + int sfd1 = orb_subscribe_multi(ORB_ID(orb_multitest), 1); + + if (OK != orb_copy(ORB_ID(orb_multitest), sfd1, &t)) + return test_fail("sub #1 copy failed: %d", errno); + + if (t.val != 203) + return test_fail("sub #1 val. mismatch: %d", t.val); + + /* test priorities */ + int prio; + if (OK != orb_priority(sfd0, &prio)) + return test_fail("prio #0"); + + if (prio != ORB_PRIO_MAX) + return test_fail("prio: %d", prio); + + if (OK != orb_priority(sfd1, &prio)) + return test_fail("prio #1"); + + if (prio != ORB_PRIO_MIN) + return test_fail("prio: %d", prio); return test_note("PASS"); } @@ -771,7 +849,7 @@ uorb_main(int argc, char *argv[]) if (!strcmp(argv[1], "start")) { if (g_dev != nullptr) { - fprintf(stderr, "[uorb] already loaded\n"); + warnx("already loaded"); /* user wanted to start uorb, its already running, no error */ return 0; } @@ -780,18 +858,17 @@ uorb_main(int argc, char *argv[]) g_dev = new ORBDevMaster(PUBSUB); if (g_dev == nullptr) { - fprintf(stderr, "[uorb] driver alloc failed\n"); + warnx("driver alloc failed"); return -ENOMEM; } if (OK != g_dev->init()) { - fprintf(stderr, "[uorb] driver init failed\n"); + warnx("driver init failed"); delete g_dev; g_dev = nullptr; return -EIO; } - printf("[uorb] ready\n"); return OK; } @@ -807,8 +884,7 @@ uorb_main(int argc, char *argv[]) if (!strcmp(argv[1], "status")) return info(); - fprintf(stderr, "unrecognised command, try 'start', 'test' or 'status'\n"); - return -EINVAL; + errx(-EINVAL, "unrecognized command, try 'start', 'test' or 'status'"); } /* @@ -825,11 +901,14 @@ namespace * we tried to advertise. */ int -node_advertise(const struct orb_metadata *meta) +node_advertise(const struct orb_metadata *meta, int *instance = nullptr, int priority = ORB_PRIO_DEFAULT) { int fd = -1; int ret = ERROR; + /* fill advertiser data */ + const struct orb_advertdata adv = { meta, instance, priority }; + /* open the control device */ fd = open(TOPIC_MASTER_DEVICE_PATH, 0); @@ -837,11 +916,12 @@ node_advertise(const struct orb_metadata *meta) goto out; /* advertise the object */ - ret = ioctl(fd, ORBIOCADVERTISE, (unsigned long)(uintptr_t)meta); + ret = ioctl(fd, ORBIOCADVERTISE, (unsigned long)(uintptr_t)&adv); /* it's OK if it already exists */ - if ((OK != ret) && (EEXIST == errno)) + if ((OK != ret) && (EEXIST == errno)) { ret = OK; + } out: @@ -858,7 +938,7 @@ out: * advertisers. */ int -node_open(Flavor f, const struct orb_metadata *meta, const void *data, bool advertiser) +node_open(Flavor f, const struct orb_metadata *meta, const void *data, bool advertiser, int *instance = nullptr, int priority = ORB_PRIO_DEFAULT) { char path[orb_maxpath]; int fd, ret; @@ -883,7 +963,7 @@ node_open(Flavor f, const struct orb_metadata *meta, const void *data, bool adve /* * Generate the path to the node and try to open it. */ - ret = node_mkpath(path, f, meta); + ret = node_mkpath(path, f, meta, instance); if (ret != OK) { errno = -ret; @@ -893,15 +973,34 @@ node_open(Flavor f, const struct orb_metadata *meta, const void *data, bool adve /* open the path as either the advertiser or the subscriber */ fd = open(path, (advertiser) ? O_WRONLY : O_RDONLY); + /* if we want to advertise and the node existed, we have to re-try again */ + if ((fd >= 0) && (instance != nullptr) && (advertiser)) { + /* close the fd, we want a new one */ + close(fd); + /* the node_advertise call will automatically go for the next free entry */ + fd = -1; + } + /* we may need to advertise the node... */ if (fd < 0) { /* try to create the node */ - ret = node_advertise(meta); + ret = node_advertise(meta, instance, priority); + + if (ret == OK) { + /* update the path, as it might have been updated during the node_advertise call */ + ret = node_mkpath(path, f, meta, instance); + + if (ret != OK) { + errno = -ret; + return ERROR; + } + } /* on success, try the open again */ - if (ret == OK) + if (ret == OK) { fd = open(path, (advertiser) ? O_WRONLY : O_RDONLY); + } } if (fd < 0) { @@ -917,12 +1016,18 @@ node_open(Flavor f, const struct orb_metadata *meta, const void *data, bool adve orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data) +{ + return orb_advertise_multi(meta, data, nullptr, ORB_PRIO_DEFAULT); +} + +orb_advert_t +orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance, int priority) { int result, fd; orb_advert_t advertiser; /* open the node as an advertiser */ - fd = node_open(PUBSUB, meta, data, true); + fd = node_open(PUBSUB, meta, data, true, instance, priority); if (fd == ERROR) return ERROR; @@ -933,7 +1038,7 @@ orb_advertise(const struct orb_metadata *meta, const void *data) return ERROR; /* the advertiser must perform an initial publish to initialise the object */ - result= orb_publish(meta, advertiser, data); + result = orb_publish(meta, advertiser, data); if (result == ERROR) return ERROR; @@ -946,6 +1051,13 @@ orb_subscribe(const struct orb_metadata *meta) return node_open(PUBSUB, meta, nullptr, false); } +int +orb_subscribe_multi(const struct orb_metadata *meta, unsigned instance) +{ + int inst = instance; + return node_open(PUBSUB, meta, nullptr, false, &inst); +} + int orb_unsubscribe(int handle) { @@ -988,6 +1100,12 @@ orb_stat(int handle, uint64_t *time) return ioctl(handle, ORBIOCLASTUPDATE, (unsigned long)(uintptr_t)time); } +int +orb_priority(int handle, int *priority) +{ + return ioctl(handle, ORBIOCGPRIORITY, (unsigned long)(uintptr_t)priority); +} + int orb_set_interval(int handle, unsigned interval) { diff --git a/src/modules/uORB/uORB.h b/src/modules/uORB/uORB.h index beb23f61df..a0ad752735 100644 --- a/src/modules/uORB/uORB.h +++ b/src/modules/uORB/uORB.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -56,6 +56,25 @@ struct orb_metadata { typedef const struct orb_metadata *orb_id_t; +/** + * Maximum number of multi topic instances + */ +#define ORB_MULTI_MAX_INSTANCES 4 + +/** + * Topic priority. + * Relevant for multi-topics / topic groups + */ +enum ORB_PRIO { + ORB_PRIO_MIN = 0, + ORB_PRIO_VERY_LOW = 25, + ORB_PRIO_LOW = 50, + ORB_PRIO_DEFAULT = 75, + ORB_PRIO_HIGH = 100, + ORB_PRIO_VERY_HIGH = 125, + ORB_PRIO_MAX = 255 +}; + /** * Generates a pointer to the uORB metadata structure for * a given topic. @@ -128,7 +147,7 @@ typedef const struct orb_metadata *orb_id_t; #define ORB_DEFINE(_name, _struct) \ const struct orb_metadata __orb_##_name = { \ #_name, \ - sizeof(_struct) \ + sizeof(_struct), \ }; struct hack __BEGIN_DECLS @@ -167,6 +186,34 @@ typedef intptr_t orb_advert_t; */ extern orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data) __EXPORT; +/** + * Advertise as the publisher of a topic. + * + * This performs the initial advertisement of a topic; it creates the topic + * node in /obj if required and publishes the initial data. + * + * Any number of advertisers may publish to a topic; publications are atomic + * but co-ordination between publishers is not provided by the ORB. + * + * @param meta The uORB metadata (usually from the ORB_ID() macro) + * for the topic. + * @param data A pointer to the initial data to be published. + * For topics updated by interrupt handlers, the advertisement + * must be performed from non-interrupt context. + * @param instance Pointer to an integer which will yield the instance ID (0-based) + * of the publication. + * @param priority The priority of the instance. If a subscriber subscribes multiple + * instances, the priority allows the subscriber to prioritize the best + * data source as long as its available. + * @return ERROR on error, otherwise returns a handle + * that can be used to publish to the topic. + * If the topic in question is not known (due to an + * ORB_DEFINE with no corresponding ORB_DECLARE) + * this function will return -1 and set errno to ENOENT. + */ +extern orb_advert_t orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance, int priority) __EXPORT; + + /** * Publish new data to a topic. * @@ -210,6 +257,8 @@ extern int orb_publish(const struct orb_metadata *meta, orb_advert_t handle, con */ extern int orb_subscribe(const struct orb_metadata *meta) __EXPORT; +extern int orb_subscribe_multi(const struct orb_metadata *meta, unsigned instance) __EXPORT; + /** * Unsubscribe from a topic. * @@ -266,6 +315,18 @@ extern int orb_check(int handle, bool *updated) __EXPORT; */ extern int orb_stat(int handle, uint64_t *time) __EXPORT; +/** + * Return the priority of the topic + * + * @param handle A handle returned from orb_subscribe. + * @param priority Returns the priority of this topic. This is only relevant for + * topics which are published by multiple publishers (e.g. mag0, mag1, etc.) + * and allows a subscriber to automatically pick the topic with the highest + * priority, independent of the startup order of the associated publishers. + * @return OK on success, ERROR otherwise with errno set accordingly. + */ +extern int orb_priority(int handle, int *priority) __EXPORT; + /** * Set the minimum interval between which updates are seen for a subscription. * From 7932e2eda238b1d480fdc9de71bb1b5fcaa3e373 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 25 Jan 2015 16:16:15 +0100 Subject: [PATCH 622/652] Add top to test build --- makefiles/config_px4fmu-v2_test.mk | 1 + src/modules/uORB/uORB.cpp | 6 +++++- 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/makefiles/config_px4fmu-v2_test.mk b/makefiles/config_px4fmu-v2_test.mk index bc6723e5f5..91fc2a7512 100644 --- a/makefiles/config_px4fmu-v2_test.mk +++ b/makefiles/config_px4fmu-v2_test.mk @@ -32,6 +32,7 @@ MODULES += systemcmds/tests MODULES += systemcmds/nshterm MODULES += systemcmds/mtd MODULES += systemcmds/ver +MODULES += systemcmds/top # # Testing modules diff --git a/src/modules/uORB/uORB.cpp b/src/modules/uORB/uORB.cpp index cfea12f044..c4de996bb9 100644 --- a/src/modules/uORB/uORB.cpp +++ b/src/modules/uORB/uORB.cpp @@ -641,9 +641,13 @@ ORBDevMaster::ioctl(struct file *filp, int cmd, unsigned long arg) if (ret != OK) { delete node; free((void *)objname); + free((void *)devpath); } - } while (ret != OK && (group_tries++ < max_group_tries)); + /* try with next larger index */ + group_tries++; + + } while (ret != OK && (group_tries < max_group_tries)); if (group_tries >= max_group_tries) { ret = -ENOMEM; From 4f9a6273cb28e50fdabdf1f60c39da6fe0b4fcd6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 25 Jan 2015 17:01:39 +0100 Subject: [PATCH 623/652] uORB: correct pub creation for multi-topics --- src/modules/uORB/uORB.cpp | 21 ++++++++++++--------- src/modules/uORB/uORB.h | 2 +- 2 files changed, 13 insertions(+), 10 deletions(-) diff --git a/src/modules/uORB/uORB.cpp b/src/modules/uORB/uORB.cpp index c4de996bb9..b3a9bedb1e 100644 --- a/src/modules/uORB/uORB.cpp +++ b/src/modules/uORB/uORB.cpp @@ -599,13 +599,6 @@ ORBDevMaster::ioctl(struct file *filp, int cmd, unsigned long arg) return ret; } - /* driver wants a permanent copy of the node name, so make one here */ - objname = strdup(meta->o_name); - - if (objname == nullptr) { - return -ENOMEM; - } - /* ensure that only one advertiser runs through this critical section */ lock(); @@ -622,9 +615,20 @@ ORBDevMaster::ioctl(struct file *filp, int cmd, unsigned long arg) *(adv->instance) = group_tries; } + /* driver wants a permanent copy of the node name, so make one here */ + objname = strdup(meta->o_name); + + if (objname == nullptr) { + return -ENOMEM; + } + /* driver wants a permanent copy of the path, so make one here */ devpath = strdup(nodepath); + if (devpath == nullptr) { + return -ENOMEM; + } + /* construct the new node */ node = new ORBDevNode(meta, objname, devpath, adv->priority); @@ -644,12 +648,11 @@ ORBDevMaster::ioctl(struct file *filp, int cmd, unsigned long arg) free((void *)devpath); } - /* try with next larger index */ group_tries++; } while (ret != OK && (group_tries < max_group_tries)); - if (group_tries >= max_group_tries) { + if (group_tries > max_group_tries) { ret = -ENOMEM; } diff --git a/src/modules/uORB/uORB.h b/src/modules/uORB/uORB.h index a0ad752735..30cd598809 100644 --- a/src/modules/uORB/uORB.h +++ b/src/modules/uORB/uORB.h @@ -59,7 +59,7 @@ typedef const struct orb_metadata *orb_id_t; /** * Maximum number of multi topic instances */ -#define ORB_MULTI_MAX_INSTANCES 4 +#define ORB_MULTI_MAX_INSTANCES 3 /** * Topic priority. From 50a58db7e605c61bb50cfb154cbc43bf9c4c67ae Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 25 Jan 2015 17:50:57 +0100 Subject: [PATCH 624/652] uORB: Ensure correct instance initialization, port complete mag API to new interface --- src/modules/uORB/objects_common.cpp | 4 +--- src/modules/uORB/uORB.cpp | 5 +++++ 2 files changed, 6 insertions(+), 3 deletions(-) diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 20a9bcc43c..a8305ebeaf 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -46,9 +46,7 @@ #include #include -ORB_DEFINE(sensor_mag0, struct mag_report); -ORB_DEFINE(sensor_mag1, struct mag_report); -ORB_DEFINE(sensor_mag2, struct mag_report); +ORB_DEFINE(sensor_mag, struct mag_report); #include ORB_DEFINE(sensor_accel0, struct accel_report); diff --git a/src/modules/uORB/uORB.cpp b/src/modules/uORB/uORB.cpp index b3a9bedb1e..6f021459ce 100644 --- a/src/modules/uORB/uORB.cpp +++ b/src/modules/uORB/uORB.cpp @@ -592,6 +592,11 @@ ORBDevMaster::ioctl(struct file *filp, int cmd, unsigned long arg) char nodepath[orb_maxpath]; ORBDevNode *node; + /* set instance to zero - we could allow selective multi-pubs later based on value */ + if (adv->instance != nullptr) { + *(adv->instance) = 0; + } + /* construct a path to the node - this also checks the node name */ ret = node_mkpath(nodepath, _flavor, meta, adv->instance); From f30b02272beee4ad5c137a25c726ec158f0135de Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 25 Jan 2015 17:51:33 +0100 Subject: [PATCH 625/652] Move HMC5883 to new interface --- src/drivers/drv_mag.h | 6 ++---- src/drivers/hmc5883/hmc5883.cpp | 35 ++++++++------------------------- 2 files changed, 10 insertions(+), 31 deletions(-) diff --git a/src/drivers/drv_mag.h b/src/drivers/drv_mag.h index d341e89472..d8fe1ae7a3 100644 --- a/src/drivers/drv_mag.h +++ b/src/drivers/drv_mag.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -81,9 +81,7 @@ struct mag_scale { /* * ObjDev tag for raw magnetometer data. */ -ORB_DECLARE(sensor_mag0); -ORB_DECLARE(sensor_mag1); -ORB_DECLARE(sensor_mag2); +ORB_DECLARE(sensor_mag); /* * mag device types, for _device_id diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index a06be72d9a..b1605a5b09 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -69,7 +69,6 @@ #include #include -#include #include #include @@ -157,10 +156,9 @@ private: float _range_ga; bool _collect_phase; int _class_instance; + int _orb_class_instance; orb_advert_t _mag_topic; - orb_advert_t _subsystem_pub; - orb_id_t _mag_orb_id; perf_counter_t _sample_perf; perf_counter_t _comms_errors; @@ -348,9 +346,8 @@ HMC5883::HMC5883(device::Device *interface, const char *path, enum Rotation rota _range_ga(1.3f), _collect_phase(false), _class_instance(-1), + _orb_class_instance(-1), _mag_topic(-1), - _subsystem_pub(-1), - _mag_orb_id(nullptr), _sample_perf(perf_alloc(PC_ELAPSED, "hmc5883_read")), _comms_errors(perf_alloc(PC_COUNT, "hmc5883_comms_errors")), _buffer_overflows(perf_alloc(PC_COUNT, "hmc5883_buffer_overflows")), @@ -419,7 +416,6 @@ HMC5883::init() reset(); _class_instance = register_class_devname(MAG_DEVICE_PATH); - _mag_orb_id = ORB_ID_TRIPLE(sensor_mag, _class_instance); ret = OK; /* sensor is ok, but not calibrated */ @@ -850,6 +846,7 @@ HMC5883::collect() perf_begin(_sample_perf); struct mag_report new_report; + bool sensor_is_onboard = false; /* this should be fairly close to the end of the measurement, so the best approximation of the time */ new_report.timestamp = hrt_absolute_time(); @@ -902,7 +899,8 @@ HMC5883::collect() // XXX revisit for SPI part, might require a bus type IOCTL unsigned dummy; - if (!_interface->ioctl(MAGIOCGEXTERNAL, dummy)) { + sensor_is_onboard = !_interface->ioctl(MAGIOCGEXTERNAL, dummy); + if (sensor_is_onboard) { // convert onboard so it matches offboard for the // scaling below report.y = -report.y; @@ -925,9 +923,10 @@ HMC5883::collect() if (_mag_topic != -1) { /* publish it */ - orb_publish(_mag_orb_id, _mag_topic, &new_report); + orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_report); } else { - _mag_topic = orb_advertise(_mag_orb_id, &new_report); + _mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &new_report, + &_orb_class_instance, (sensor_is_onboard) ? ORB_PRIO_HIGH : ORB_PRIO_MAX); if (_mag_topic < 0) debug("ADVERT FAIL"); @@ -1185,24 +1184,6 @@ int HMC5883::check_calibration() warnx("mag cal status changed %s%s", (scale_valid) ? "" : "scale invalid ", (offset_valid) ? "" : "offset invalid"); _calibrated = (offset_valid && scale_valid); - - - // XXX Change advertisement - - /* notify about state change */ - struct subsystem_info_s info = { - true, - true, - _calibrated, - SUBSYSTEM_TYPE_MAG}; - - if (!(_pub_blocked)) { - if (_subsystem_pub > 0) { - orb_publish(ORB_ID(subsystem_info), _subsystem_pub, &info); - } else { - _subsystem_pub = orb_advertise(ORB_ID(subsystem_info), &info); - } - } } /* return 0 if calibrated, 1 else */ From 114465aba4b65ecdc9ffe3f4125afb2391fbdc2b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 25 Jan 2015 17:51:50 +0100 Subject: [PATCH 626/652] Move LSM303D mag to new multi-pub interface --- src/drivers/lsm303d/lsm303d.cpp | 22 ++++------------------ 1 file changed, 4 insertions(+), 18 deletions(-) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 57754e4c0d..6b65965b49 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -507,7 +507,7 @@ private: LSM303D *_parent; orb_advert_t _mag_topic; - orb_id_t _mag_orb_id; + int _mag_orb_class_instance; int _mag_class_instance; void measure(); @@ -641,21 +641,7 @@ LSM303D::init() _mag_reports->get(&mrp); /* measurement will have generated a report, publish */ - switch (_mag->_mag_class_instance) { - case CLASS_DEVICE_PRIMARY: - _mag->_mag_orb_id = ORB_ID(sensor_mag0); - break; - - case CLASS_DEVICE_SECONDARY: - _mag->_mag_orb_id = ORB_ID(sensor_mag1); - break; - - case CLASS_DEVICE_TERTIARY: - _mag->_mag_orb_id = ORB_ID(sensor_mag2); - break; - } - - _mag->_mag_topic = orb_advertise(_mag->_mag_orb_id, &mrp); + _mag->_mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &mrp, &_mag->_mag_orb_class_instance, ORB_PRIO_LOW); if (_mag->_mag_topic < 0) { warnx("ADVERT ERR"); @@ -1641,7 +1627,7 @@ LSM303D::mag_measure() if (!(_pub_blocked)) { /* publish it */ - orb_publish(_mag->_mag_orb_id, _mag->_mag_topic, &mag_report); + orb_publish(ORB_ID(sensor_mag), _mag->_mag_topic, &mag_report); } _mag_read++; @@ -1742,7 +1728,7 @@ LSM303D_mag::LSM303D_mag(LSM303D *parent) : CDev("LSM303D_mag", LSM303D_DEVICE_PATH_MAG), _parent(parent), _mag_topic(-1), - _mag_orb_id(nullptr), + _mag_orb_class_instance(-1), _mag_class_instance(-1) { } From 801e9ed4fbc66d0aa359184be9f4ed3899f10096 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 25 Jan 2015 17:52:36 +0100 Subject: [PATCH 627/652] Commander: move sensor interface for mag to new multi-sub --- src/modules/commander/mag_calibration.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 53013fdb97..2afb9a4408 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -149,7 +149,7 @@ int do_mag_calibration(int mavlink_fd) } if (res == OK) { - int sub_mag = orb_subscribe(ORB_ID(sensor_mag0)); + int sub_mag = orb_subscribe_multi(ORB_ID(sensor_mag), 0); if (sub_mag < 0) { mavlink_log_critical(mavlink_fd, "No mag found, abort"); @@ -179,7 +179,7 @@ int do_mag_calibration(int mavlink_fd) int poll_ret = poll(fds, 1, 1000); if (poll_ret > 0) { - orb_copy(ORB_ID(sensor_mag0), sub_mag, &mag); + orb_copy(ORB_ID(sensor_mag), sub_mag, &mag); x[calibration_counter] = mag.x; y[calibration_counter] = mag.y; From f641a26feee76f4a811fec34834cc818a8e8f76a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 25 Jan 2015 17:53:04 +0100 Subject: [PATCH 628/652] Move MAVLink to new mag interface --- src/modules/mavlink/mavlink_receiver.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index e5a21651d2..5aa623e955 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1086,10 +1086,11 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) mag.z = imu.zmag; if (_mag_pub < 0) { - _mag_pub = orb_advertise(ORB_ID(sensor_mag0), &mag); + /* publish to the first mag topic */ + _mag_pub = orb_advertise(ORB_ID(sensor_mag), &mag); } else { - orb_publish(ORB_ID(sensor_mag0), _mag_pub, &mag); + orb_publish(ORB_ID(sensor_mag), _mag_pub, &mag); } } From c3eb10560ba255fdda2454e6044bb4efac35b38f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 25 Jan 2015 17:53:15 +0100 Subject: [PATCH 629/652] Move sensors to new mag interface --- src/modules/sensors/sensors.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 793b7c2b65..3f66d7995e 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1249,7 +1249,7 @@ Sensors::mag_poll(struct sensor_combined_s &raw) if (mag_updated) { struct mag_report mag_report; - orb_copy(ORB_ID(sensor_mag0), _mag_sub, &mag_report); + orb_copy(ORB_ID(sensor_mag), _mag_sub, &mag_report); math::Vector<3> vect(mag_report.x, mag_report.y, mag_report.z); @@ -1278,7 +1278,7 @@ Sensors::mag_poll(struct sensor_combined_s &raw) if (mag_updated) { struct mag_report mag_report; - orb_copy(ORB_ID(sensor_mag1), _mag1_sub, &mag_report); + orb_copy(ORB_ID(sensor_mag), _mag1_sub, &mag_report); raw.magnetometer1_raw[0] = mag_report.x_raw; raw.magnetometer1_raw[1] = mag_report.y_raw; @@ -1292,7 +1292,7 @@ Sensors::mag_poll(struct sensor_combined_s &raw) if (mag_updated) { struct mag_report mag_report; - orb_copy(ORB_ID(sensor_mag2), _mag2_sub, &mag_report); + orb_copy(ORB_ID(sensor_mag), _mag2_sub, &mag_report); raw.magnetometer2_raw[0] = mag_report.x_raw; raw.magnetometer2_raw[1] = mag_report.y_raw; @@ -1945,13 +1945,13 @@ Sensors::task_main() */ _gyro_sub = orb_subscribe(ORB_ID(sensor_gyro0)); _accel_sub = orb_subscribe(ORB_ID(sensor_accel0)); - _mag_sub = orb_subscribe(ORB_ID(sensor_mag0)); + _mag_sub = orb_subscribe_multi(ORB_ID(sensor_mag), 0); _gyro1_sub = orb_subscribe(ORB_ID(sensor_gyro1)); _accel1_sub = orb_subscribe(ORB_ID(sensor_accel1)); - _mag1_sub = orb_subscribe(ORB_ID(sensor_mag1)); + _mag1_sub = orb_subscribe_multi(ORB_ID(sensor_mag), 1); _gyro2_sub = orb_subscribe(ORB_ID(sensor_gyro2)); _accel2_sub = orb_subscribe(ORB_ID(sensor_accel2)); - _mag2_sub = orb_subscribe(ORB_ID(sensor_mag2)); + _mag2_sub = orb_subscribe_multi(ORB_ID(sensor_mag), 2); _rc_sub = orb_subscribe(ORB_ID(input_rc)); _baro_sub = orb_subscribe(ORB_ID(sensor_baro0)); _baro1_sub = orb_subscribe(ORB_ID(sensor_baro1)); From cc7a00b96e658f5d36763ef90ebac7fa813b55af Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 25 Jan 2015 17:54:09 +0100 Subject: [PATCH 630/652] Disable UAVCAN build until sensors use all new-style API and UAVCAN sensors base class can be reworked to use it consistently --- makefiles/config_px4fmu-v2_default.mk | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index 3abebd82fa..68a65efed1 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -70,7 +70,7 @@ MODULES += modules/commander MODULES += modules/navigator MODULES += modules/mavlink MODULES += modules/gpio_led -MODULES += modules/uavcan +#MODULES += modules/uavcan MODULES += modules/land_detector # From d851a630d874f639e0861dc8411405f83ee23769 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 25 Jan 2015 21:05:38 +0100 Subject: [PATCH 631/652] Move all drivers to multi pub/sub API --- src/drivers/drv_accel.h | 6 ++-- src/drivers/drv_baro.h | 6 ++-- src/drivers/drv_blinkm.h | 2 +- src/drivers/drv_gyro.h | 6 ++-- src/drivers/l3gd20/l3gd20.cpp | 40 ++++++++++----------- src/drivers/lsm303d/lsm303d.cpp | 42 +++++++++++----------- src/drivers/mpu6000/mpu6000.cpp | 63 +++++++++++++-------------------- src/drivers/ms5611/ms5611.cpp | 21 +++++++---- 8 files changed, 87 insertions(+), 99 deletions(-) diff --git a/src/drivers/drv_accel.h b/src/drivers/drv_accel.h index 1f98d966bd..1eca6155b0 100644 --- a/src/drivers/drv_accel.h +++ b/src/drivers/drv_accel.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -81,9 +81,7 @@ struct accel_scale { /* * ObjDev tag for raw accelerometer data. */ -ORB_DECLARE(sensor_accel0); -ORB_DECLARE(sensor_accel1); -ORB_DECLARE(sensor_accel2); +ORB_DECLARE(sensor_accel); /* * ioctl() definitions diff --git a/src/drivers/drv_baro.h b/src/drivers/drv_baro.h index 3e28d3d3d9..3d275d6198 100644 --- a/src/drivers/drv_baro.h +++ b/src/drivers/drv_baro.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -63,9 +63,7 @@ struct baro_report { /* * ObjDev tag for raw barometer data. */ -ORB_DECLARE(sensor_baro0); -ORB_DECLARE(sensor_baro1); -ORB_DECLARE(sensor_baro2); +ORB_DECLARE(sensor_baro); /* * ioctl() definitions diff --git a/src/drivers/drv_blinkm.h b/src/drivers/drv_blinkm.h index 9c278f6c50..b757da5459 100644 --- a/src/drivers/drv_blinkm.h +++ b/src/drivers/drv_blinkm.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions diff --git a/src/drivers/drv_gyro.h b/src/drivers/drv_gyro.h index 41b013a443..5e0334a187 100644 --- a/src/drivers/drv_gyro.h +++ b/src/drivers/drv_gyro.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -81,9 +81,7 @@ struct gyro_scale { /* * ObjDev tag for raw gyro data. */ -ORB_DECLARE(sensor_gyro0); -ORB_DECLARE(sensor_gyro1); -ORB_DECLARE(sensor_gyro2); +ORB_DECLARE(sensor_gyro); /* * ioctl() definitions diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 08bc1fead8..bd1bd9f86d 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -33,7 +33,7 @@ /** * @file l3gd20.cpp - * Driver for the ST L3GD20 MEMS gyro connected via SPI. + * Driver for the ST L3GD20 MEMS and L3GD20H mems gyros connected via SPI. * * Note: With the exception of the self-test feature, the ST L3G4200D is * also supported by this driver. @@ -179,6 +179,12 @@ static const int ERROR = -1; #define L3GD20_DEFAULT_FILTER_FREQ 30 #define L3GD20_TEMP_OFFSET_CELSIUS 40 +#ifdef PX4_SPI_BUS_EXT +#define EXTERNAL_BUS PX4_SPI_BUS_EXT +#else +#define EXTERNAL_BUS 0 +#endif + #ifndef SENSOR_BOARD_ROTATION_DEFAULT #define SENSOR_BOARD_ROTATION_DEFAULT SENSOR_BOARD_ROTATION_270_DEG #endif @@ -221,7 +227,7 @@ private: float _gyro_range_scale; float _gyro_range_rad_s; orb_advert_t _gyro_topic; - orb_id_t _orb_id; + int _orb_class_instance; int _class_instance; unsigned _current_rate; @@ -273,6 +279,13 @@ private: */ void disable_i2c(); + /** + * Get the internal / external state + * + * @return true if the sensor is not on the main MCU board + */ + bool is_external() { return (_bus == EXTERNAL_BUS); } + /** * Static trampoline from the hrt_call context; because we don't have a * generic hrt wrapper yet. @@ -391,7 +404,7 @@ L3GD20::L3GD20(int bus, const char* path, spi_dev_e device, enum Rotation rotati _gyro_range_scale(0.0f), _gyro_range_rad_s(0.0f), _gyro_topic(-1), - _orb_id(nullptr), + _orb_class_instance(-1), _class_instance(-1), _current_rate(0), _orientation(SENSOR_BOARD_ROTATION_DEFAULT), @@ -456,20 +469,6 @@ L3GD20::init() _class_instance = register_class_devname(GYRO_DEVICE_PATH); - switch (_class_instance) { - case CLASS_DEVICE_PRIMARY: - _orb_id = ORB_ID(sensor_gyro0); - break; - - case CLASS_DEVICE_SECONDARY: - _orb_id = ORB_ID(sensor_gyro1); - break; - - case CLASS_DEVICE_TERTIARY: - _orb_id = ORB_ID(sensor_gyro2); - break; - } - reset(); measure(); @@ -478,7 +477,8 @@ L3GD20::init() struct gyro_report grp; _reports->get(&grp); - _gyro_topic = orb_advertise(_orb_id, &grp); + _gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &grp, + &_orb_class_instance, (is_external()) ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT); if (_gyro_topic < 0) { debug("failed to create sensor_gyro publication"); @@ -1050,7 +1050,7 @@ L3GD20::measure() /* publish for subscribers */ if (!(_pub_blocked)) { /* publish it */ - orb_publish(_orb_id, _gyro_topic, &report); + orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &report); } _read++; diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 6b65965b49..ff7068936a 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -211,6 +211,12 @@ static const int ERROR = -1; #define LSM303D_ONE_G 9.80665f +#ifdef PX4_SPI_BUS_EXT +#define EXTERNAL_BUS PX4_SPI_BUS_EXT +#else +#define EXTERNAL_BUS 0 +#endif + extern "C" { __EXPORT int lsm303d_main(int argc, char *argv[]); } @@ -275,7 +281,7 @@ private: unsigned _mag_samplerate; orb_advert_t _accel_topic; - orb_id_t _accel_orb_id; + int _accel_orb_class_instance; int _accel_class_instance; unsigned _accel_read; @@ -329,6 +335,13 @@ private: */ void disable_i2c(); + /** + * Get the internal / external state + * + * @return true if the sensor is not on the main MCU board + */ + bool is_external() { return (_bus == EXTERNAL_BUS); } + /** * Static trampoline from the hrt_call context; because we don't have a * generic hrt wrapper yet. @@ -539,7 +552,7 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device, enum Rotation rota _mag_range_scale(0.0f), _mag_samplerate(0), _accel_topic(-1), - _accel_orb_id(nullptr), + _accel_orb_class_instance(-1), _accel_class_instance(-1), _accel_read(0), _mag_read(0), @@ -618,7 +631,6 @@ LSM303D::init() if (_accel_reports == nullptr) goto out; - /* advertise accel topic */ _mag_reports = new RingBuffer(2, sizeof(mag_report)); if (_mag_reports == nullptr) @@ -641,7 +653,8 @@ LSM303D::init() _mag_reports->get(&mrp); /* measurement will have generated a report, publish */ - _mag->_mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &mrp, &_mag->_mag_orb_class_instance, ORB_PRIO_LOW); + _mag->_mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &mrp, + &_mag->_mag_orb_class_instance, ORB_PRIO_LOW); if (_mag->_mag_topic < 0) { warnx("ADVERT ERR"); @@ -654,21 +667,8 @@ LSM303D::init() _accel_reports->get(&arp); /* measurement will have generated a report, publish */ - switch (_accel_class_instance) { - case CLASS_DEVICE_PRIMARY: - _accel_orb_id = ORB_ID(sensor_accel0); - break; - - case CLASS_DEVICE_SECONDARY: - _accel_orb_id = ORB_ID(sensor_accel1); - break; - - case CLASS_DEVICE_TERTIARY: - _accel_orb_id = ORB_ID(sensor_accel2); - break; - } - - _accel_topic = orb_advertise(_accel_orb_id, &arp); + _accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &arp, + &_accel_orb_class_instance, (is_external()) ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT); if (_accel_topic < 0) { warnx("ADVERT ERR"); @@ -1556,7 +1556,7 @@ LSM303D::measure() if (!(_pub_blocked)) { /* publish it */ - orb_publish(_accel_orb_id, _accel_topic, &accel_report); + orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report); } _accel_read++; diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 2be7582442..e4e9824902 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -175,6 +175,12 @@ #define MPU6000_ONE_G 9.80665f +#ifdef PX4_SPI_BUS_EXT +#define EXTERNAL_BUS PX4_SPI_BUS_EXT +#else +#define EXTERNAL_BUS 0 +#endif + /* the MPU6000 can only handle high SPI bus speeds on the sensor and interrupt status registers. All other registers have a maximum 1MHz @@ -234,7 +240,7 @@ private: float _accel_range_scale; float _accel_range_m_s2; orb_advert_t _accel_topic; - orb_id_t _accel_orb_id; + int _accel_orb_class_instance; int _accel_class_instance; RingBuffer *_gyro_reports; @@ -360,6 +366,13 @@ private: */ uint16_t swap16(uint16_t val) { return (val >> 8) | (val << 8); } + /** + * Get the internal / external state + * + * @return true if the sensor is not on the main MCU board + */ + bool is_external() { return (_bus == EXTERNAL_BUS); } + /** * Measurement self test * @@ -457,7 +470,7 @@ protected: private: MPU6000 *_parent; orb_advert_t _gyro_topic; - orb_id_t _gyro_orb_id; + int _gyro_orb_class_instance; int _gyro_class_instance; /* do not allow to copy this class due to pointer data members */ @@ -479,7 +492,7 @@ MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev _accel_range_scale(0.0f), _accel_range_m_s2(0.0f), _accel_topic(-1), - _accel_orb_id(nullptr), + _accel_orb_class_instance(-1), _accel_class_instance(-1), _gyro_reports(nullptr), _gyro_scale{}, @@ -613,22 +626,8 @@ MPU6000::init() _accel_reports->get(&arp); /* measurement will have generated a report, publish */ - switch (_accel_class_instance) { - case CLASS_DEVICE_PRIMARY: - _accel_orb_id = ORB_ID(sensor_accel0); - break; - - case CLASS_DEVICE_SECONDARY: - _accel_orb_id = ORB_ID(sensor_accel1); - break; - - case CLASS_DEVICE_TERTIARY: - _accel_orb_id = ORB_ID(sensor_accel2); - break; - - } - - _accel_topic = orb_advertise(_accel_orb_id, &arp); + _accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &arp, + &_accel_orb_class_instance, (is_external()) ? ORB_PRIO_MAX : ORB_PRIO_HIGH); if (_accel_topic < 0) { warnx("ADVERT FAIL"); @@ -639,22 +638,8 @@ MPU6000::init() struct gyro_report grp; _gyro_reports->get(&grp); - switch (_gyro->_gyro_class_instance) { - case CLASS_DEVICE_PRIMARY: - _gyro->_gyro_orb_id = ORB_ID(sensor_gyro0); - break; - - case CLASS_DEVICE_SECONDARY: - _gyro->_gyro_orb_id = ORB_ID(sensor_gyro1); - break; - - case CLASS_DEVICE_TERTIARY: - _gyro->_gyro_orb_id = ORB_ID(sensor_gyro2); - break; - - } - - _gyro->_gyro_topic = orb_advertise(_gyro->_gyro_orb_id, &grp); + _gyro->_gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &grp, + &_gyro->_gyro_orb_class_instance, (is_external()) ? ORB_PRIO_MAX : ORB_PRIO_HIGH); if (_gyro->_gyro_topic < 0) { warnx("ADVERT FAIL"); @@ -1763,12 +1748,12 @@ MPU6000::measure() perf_begin(_controller_latency_perf); perf_begin(_system_latency_perf); /* publish it */ - orb_publish(_accel_orb_id, _accel_topic, &arb); + orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb); } if (!(_pub_blocked)) { /* publish it */ - orb_publish(_gyro->_gyro_orb_id, _gyro->_gyro_topic, &grb); + orb_publish(ORB_ID(sensor_gyro), _gyro->_gyro_topic, &grb); } /* stop measuring */ @@ -1818,7 +1803,7 @@ MPU6000_gyro::MPU6000_gyro(MPU6000 *parent, const char *path) : CDev("MPU6000_gyro", path), _parent(parent), _gyro_topic(-1), - _gyro_orb_id(nullptr), + _gyro_orb_class_instance(-1), _gyro_class_instance(-1) { } diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp index 0a793cbc97..75b1f65fdb 100644 --- a/src/drivers/ms5611/ms5611.cpp +++ b/src/drivers/ms5611/ms5611.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -57,6 +57,7 @@ #include #include +#include #include #include @@ -134,8 +135,7 @@ protected: unsigned _msl_pressure; /* in Pa */ orb_advert_t _baro_topic; - orb_id_t _orb_id; - + int _orb_class_instance; int _class_instance; perf_counter_t _sample_perf; @@ -171,6 +171,13 @@ protected: */ void cycle(); + /** + * Get the internal / external state + * + * @return true if the sensor is not on the main MCU board + */ + bool is_external() { return (_orb_class_instance == 0); /* XXX put this into the interface class */ } + /** * Static trampoline from the workq context; because we don't have a * generic workq wrapper yet. @@ -210,6 +217,7 @@ MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf, const char* _SENS(0), _msl_pressure(101325), _baro_topic(-1), + _orb_class_instance(-1), _class_instance(-1), _sample_perf(perf_alloc(PC_ELAPSED, "ms5611_read")), _measure_perf(perf_alloc(PC_ELAPSED, "ms5611_measure")), @@ -263,7 +271,6 @@ MS5611::init() /* register alternate interfaces if we have to */ _class_instance = register_class_devname(BARO_DEVICE_PATH); - _orb_id = ORB_ID_TRIPLE(sensor_baro, _class_instance); struct baro_report brp; /* do a first measurement cycle to populate reports with valid data */ @@ -303,7 +310,9 @@ MS5611::init() ret = OK; - _baro_topic = orb_advertise(_orb_id, &brp); + _baro_topic = orb_advertise_multi(ORB_ID(sensor_baro), &brp, + &_orb_class_instance, (is_external()) ? ORB_PRIO_HIGH : ORB_PRIO_DEFAULT); + if (_baro_topic < 0) { warnx("failed to create sensor_baro publication"); @@ -725,7 +734,7 @@ MS5611::collect() /* publish it */ if (!(_pub_blocked)) { /* publish it */ - orb_publish(_orb_id, _baro_topic, &report); + orb_publish(ORB_ID(sensor_baro), _baro_topic, &report); } if (_reports->force(&report)) { From 7f299ea0cc5aedbf1f7913930d592bdc696e0ca9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 25 Jan 2015 21:06:03 +0100 Subject: [PATCH 632/652] Move commander to multi pub/sub API --- src/modules/commander/gyro_calibration.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index 8ab14dd52b..2be0e881ee 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -95,7 +95,7 @@ int do_gyro_calibration(int mavlink_fd) unsigned poll_errcount = 0; /* subscribe to gyro sensor topic */ - int sub_sensor_gyro = orb_subscribe(ORB_ID(sensor_gyro0)); + int sub_sensor_gyro = orb_subscribe_multi(ORB_ID(sensor_gyro), 0); struct gyro_report gyro_report; while (calibration_counter < calibration_count) { @@ -107,7 +107,7 @@ int do_gyro_calibration(int mavlink_fd) int poll_ret = poll(fds, 1, 1000); if (poll_ret > 0) { - orb_copy(ORB_ID(sensor_gyro0), sub_sensor_gyro, &gyro_report); + orb_copy(ORB_ID(sensor_gyro), sub_sensor_gyro, &gyro_report); gyro_scale.x_offset += gyro_report.x; gyro_scale.y_offset += gyro_report.y; gyro_scale.z_offset += gyro_report.z; From 777eda1a89222b9b7b91fd12c92475291ed56ce7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 25 Jan 2015 21:06:27 +0100 Subject: [PATCH 633/652] Move MATLAB example to multi pub/sub API --- .../matlab_csv_serial/matlab_csv_serial.c | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/src/examples/matlab_csv_serial/matlab_csv_serial.c b/src/examples/matlab_csv_serial/matlab_csv_serial.c index a95f45d1ad..145cf99cc4 100644 --- a/src/examples/matlab_csv_serial/matlab_csv_serial.c +++ b/src/examples/matlab_csv_serial/matlab_csv_serial.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2014-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -190,10 +190,10 @@ int matlab_csv_serial_thread_main(int argc, char *argv[]) struct gyro_report gyro1; /* subscribe to parameter changes */ - int accel0_sub = orb_subscribe(ORB_ID(sensor_accel0)); - int accel1_sub = orb_subscribe(ORB_ID(sensor_accel1)); - int gyro0_sub = orb_subscribe(ORB_ID(sensor_gyro0)); - int gyro1_sub = orb_subscribe(ORB_ID(sensor_gyro1)); + int accel0_sub = orb_subscribe_multi(ORB_ID(sensor_accel), 0); + int accel1_sub = orb_subscribe_multi(ORB_ID(sensor_accel), 1); + int gyro0_sub = orb_subscribe_multi(ORB_ID(sensor_gyro), 0); + int gyro1_sub = orb_subscribe_multi(ORB_ID(sensor_gyro), 1); thread_running = true; @@ -224,10 +224,10 @@ int matlab_csv_serial_thread_main(int argc, char *argv[]) /* accel0 update available? */ if (fds[0].revents & POLLIN) { - orb_copy(ORB_ID(sensor_accel0), accel0_sub, &accel0); - orb_copy(ORB_ID(sensor_accel1), accel1_sub, &accel1); - orb_copy(ORB_ID(sensor_gyro0), gyro0_sub, &gyro0); - orb_copy(ORB_ID(sensor_gyro1), gyro1_sub, &gyro1); + orb_copy(ORB_ID(sensor_accel), accel0_sub, &accel0); + orb_copy(ORB_ID(sensor_accel), accel1_sub, &accel1); + orb_copy(ORB_ID(sensor_gyro), gyro0_sub, &gyro0); + orb_copy(ORB_ID(sensor_gyro), gyro1_sub, &gyro1); // write out on accel 0, but collect for all other sensors as they have updates dprintf(serial_fd, "%llu,%d,%d,%d,%d,%d,%d\n", accel0.timestamp, (int)accel0.x_raw, (int)accel0.y_raw, (int)accel0.z_raw, From 83a0f8e5ce2041494f57643246a612f2cd836752 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 25 Jan 2015 21:06:48 +0100 Subject: [PATCH 634/652] Move EKF to multi pub/sub API --- .../ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 923aa28614..d0f5fb6f8a 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -719,7 +719,7 @@ FixedwingEstimator::task_main() * do subscriptions */ _distance_sub = orb_subscribe(ORB_ID(sensor_range_finder)); - _baro_sub = orb_subscribe(ORB_ID(sensor_baro0)); + _baro_sub = orb_subscribe_multi(ORB_ID(sensor_baro), 0); _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); _gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); @@ -1087,7 +1087,7 @@ FixedwingEstimator::task_main() if (baro_updated) { - orb_copy(ORB_ID(sensor_baro0), _baro_sub, &_baro); + orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro); float baro_elapsed = (_baro.timestamp - baro_last) / 1e6f; baro_last = _baro.timestamp; @@ -1216,7 +1216,7 @@ FixedwingEstimator::task_main() initVelNED[2] = _gps.vel_d_m_s; // Set up height correctly - orb_copy(ORB_ID(sensor_baro0), _baro_sub, &_baro); + orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro); _baro_ref_offset = _ekf->states[9]; // this should become zero in the local frame // init filtered gps and baro altitudes From c4cf28fa9529ed12a1c78f0e297863f34644167a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 25 Jan 2015 21:07:06 +0100 Subject: [PATCH 635/652] Move FW att control to multi pub sub API --- src/modules/fw_att_control/fw_att_control_main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 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 6f225bb48f..4cb6c1aef2 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -549,7 +549,7 @@ FixedwingAttitudeControl::vehicle_accel_poll() orb_check(_accel_sub, &accel_updated); if (accel_updated) { - orb_copy(ORB_ID(sensor_accel0), _accel_sub, &_accel); + orb_copy(ORB_ID(sensor_accel), _accel_sub, &_accel); } } @@ -619,7 +619,7 @@ FixedwingAttitudeControl::task_main() */ _att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); _att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - _accel_sub = orb_subscribe(ORB_ID(sensor_accel0)); + _accel_sub = orb_subscribe_multi(ORB_ID(sensor_accel), 0); _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); _vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); From 165e5f5a62eedf1a4776b921b50fb84a3acdd3db Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 25 Jan 2015 21:07:31 +0100 Subject: [PATCH 636/652] Move MAVLink to multi pub/sub API (to first index) --- src/modules/mavlink/mavlink_receiver.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 5aa623e955..cbd24f0d4a 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1043,10 +1043,10 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) gyro.temperature = imu.temperature; if (_gyro_pub < 0) { - _gyro_pub = orb_advertise(ORB_ID(sensor_gyro0), &gyro); + _gyro_pub = orb_advertise(ORB_ID(sensor_gyro), &gyro); } else { - orb_publish(ORB_ID(sensor_gyro0), _gyro_pub, &gyro); + orb_publish(ORB_ID(sensor_gyro), _gyro_pub, &gyro); } } @@ -1065,10 +1065,10 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) accel.temperature = imu.temperature; if (_accel_pub < 0) { - _accel_pub = orb_advertise(ORB_ID(sensor_accel0), &accel); + _accel_pub = orb_advertise(ORB_ID(sensor_accel), &accel); } else { - orb_publish(ORB_ID(sensor_accel0), _accel_pub, &accel); + orb_publish(ORB_ID(sensor_accel), _accel_pub, &accel); } } @@ -1105,10 +1105,10 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) baro.temperature = imu.temperature; if (_baro_pub < 0) { - _baro_pub = orb_advertise(ORB_ID(sensor_baro0), &baro); + _baro_pub = orb_advertise(ORB_ID(sensor_baro), &baro); } else { - orb_publish(ORB_ID(sensor_baro0), _baro_pub, &baro); + orb_publish(ORB_ID(sensor_baro), _baro_pub, &baro); } } @@ -1395,10 +1395,10 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) accel.temperature = 25.0f; if (_accel_pub < 0) { - _accel_pub = orb_advertise(ORB_ID(sensor_accel0), &accel); + _accel_pub = orb_advertise(ORB_ID(sensor_accel), &accel); } else { - orb_publish(ORB_ID(sensor_accel0), _accel_pub, &accel); + orb_publish(ORB_ID(sensor_accel), _accel_pub, &accel); } } From 9190d597912d90b6d5fbf7a606f8e9d083797534 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 25 Jan 2015 21:08:07 +0100 Subject: [PATCH 637/652] Move sensors app to multi pub/sub --- src/modules/sensors/sensors.cpp | 32 ++++++++++++++++---------------- 1 file changed, 16 insertions(+), 16 deletions(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 3f66d7995e..630c54335a 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1113,7 +1113,7 @@ Sensors::accel_poll(struct sensor_combined_s &raw) if (accel_updated) { struct accel_report accel_report; - orb_copy(ORB_ID(sensor_accel0), _accel_sub, &accel_report); + orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel_report); math::Vector<3> vect(accel_report.x, accel_report.y, accel_report.z); vect = _board_rotation * vect; @@ -1134,7 +1134,7 @@ Sensors::accel_poll(struct sensor_combined_s &raw) if (accel_updated) { struct accel_report accel_report; - orb_copy(ORB_ID(sensor_accel1), _accel_sub, &accel_report); + orb_copy(ORB_ID(sensor_accel), _accel1_sub, &accel_report); math::Vector<3> vect(accel_report.x, accel_report.y, accel_report.z); vect = _board_rotation * vect; @@ -1155,7 +1155,7 @@ Sensors::accel_poll(struct sensor_combined_s &raw) if (accel_updated) { struct accel_report accel_report; - orb_copy(ORB_ID(sensor_accel2), _accel_sub, &accel_report); + orb_copy(ORB_ID(sensor_accel), _accel2_sub, &accel_report); math::Vector<3> vect(accel_report.x, accel_report.y, accel_report.z); vect = _board_rotation * vect; @@ -1181,7 +1181,7 @@ Sensors::gyro_poll(struct sensor_combined_s &raw) if (gyro_updated) { struct gyro_report gyro_report; - orb_copy(ORB_ID(sensor_gyro0), _gyro_sub, &gyro_report); + orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &gyro_report); math::Vector<3> vect(gyro_report.x, gyro_report.y, gyro_report.z); vect = _board_rotation * vect; @@ -1202,7 +1202,7 @@ Sensors::gyro_poll(struct sensor_combined_s &raw) if (gyro_updated) { struct gyro_report gyro_report; - orb_copy(ORB_ID(sensor_gyro1), _gyro1_sub, &gyro_report); + orb_copy(ORB_ID(sensor_gyro), _gyro1_sub, &gyro_report); math::Vector<3> vect(gyro_report.x, gyro_report.y, gyro_report.z); vect = _board_rotation * vect; @@ -1223,7 +1223,7 @@ Sensors::gyro_poll(struct sensor_combined_s &raw) if (gyro_updated) { struct gyro_report gyro_report; - orb_copy(ORB_ID(sensor_gyro2), _gyro_sub, &gyro_report); + orb_copy(ORB_ID(sensor_gyro), _gyro2_sub, &gyro_report); math::Vector<3> vect(gyro_report.x, gyro_report.y, gyro_report.z); vect = _board_rotation * vect; @@ -1310,7 +1310,7 @@ Sensors::baro_poll(struct sensor_combined_s &raw) if (baro_updated) { - orb_copy(ORB_ID(sensor_baro0), _baro_sub, &_barometer); + orb_copy(ORB_ID(sensor_baro), _baro_sub, &_barometer); raw.baro_pres_mbar = _barometer.pressure; // Pressure in mbar raw.baro_alt_meter = _barometer.altitude; // Altitude in meters @@ -1325,7 +1325,7 @@ Sensors::baro_poll(struct sensor_combined_s &raw) struct baro_report baro_report; - orb_copy(ORB_ID(sensor_baro1), _baro1_sub, &baro_report); + orb_copy(ORB_ID(sensor_baro), _baro1_sub, &baro_report); raw.baro1_pres_mbar = baro_report.pressure; // Pressure in mbar raw.baro1_alt_meter = baro_report.altitude; // Altitude in meters @@ -1943,18 +1943,18 @@ Sensors::task_main() /* * do subscriptions */ - _gyro_sub = orb_subscribe(ORB_ID(sensor_gyro0)); - _accel_sub = orb_subscribe(ORB_ID(sensor_accel0)); + _gyro_sub = orb_subscribe_multi(ORB_ID(sensor_gyro), 0); + _accel_sub = orb_subscribe_multi(ORB_ID(sensor_accel), 0); _mag_sub = orb_subscribe_multi(ORB_ID(sensor_mag), 0); - _gyro1_sub = orb_subscribe(ORB_ID(sensor_gyro1)); - _accel1_sub = orb_subscribe(ORB_ID(sensor_accel1)); + _gyro1_sub = orb_subscribe_multi(ORB_ID(sensor_gyro), 1); + _accel1_sub = orb_subscribe_multi(ORB_ID(sensor_accel), 1); _mag1_sub = orb_subscribe_multi(ORB_ID(sensor_mag), 1); - _gyro2_sub = orb_subscribe(ORB_ID(sensor_gyro2)); - _accel2_sub = orb_subscribe(ORB_ID(sensor_accel2)); + _gyro2_sub = orb_subscribe_multi(ORB_ID(sensor_gyro), 2); + _accel2_sub = orb_subscribe_multi(ORB_ID(sensor_accel), 2); _mag2_sub = orb_subscribe_multi(ORB_ID(sensor_mag), 2); _rc_sub = orb_subscribe(ORB_ID(input_rc)); - _baro_sub = orb_subscribe(ORB_ID(sensor_baro0)); - _baro1_sub = orb_subscribe(ORB_ID(sensor_baro1)); + _baro_sub = orb_subscribe_multi(ORB_ID(sensor_baro), 0); + _baro1_sub = orb_subscribe_multi(ORB_ID(sensor_baro), 1); _diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure)); _vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); From 95462c5b7c357b343d39da3c5e6a49ae2055264c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 25 Jan 2015 21:08:22 +0100 Subject: [PATCH 638/652] uORB: Remove duplicate topics --- src/modules/uORB/objects_common.cpp | 12 +++--------- 1 file changed, 3 insertions(+), 9 deletions(-) diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index a8305ebeaf..4a41aa4655 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -49,19 +49,13 @@ ORB_DEFINE(sensor_mag, struct mag_report); #include -ORB_DEFINE(sensor_accel0, struct accel_report); -ORB_DEFINE(sensor_accel1, struct accel_report); -ORB_DEFINE(sensor_accel2, struct accel_report); +ORB_DEFINE(sensor_accel, struct accel_report); #include -ORB_DEFINE(sensor_gyro0, struct gyro_report); -ORB_DEFINE(sensor_gyro1, struct gyro_report); -ORB_DEFINE(sensor_gyro2, struct gyro_report); +ORB_DEFINE(sensor_gyro, struct gyro_report); #include -ORB_DEFINE(sensor_baro0, struct baro_report); -ORB_DEFINE(sensor_baro1, struct baro_report); -ORB_DEFINE(sensor_baro2, struct baro_report); +ORB_DEFINE(sensor_baro, struct baro_report); #include ORB_DEFINE(sensor_range_finder, struct range_finder_report); From 1cc4c808a88787b4e1156896453fa9369841bcde Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 25 Jan 2015 21:56:56 +0100 Subject: [PATCH 639/652] Upgrade UAVCAN to multi pub/sub A API --- makefiles/config_px4fmu-v2_default.mk | 2 +- src/modules/uavcan/module.mk | 2 +- src/modules/uavcan/sensors/baro.cpp | 9 ++------- src/modules/uavcan/sensors/baro.hpp | 2 +- src/modules/uavcan/sensors/gnss.cpp | 2 +- src/modules/uavcan/sensors/gnss.hpp | 2 +- src/modules/uavcan/sensors/mag.cpp | 10 ++-------- src/modules/uavcan/sensors/mag.hpp | 2 +- src/modules/uavcan/sensors/sensor_bridge.cpp | 6 ++---- src/modules/uavcan/sensors/sensor_bridge.hpp | 11 +++++------ 10 files changed, 17 insertions(+), 31 deletions(-) diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index 68a65efed1..3abebd82fa 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -70,7 +70,7 @@ MODULES += modules/commander MODULES += modules/navigator MODULES += modules/mavlink MODULES += modules/gpio_led -#MODULES += modules/uavcan +MODULES += modules/uavcan MODULES += modules/land_detector # diff --git a/src/modules/uavcan/module.mk b/src/modules/uavcan/module.mk index 600cb47f39..4f63629a0c 100644 --- a/src/modules/uavcan/module.mk +++ b/src/modules/uavcan/module.mk @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (C) 2013 PX4 Development Team. All rights reserved. +# Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. # Author: Pavel Kirienko # # Redistribution and use in source and binary forms, with or without diff --git a/src/modules/uavcan/sensors/baro.cpp b/src/modules/uavcan/sensors/baro.cpp index 8741ae20dd..ad09dfcac4 100644 --- a/src/modules/uavcan/sensors/baro.cpp +++ b/src/modules/uavcan/sensors/baro.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2014, 2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -38,15 +38,10 @@ #include "baro.hpp" #include -static const orb_id_t BARO_TOPICS[2] = { - ORB_ID(sensor_baro0), - ORB_ID(sensor_baro1) -}; - const char *const UavcanBarometerBridge::NAME = "baro"; UavcanBarometerBridge::UavcanBarometerBridge(uavcan::INode& node) : -UavcanCDevSensorBridgeBase("uavcan_baro", "/dev/uavcan/baro", BARO_DEVICE_PATH, BARO_TOPICS), +UavcanCDevSensorBridgeBase("uavcan_baro", "/dev/uavcan/baro", BARO_DEVICE_PATH, ORB_ID(sensor_baro)), _sub_air_data(node) { } diff --git a/src/modules/uavcan/sensors/baro.hpp b/src/modules/uavcan/sensors/baro.hpp index 9d470219ed..c7bbc5af8d 100644 --- a/src/modules/uavcan/sensors/baro.hpp +++ b/src/modules/uavcan/sensors/baro.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2014, 2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions diff --git a/src/modules/uavcan/sensors/gnss.cpp b/src/modules/uavcan/sensors/gnss.cpp index 571a6f1cd7..3ae07367fa 100644 --- a/src/modules/uavcan/sensors/gnss.cpp +++ b/src/modules/uavcan/sensors/gnss.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2014, 2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions diff --git a/src/modules/uavcan/sensors/gnss.hpp b/src/modules/uavcan/sensors/gnss.hpp index 2111ff80b6..96ff9404f5 100644 --- a/src/modules/uavcan/sensors/gnss.hpp +++ b/src/modules/uavcan/sensors/gnss.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2014, 2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions diff --git a/src/modules/uavcan/sensors/mag.cpp b/src/modules/uavcan/sensors/mag.cpp index 35ebee5426..ee278aaf53 100644 --- a/src/modules/uavcan/sensors/mag.cpp +++ b/src/modules/uavcan/sensors/mag.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2014, 2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -39,16 +39,10 @@ #include -static const orb_id_t MAG_TOPICS[3] = { - ORB_ID(sensor_mag0), - ORB_ID(sensor_mag1), - ORB_ID(sensor_mag2) -}; - const char *const UavcanMagnetometerBridge::NAME = "mag"; UavcanMagnetometerBridge::UavcanMagnetometerBridge(uavcan::INode& node) : -UavcanCDevSensorBridgeBase("uavcan_mag", "/dev/uavcan/mag", MAG_DEVICE_PATH, MAG_TOPICS), +UavcanCDevSensorBridgeBase("uavcan_mag", "/dev/uavcan/mag", MAG_DEVICE_PATH, ORB_ID(sensor_mag)), _sub_mag(node) { _device_id.devid_s.devtype = DRV_MAG_DEVTYPE_HMC5883; diff --git a/src/modules/uavcan/sensors/mag.hpp b/src/modules/uavcan/sensors/mag.hpp index 74077d883e..db38aee1d3 100644 --- a/src/modules/uavcan/sensors/mag.hpp +++ b/src/modules/uavcan/sensors/mag.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2014, 2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions diff --git a/src/modules/uavcan/sensors/sensor_bridge.cpp b/src/modules/uavcan/sensors/sensor_bridge.cpp index 0999938fc3..b370764440 100644 --- a/src/modules/uavcan/sensors/sensor_bridge.cpp +++ b/src/modules/uavcan/sensors/sensor_bridge.cpp @@ -62,7 +62,6 @@ UavcanCDevSensorBridgeBase::~UavcanCDevSensorBridgeBase() (void)unregister_class_devname(_class_devname, _channels[i].class_instance); } } - delete [] _orb_topics; delete [] _channels; } @@ -116,11 +115,10 @@ void UavcanCDevSensorBridgeBase::publish(const int node_id, const void *report) } // Publish to the appropriate topic, abort on failure - channel->orb_id = _orb_topics[class_instance]; channel->node_id = node_id; channel->class_instance = class_instance; - channel->orb_advert = orb_advertise(channel->orb_id, report); + channel->orb_advert = orb_advertise_multi(_orb_topic, report, &channel->orb_instance, ORB_PRIO_HIGH); if (channel->orb_advert < 0) { log("ADVERTISE FAILED"); (void)unregister_class_devname(_class_devname, class_instance); @@ -132,7 +130,7 @@ void UavcanCDevSensorBridgeBase::publish(const int node_id, const void *report) } assert(channel != nullptr); - (void)orb_publish(channel->orb_id, channel->orb_advert, report); + (void)orb_publish(_orb_topic, channel->orb_advert, report); } unsigned UavcanCDevSensorBridgeBase::get_num_redundant_channels() const diff --git a/src/modules/uavcan/sensors/sensor_bridge.hpp b/src/modules/uavcan/sensors/sensor_bridge.hpp index e319605370..94e52dbe50 100644 --- a/src/modules/uavcan/sensors/sensor_bridge.hpp +++ b/src/modules/uavcan/sensors/sensor_bridge.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2014, 2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -90,28 +90,27 @@ class UavcanCDevSensorBridgeBase : public IUavcanSensorBridge, public device::CD struct Channel { int node_id = -1; - orb_id_t orb_id = nullptr; orb_advert_t orb_advert = -1; int class_instance = -1; + int orb_instance = -1; }; const unsigned _max_channels; const char *const _class_devname; - orb_id_t *const _orb_topics; + const orb_id_t _orb_topic; Channel *const _channels; bool _out_of_channels = false; protected: template UavcanCDevSensorBridgeBase(const char *name, const char *devname, const char *class_devname, - const orb_id_t (&orb_topics)[MaxChannels]) : + const orb_id_t orb_topic_sensor) : device::CDev(name, devname), _max_channels(MaxChannels), _class_devname(class_devname), - _orb_topics(new orb_id_t[MaxChannels]), + _orb_topic(orb_topic_sensor), _channels(new Channel[MaxChannels]) { - memcpy(_orb_topics, orb_topics, sizeof(orb_id_t) * MaxChannels); _device_id.devid_s.bus_type = DeviceBusType_UAVCAN; _device_id.devid_s.bus = 0; } From 2f7a9eaf6553d7da6c5d6e9b3edf6e710b4dc292 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Mon, 26 Jan 2015 02:34:03 +0300 Subject: [PATCH 640/652] Fix for a compilation failure --- src/modules/uavcan/sensors/sensor_bridge.hpp | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/src/modules/uavcan/sensors/sensor_bridge.hpp b/src/modules/uavcan/sensors/sensor_bridge.hpp index 94e52dbe50..de130b0788 100644 --- a/src/modules/uavcan/sensors/sensor_bridge.hpp +++ b/src/modules/uavcan/sensors/sensor_bridge.hpp @@ -75,8 +75,7 @@ public: /** * Sensor bridge factory. - * Creates a bridge object by its ASCII name, e.g. "gnss", "mag". - * @return nullptr if such bridge can't be created. + * Creates all known sensor bridges and puts them in the linked list. */ static void make_all(uavcan::INode &node, List &list); }; @@ -102,14 +101,16 @@ class UavcanCDevSensorBridgeBase : public IUavcanSensorBridge, public device::CD bool _out_of_channels = false; protected: - template + static constexpr unsigned DEFAULT_MAX_CHANNELS = 5; // 640 KB ought to be enough for anybody + UavcanCDevSensorBridgeBase(const char *name, const char *devname, const char *class_devname, - const orb_id_t orb_topic_sensor) : + const orb_id_t orb_topic_sensor, + const unsigned max_channels = DEFAULT_MAX_CHANNELS) : device::CDev(name, devname), - _max_channels(MaxChannels), + _max_channels(max_channels), _class_devname(class_devname), _orb_topic(orb_topic_sensor), - _channels(new Channel[MaxChannels]) + _channels(new Channel[max_channels]) { _device_id.devid_s.bus_type = DeviceBusType_UAVCAN; _device_id.devid_s.bus = 0; From cbe3783d5eeeb6597b1f02f38abf16737f6d4d64 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 26 Jan 2015 09:49:30 +0100 Subject: [PATCH 641/652] Support topic groups in MAVLink subscription handling --- src/modules/mavlink/mavlink_main.cpp | 6 +++--- src/modules/mavlink/mavlink_main.h | 2 +- src/modules/mavlink/mavlink_messages.cpp | 11 ++--------- src/modules/mavlink/mavlink_orb_subscription.cpp | 11 +++++++++-- src/modules/mavlink/mavlink_orb_subscription.h | 4 +++- 5 files changed, 18 insertions(+), 16 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 9e4ab00df2..d6e9982de0 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -904,20 +904,20 @@ Mavlink::send_statustext(unsigned char severity, const char *string) mavlink_logbuffer_write(&_logbuffer, &logmsg); } -MavlinkOrbSubscription *Mavlink::add_orb_subscription(const orb_id_t topic) +MavlinkOrbSubscription *Mavlink::add_orb_subscription(const orb_id_t topic, int instance) { /* check if already subscribed to this topic */ MavlinkOrbSubscription *sub; LL_FOREACH(_subscriptions, sub) { - if (sub->get_topic() == topic) { + if (sub->get_topic() == topic && sub->get_instance() == instance) { /* already subscribed */ return sub; } } /* add new subscription */ - MavlinkOrbSubscription *sub_new = new MavlinkOrbSubscription(topic); + MavlinkOrbSubscription *sub_new = new MavlinkOrbSubscription(topic, instance); LL_APPEND(_subscriptions, sub_new); diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index ad5e5001b9..baaa7bc139 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -171,7 +171,7 @@ public: void handle_message(const mavlink_message_t *msg); - MavlinkOrbSubscription *add_orb_subscription(const orb_id_t topic); + MavlinkOrbSubscription *add_orb_subscription(const orb_id_t topic, int instance=0); int get_instance_id(); diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 6765100c70..4a095a765e 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1342,14 +1342,7 @@ protected: _act_sub(nullptr), _act_time(0) { - orb_id_t act_topics[] = { - ORB_ID(actuator_outputs_0), - ORB_ID(actuator_outputs_1), - ORB_ID(actuator_outputs_2), - ORB_ID(actuator_outputs_3) - }; - - _act_sub = _mavlink->add_orb_subscription(act_topics[N]); + _act_sub = _mavlink->add_orb_subscription(ORB_ID(actuator_outputs), N); } void send(const hrt_abstime t) @@ -1424,7 +1417,7 @@ protected: _status_time(0), _pos_sp_triplet_sub(_mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet))), _pos_sp_triplet_time(0), - _act_sub(_mavlink->add_orb_subscription(ORB_ID(actuator_outputs_0))), + _act_sub(_mavlink->add_orb_subscription(ORB_ID(actuator_outputs))), _act_time(0) {} diff --git a/src/modules/mavlink/mavlink_orb_subscription.cpp b/src/modules/mavlink/mavlink_orb_subscription.cpp index 734f0903a5..315776e297 100644 --- a/src/modules/mavlink/mavlink_orb_subscription.cpp +++ b/src/modules/mavlink/mavlink_orb_subscription.cpp @@ -46,10 +46,11 @@ #include "mavlink_orb_subscription.h" -MavlinkOrbSubscription::MavlinkOrbSubscription(const orb_id_t topic) : +MavlinkOrbSubscription::MavlinkOrbSubscription(const orb_id_t topic, int instance) : next(nullptr), _topic(topic), - _fd(orb_subscribe(_topic)), + _instance(instance), + _fd(orb_subscribe_multi(_topic, instance)), _published(false) { } @@ -65,6 +66,12 @@ MavlinkOrbSubscription::get_topic() const return _topic; } +int +MavlinkOrbSubscription::get_instance() const +{ + return _instance; +} + bool MavlinkOrbSubscription::update(uint64_t *time, void* data) { diff --git a/src/modules/mavlink/mavlink_orb_subscription.h b/src/modules/mavlink/mavlink_orb_subscription.h index 7af454df65..5394e5097b 100644 --- a/src/modules/mavlink/mavlink_orb_subscription.h +++ b/src/modules/mavlink/mavlink_orb_subscription.h @@ -50,7 +50,7 @@ class MavlinkOrbSubscription public: MavlinkOrbSubscription *next; ///< pointer to next subscription in list - MavlinkOrbSubscription(const orb_id_t topic); + MavlinkOrbSubscription(const orb_id_t topic, int instance); ~MavlinkOrbSubscription(); /** @@ -77,9 +77,11 @@ public: */ bool is_published(); orb_id_t get_topic() const; + int get_instance() const; private: const orb_id_t _topic; ///< topic metadata + const int _instance; ///< get topic instance int _fd; ///< subscription handle bool _published; ///< topic was ever published From 455f6abfcf25e48edd4d707fdf9b278e02a81772 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 26 Jan 2015 09:49:42 +0100 Subject: [PATCH 642/652] Support topic groups in sdlog2 --- src/modules/sdlog2/sdlog2.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index a3407e42c4..99632ef0bc 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1096,7 +1096,7 @@ int sdlog2_thread_main(int argc, char *argv[]) subs.att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); subs.att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); subs.rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); - subs.act_outputs_sub = orb_subscribe(ORB_ID_VEHICLE_CONTROLS); + subs.act_outputs_sub = orb_subscribe(ORB_ID(actuator_outputs)); 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)); @@ -1473,7 +1473,7 @@ int sdlog2_thread_main(int argc, char *argv[]) } /* --- ACTUATOR OUTPUTS --- */ - if (copy_if_updated(ORB_ID(actuator_outputs_0), subs.act_outputs_sub, &buf.act_outputs)) { + if (copy_if_updated(ORB_ID(actuator_outputs), subs.act_outputs_sub, &buf.act_outputs)) { log_msg.msg_type = LOG_OUT0_MSG; memcpy(log_msg.body.log_OUT0.output, buf.act_outputs.output, sizeof(log_msg.body.log_OUT0.output)); LOGBUFFER_WRITE_AND_COUNT(OUT0); From 678d5c24fb978f0c18bb87a6894ed2922fe227ca Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 26 Jan 2015 09:49:57 +0100 Subject: [PATCH 643/652] FMU driver: Move to topic groups --- src/drivers/px4fmu/fmu.cpp | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 112c015136..363d2cdcfe 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -141,7 +141,7 @@ private: int _control_subs[NUM_ACTUATOR_CONTROL_GROUPS]; actuator_controls_s _controls[NUM_ACTUATOR_CONTROL_GROUPS]; orb_id_t _control_topics[NUM_ACTUATOR_CONTROL_GROUPS]; - orb_id_t _actuator_output_topic; + int _actuator_output_topic_instance; pollfd _poll_fds[NUM_ACTUATOR_CONTROL_GROUPS]; unsigned _poll_fds_num; @@ -256,7 +256,7 @@ PX4FMU::PX4FMU() : _groups_required(0), _groups_subscribed(0), _control_subs{-1}, - _actuator_output_topic(nullptr), + _actuator_output_topic_instance(-1), _poll_fds_num(0), _pwm_limit{}, _failsafe_pwm{0}, @@ -327,8 +327,6 @@ PX4FMU::init() log("default PWM output device"); } - _actuator_output_topic = ORB_ID_DOUBLE(actuator_outputs_, _class_instance); - /* reset GPIOs */ gpio_reset(); @@ -679,10 +677,10 @@ PX4FMU::task_main() /* publish mixed control outputs */ if (_outputs_pub < 0) { - _outputs_pub = orb_advertise(_actuator_output_topic, &outputs); + _outputs_pub = orb_advertise_multi(ORB_ID(actuator_outputs), &outputs, &_actuator_output_topic_instance, ORB_PRIO_DEFAULT); } else { - orb_publish(_actuator_output_topic, _outputs_pub, &outputs); + orb_publish(ORB_ID(actuator_outputs), _outputs_pub, &outputs); } } } From 51088026c79aa6dfa237ecfd8276d7e4997bbb9e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 26 Jan 2015 09:50:09 +0100 Subject: [PATCH 644/652] IO driver: move to topic groups --- src/drivers/px4io/px4io.cpp | 13 ++++--------- 1 file changed, 4 insertions(+), 9 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 556eebab6d..653d0d5d7f 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1681,17 +1681,12 @@ PX4IO::io_publish_pwm_outputs() /* lazily advertise on first publication */ if (_to_outputs == 0) { - _to_outputs = orb_advertise((_primary_pwm_device ? - ORB_ID_VEHICLE_CONTROLS : - ORB_ID(actuator_outputs_1)), - &outputs); + int instance; + _to_outputs = orb_advertise_multi(ORB_ID(actuator_outputs), + &outputs, &instance, ORB_PRIO_MAX); } else { - orb_publish((_primary_pwm_device ? - ORB_ID_VEHICLE_CONTROLS : - ORB_ID(actuator_outputs_1)), - _to_outputs, - &outputs); + orb_publish(ORB_ID(actuator_outputs), _to_outputs, &outputs); } return OK; From d9d9a59ce440fab4bb1177bd888dc99c77a82350 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 26 Jan 2015 09:50:23 +0100 Subject: [PATCH 645/652] MKBL: Move to topic groups --- src/drivers/mkblctrl/mkblctrl.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index 1055487cb9..15412984ba 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -456,8 +456,9 @@ MK::task_main() actuator_outputs_s outputs; memset(&outputs, 0, sizeof(outputs)); /* advertise the mixed control outputs */ - _t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), - &outputs); + int dummy; + _t_outputs = orb_advertise_multi(ORB_ID(actuator_outputs), + &outputs, &dummy, ORB_PRIO_HIGH); /* advertise the blctrl status */ esc_status_s esc; From 8d37d58da732a621695d39c886ff4877151064cc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 26 Jan 2015 09:50:36 +0100 Subject: [PATCH 646/652] HIL: Move to topic groups --- src/drivers/hil/hil.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/drivers/hil/hil.cpp b/src/drivers/hil/hil.cpp index 9b5c8133b4..0584256762 100644 --- a/src/drivers/hil/hil.cpp +++ b/src/drivers/hil/hil.cpp @@ -330,8 +330,9 @@ HIL::task_main() actuator_outputs_s outputs; memset(&outputs, 0, sizeof(outputs)); /* advertise the mixed control outputs */ - _t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), - &outputs); + int dummy; + _t_outputs = orb_advertise_multi(ORB_ID(actuator_outputs), + &outputs, &dummy, ORB_PRIO_LOW); pollfd fds[2]; fds[0].fd = _t_actuators; @@ -423,7 +424,7 @@ HIL::task_main() } /* and publish for anyone that cares to see */ - orb_publish(ORB_ID_VEHICLE_CONTROLS, _t_outputs, &outputs); + orb_publish(ORB_ID(actuator_outputs), _t_outputs, &outputs); } } From aadbcb42a91d5158330c7e1f63103ee64f3fbd7c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 26 Jan 2015 09:50:50 +0100 Subject: [PATCH 647/652] ARDrone driver: Move to topic groups --- src/drivers/ardrone_interface/ardrone_motor_control.c | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/drivers/ardrone_interface/ardrone_motor_control.c b/src/drivers/ardrone_interface/ardrone_motor_control.c index 4fa24275f1..0f45b65366 100644 --- a/src/drivers/ardrone_interface/ardrone_motor_control.c +++ b/src/drivers/ardrone_interface/ardrone_motor_control.c @@ -339,7 +339,8 @@ int ardrone_write_motor_commands(int ardrone_fd, uint16_t motor1, uint16_t motor outputs.output[3] = motor4; static orb_advert_t pub = 0; if (pub == 0) { - pub = orb_advertise(ORB_ID_VEHICLE_CONTROLS, &outputs); + /* advertise to channel 0 / primary */ + pub = orb_advertise(ORB_ID(actuator_outputs), &outputs); } if (hrt_absolute_time() - last_motor_time > min_motor_interval) { @@ -350,7 +351,7 @@ int ardrone_write_motor_commands(int ardrone_fd, uint16_t motor1, uint16_t motor fsync(ardrone_fd); /* publish just written values */ - orb_publish(ORB_ID_VEHICLE_CONTROLS, pub, &outputs); + orb_publish(ORB_ID(actuator_outputs), pub, &outputs); if (ret == sizeof(buf)) { return OK; From e532b81ac1f3356a3f2771d605b8d92d88d19d67 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 26 Jan 2015 09:51:36 +0100 Subject: [PATCH 648/652] uORB: Remove last remnants of ORB_ID_DOUBLE/TRIPLE and migrate actuator outputs groups to new style interface --- src/modules/uORB/objects_common.cpp | 5 +- src/modules/uORB/topics/actuator_outputs.h | 8 +-- src/modules/uORB/uORB.h | 58 +++++++++++----------- 3 files changed, 32 insertions(+), 39 deletions(-) diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 4a41aa4655..ba1ac0350e 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -196,10 +196,7 @@ ORB_DEFINE(actuator_controls_virtual_fw, struct actuator_controls_s); ORB_DEFINE(actuator_armed, struct actuator_armed_s); #include "topics/actuator_outputs.h" -ORB_DEFINE(actuator_outputs_0, struct actuator_outputs_s); -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); +ORB_DEFINE(actuator_outputs, struct actuator_outputs_s); #include "topics/actuator_direct.h" ORB_DEFINE(actuator_direct, struct actuator_direct_s); diff --git a/src/modules/uORB/topics/actuator_outputs.h b/src/modules/uORB/topics/actuator_outputs.h index 4461404230..5f8f6d83ec 100644 --- a/src/modules/uORB/topics/actuator_outputs.h +++ b/src/modules/uORB/topics/actuator_outputs.h @@ -68,12 +68,6 @@ struct actuator_outputs_s { */ /* actuator output sets; this list can be expanded as more drivers emerge */ -ORB_DECLARE(actuator_outputs_0); -ORB_DECLARE(actuator_outputs_1); -ORB_DECLARE(actuator_outputs_2); -ORB_DECLARE(actuator_outputs_3); - -/* output sets with pre-defined applications */ -#define ORB_ID_VEHICLE_CONTROLS ORB_ID(actuator_outputs_0) +ORB_DECLARE(actuator_outputs); #endif \ No newline at end of file diff --git a/src/modules/uORB/uORB.h b/src/modules/uORB/uORB.h index 30cd598809..9c33c8a3ef 100644 --- a/src/modules/uORB/uORB.h +++ b/src/modules/uORB/uORB.h @@ -86,33 +86,6 @@ enum ORB_PRIO { */ #define ORB_ID(_name) &__orb_##_name -/** - * Generates a pointer to the uORB metadata structure for - * a given topic. - * - * The topic must have been declared previously in scope - * with ORB_DECLARE(). - * - * @param _name The name of the topic. - * @param _count The class ID of the topic - */ -#define ORB_ID_DOUBLE(_name, _count) ((_count == CLASS_DEVICE_PRIMARY) ? &__orb_##_name##0 : &__orb_##_name##1) - -/** - * Generates a pointer to the uORB metadata structure for - * a given topic. - * - * The topic must have been declared previously in scope - * with ORB_DECLARE(). - * - * @param _name The name of the topic. - * @param _count The class ID of the topic - */ -#define ORB_ID_TRIPLE(_name, _count) \ - ((_count == CLASS_DEVICE_PRIMARY) ? &__orb_##_name##0 : \ - ((_count == CLASS_DEVICE_SECONDARY) ? &__orb_##_name##1 : \ - (((_count == CLASS_DEVICE_TERTIARY) ? &__orb_##_name##2 : 0)))) - /** * Declare (prototype) the uORB metadata for a topic. * @@ -147,7 +120,7 @@ enum ORB_PRIO { #define ORB_DEFINE(_name, _struct) \ const struct orb_metadata __orb_##_name = { \ #_name, \ - sizeof(_struct), \ + sizeof(_struct) \ }; struct hack __BEGIN_DECLS @@ -257,6 +230,35 @@ extern int orb_publish(const struct orb_metadata *meta, orb_advert_t handle, con */ extern int orb_subscribe(const struct orb_metadata *meta) __EXPORT; +/** + * Subscribe to a multi-instance of a topic. + * + * The returned value is a file descriptor that can be passed to poll() + * in order to wait for updates to a topic, as well as topic_read, + * orb_check and orb_stat. + * + * Subscription will succeed even if the topic has not been advertised; + * in this case the topic will have a timestamp of zero, it will never + * signal a poll() event, checking will always return false and it cannot + * be copied. When the topic is subsequently advertised, poll, check, + * stat and copy calls will react to the initial publication that is + * performed as part of the advertisement. + * + * Subscription will fail if the topic is not known to the system, i.e. + * there is nothing in the system that has declared the topic and thus it + * can never be published. + * + * @param meta The uORB metadata (usually from the ORB_ID() macro) + * for the topic. + * @param instance The instance of the topic. Instance 0 matches the + * topic of the orb_subscribe() call, higher indices + * are for topics created with orb_publish_multi(). + * @return ERROR on error, otherwise returns a handle + * that can be used to read and update the topic. + * If the topic in question is not known (due to an + * ORB_DEFINE_OPTIONAL with no corresponding ORB_DECLARE) + * this function will return -1 and set errno to ENOENT. + */ extern int orb_subscribe_multi(const struct orb_metadata *meta, unsigned instance) __EXPORT; /** From a381ce37323b08597df637e901099fcdd69c6ec1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 26 Jan 2015 09:56:15 +0100 Subject: [PATCH 649/652] Fix newline --- src/modules/uORB/topics/actuator_outputs.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/uORB/topics/actuator_outputs.h b/src/modules/uORB/topics/actuator_outputs.h index 5f8f6d83ec..c6fbaaed5a 100644 --- a/src/modules/uORB/topics/actuator_outputs.h +++ b/src/modules/uORB/topics/actuator_outputs.h @@ -70,4 +70,4 @@ struct actuator_outputs_s { /* actuator output sets; this list can be expanded as more drivers emerge */ ORB_DECLARE(actuator_outputs); -#endif \ No newline at end of file +#endif From e9bcc0a2624c77c6f71bb926347e85b8c7592e34 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Thu, 29 Jan 2015 14:05:48 +0100 Subject: [PATCH 650/652] Add yaw modes that define multirotor heading behaviour during missions. --- src/modules/navigator/mission.cpp | 72 +++++++++++++++++++++++-- src/modules/navigator/mission.h | 16 +++++- src/modules/navigator/mission_block.cpp | 3 +- src/modules/navigator/mission_params.c | 16 ++++++ 4 files changed, 102 insertions(+), 5 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 9b0a092dad..b52b12ce7c 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -39,6 +39,7 @@ * @author Thomas Gubler * @author Anton Babushkin * @author Ban Siesta + * @author Simon Wilks */ #include @@ -68,6 +69,7 @@ Mission::Mission(Navigator *navigator, const char *name) : _param_takeoff_alt(this, "MIS_TAKEOFF_ALT", false), _param_dist_1wp(this, "MIS_DIST_1WP", false), _param_altmode(this, "MIS_ALTMODE", false), + _param_yawmode(this, "MIS_YAWMODE", false), _onboard_mission({0}), _offboard_mission({0}), _current_onboard_mission_index(-1), @@ -80,6 +82,7 @@ Mission::Mission(Navigator *navigator, const char *name) : _missionFeasiblityChecker(), _min_current_sp_distance_xy(FLT_MAX), _mission_item_previous_alt(NAN), + _on_arrival_yaw(NAN), _distance_current_previous(0.0f) { /* load initial params */ @@ -166,6 +169,13 @@ Mission::on_active() _navigator->set_can_loiter_at_sp(true); } } + + /* see if we need to update the current yaw heading for rotary wing types */ + if (_navigator->get_vstatus()->is_rotary_wing + && _param_yawmode.get() != MISSION_YAWMODE_NONE + && _mission_type != MISSION_TYPE_NONE) { + heading_sp_update(); + } } void @@ -275,7 +285,7 @@ Mission::check_dist_1wp() &mission_item, sizeof(mission_item_s)) == sizeof(mission_item_s)) { /* check only items with valid lat/lon */ - if ( mission_item.nav_cmd == NAV_CMD_WAYPOINT || + if ( mission_item.nav_cmd == NAV_CMD_WAYPOINT || mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED || @@ -362,7 +372,6 @@ Mission::set_mission_items() mavlink_log_critical(_navigator->get_mavlink_fd(), "offboard mission now running"); } _mission_type = MISSION_TYPE_OFFBOARD; - } else { /* no mission available or mission finished, switch to loiter */ if (_mission_type != MISSION_TYPE_NONE) { @@ -396,6 +405,10 @@ Mission::set_mission_items() return; } + if (pos_sp_triplet->current.valid) { + _on_arrival_yaw = _mission_item.yaw; + } + /* do takeoff on first waypoint for rotary wing vehicles */ if (_navigator->get_vstatus()->is_rotary_wing) { /* force takeoff if landed (additional protection) */ @@ -442,6 +455,7 @@ Mission::set_mission_items() _mission_item.nav_cmd = NAV_CMD_TAKEOFF; _mission_item.lat = _navigator->get_global_position()->lat; _mission_item.lon = _navigator->get_global_position()->lon; + _mission_item.yaw = NAN; _mission_item.altitude = takeoff_alt; _mission_item.altitude_is_relative = false; _mission_item.autocontinue = true; @@ -481,7 +495,6 @@ Mission::set_mission_items() if (read_mission_item(_mission_type == MISSION_TYPE_ONBOARD, false, &mission_item_next)) { /* got next mission item, update setpoint triplet */ mission_item_to_position_setpoint(&mission_item_next, &pos_sp_triplet->next); - } else { /* next mission item is not available */ pos_sp_triplet->next.valid = false; @@ -503,6 +516,59 @@ Mission::set_mission_items() _navigator->set_position_setpoint_triplet_updated(); } +void +Mission::heading_sp_update() +{ + if (_takeoff) { + /* we don't want to be yawing during takeoff */ + return; + } + + struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + + /* Don't change setpoint if last and current waypoint are not valid */ + if (!pos_sp_triplet->previous.valid || !pos_sp_triplet->current.valid || + !isfinite(_on_arrival_yaw)) { + return; + } + + /* Don't do FOH for landing and takeoff waypoints, the ground may be near + * and the FW controller has a custom landing logic */ + if (_mission_item.nav_cmd == NAV_CMD_LAND || _mission_item.nav_cmd == NAV_CMD_TAKEOFF) { + return; + } + + /* set yaw angle for the waypoint iff a loiter time has been specified */ + if (_waypoint_position_reached && _mission_item.time_inside > 0.0f) { + _mission_item.yaw = _on_arrival_yaw; + /* always keep the front of the rotary wing pointing to the next waypoint */ + } else if (_param_yawmode.get() == MISSION_YAWMODE_FRONT_TO_WAYPOINT) { + _mission_item.yaw = get_bearing_to_next_waypoint( + _navigator->get_global_position()->lat, + _navigator->get_global_position()->lon, + _mission_item.lat, + _mission_item.lon); + /* always keep the back of the rotary wing pointing towards home */ + } else if (_param_yawmode.get() == MISSION_YAWMODE_FRONT_TO_HOME) { + _mission_item.yaw = get_bearing_to_next_waypoint( + _navigator->get_global_position()->lat, + _navigator->get_global_position()->lon, + _navigator->get_home_position()->lat, + _navigator->get_home_position()->lon); + /* always keep the back of the rotary wing pointing towards home */ + } else if (_param_yawmode.get() == MISSION_YAWMODE_BACK_TO_HOME) { + _mission_item.yaw = _wrap_pi(get_bearing_to_next_waypoint( + _navigator->get_global_position()->lat, + _navigator->get_global_position()->lon, + _navigator->get_home_position()->lat, + _navigator->get_home_position()->lon) + M_PI_F); + } + + mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); + _navigator->set_position_setpoint_triplet_updated(); +} + + void Mission::altitude_sp_foh_update() { diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index a8a644b0f5..e9f78e8fdc 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -83,6 +83,13 @@ public: MISSION_ALTMODE_FOH = 1 }; + enum mission_yaw_mode { + MISSION_YAWMODE_NONE = 0, + MISSION_YAWMODE_FRONT_TO_WAYPOINT = 1, + MISSION_YAWMODE_FRONT_TO_HOME = 2, + MISSION_YAWMODE_BACK_TO_HOME = 3 + }; + private: /** * Update onboard mission topic @@ -110,6 +117,11 @@ private: */ void set_mission_items(); + /** + * Updates the heading of the vehicle. Rotary wings only. + */ + void heading_sp_update(); + /** * Updates the altitude sp to follow a foh */ @@ -155,6 +167,7 @@ private: control::BlockParamFloat _param_takeoff_alt; control::BlockParamFloat _param_dist_1wp; control::BlockParamInt _param_altmode; + control::BlockParamInt _param_yawmode; struct mission_s _onboard_mission; struct mission_s _offboard_mission; @@ -177,7 +190,8 @@ private: float _min_current_sp_distance_xy; /**< minimum distance which was achieved to the current waypoint */ float _mission_item_previous_alt; /**< holds the altitude of the previous mission item, - can be replaced by a full copy of the previous mission item if needed*/ + can be replaced by a full copy of the previous mission item if needed */ + float _on_arrival_yaw; /**< holds the yaw value that should be applied when the current waypoint is reached */ float _distance_current_previous; /**< distance from previous to current sp in pos_sp_triplet, only use if current and previous are valid */ }; diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 723caec7ce..e39fb32164 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -134,6 +134,7 @@ MissionBlock::is_mission_item_reached() } } + /* Check if the waypoint and the requested yaw setpoint. */ if (_waypoint_position_reached && !_waypoint_yaw_reached) { /* TODO: removed takeoff, why? */ @@ -151,7 +152,7 @@ MissionBlock::is_mission_item_reached() } } - /* check if the current waypoint was reached */ + /* Once the waypoint and yaw setpoint have been reached we can start the loiter time countdown */ if (_waypoint_position_reached && _waypoint_yaw_reached) { if (_time_first_inside_orbit == 0) { diff --git a/src/modules/navigator/mission_params.c b/src/modules/navigator/mission_params.c index 04c01fe51c..6310cf6de4 100644 --- a/src/modules/navigator/mission_params.c +++ b/src/modules/navigator/mission_params.c @@ -95,3 +95,19 @@ PARAM_DEFINE_FLOAT(MIS_DIST_1WP, 500); * @group Mission */ PARAM_DEFINE_INT32(MIS_ALTMODE, 0); + +/** + * Multirotor only. Yaw setpoint mode. + * + * 0: Set the yaw heading to the yaw value specified for the destination waypoint. + * 1: Maintain a yaw heading pointing towards the next waypoint. + * 2: Maintain a yaw heading that always points to the home location. + * 3: Maintain a yaw heading that always points away from the home location (ie: back always faces home). + * + * The values are defined in the enum mission_altitude_mode + * + * @min 0 + * @max 3 + * @group Mission + */ +PARAM_DEFINE_INT32(MIS_YAWMODE, 0); From 0b784c20c8bf69cee281f6717f055b0309d331b1 Mon Sep 17 00:00:00 2001 From: hauptmech Date: Wed, 28 Jan 2015 15:45:00 +1300 Subject: [PATCH 651/652] Save and check device id for acc and gyro calibration parameters. Fix config utility to work with all devices of each type. Accel, gyro and mag devices correctly set their device_id devtype. Combo devices (mpu6000 lsm303d) now correctly return their devtype. config util shows device id for all sensor types. Add, save during calibration and check during preflight ID parameters for accelerometer and gyro --- src/drivers/drv_mag.h | 6 -- src/drivers/drv_sensor.h | 18 ++++++ src/drivers/l3gd20/l3gd20.cpp | 16 +++--- src/drivers/lsm303d/lsm303d.cpp | 32 ++++++++--- src/drivers/mpu6000/mpu6000.cpp | 56 ++++++++++++------- .../commander/accelerometer_calibration.cpp | 8 +++ src/modules/commander/gyro_calibration.cpp | 7 +++ src/modules/sensors/sensor_params.c | 15 ++++- src/systemcmds/config/config.c | 17 ++++-- .../preflight_check/preflight_check.c | 38 ++++++++++--- 10 files changed, 156 insertions(+), 57 deletions(-) diff --git a/src/drivers/drv_mag.h b/src/drivers/drv_mag.h index d8fe1ae7a3..193c816e0e 100644 --- a/src/drivers/drv_mag.h +++ b/src/drivers/drv_mag.h @@ -41,7 +41,6 @@ #include #include -#include "drv_device.h" #include "drv_sensor.h" #include "drv_orb_dev.h" @@ -83,11 +82,6 @@ struct mag_scale { */ ORB_DECLARE(sensor_mag); -/* - * mag device types, for _device_id - */ -#define DRV_MAG_DEVTYPE_HMC5883 1 -#define DRV_MAG_DEVTYPE_LSM303D 2 /* * ioctl() definitions diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h index 5e4598de86..467dace082 100644 --- a/src/drivers/drv_sensor.h +++ b/src/drivers/drv_sensor.h @@ -43,6 +43,24 @@ #include #include +#include "drv_device.h" + +/** + * Sensor type definitions. + * + * Used to create a unique device id for redundant and combo sensors + */ + +#define DRV_MAG_DEVTYPE_HMC5883 0x01 +#define DRV_MAG_DEVTYPE_LSM303D 0x02 +#define DRV_ACC_DEVTYPE_LSM303D 0x11 +#define DRV_ACC_DEVTYPE_BMA180 0x12 +#define DRV_ACC_DEVTYPE_MPU6000 0x13 +#define DRV_GYR_DEVTYPE_MPU6000 0x21 +#define DRV_GYR_DEVTYPE_L3GD20 0x22 +#define DRV_RNG_DEVTYPE_MB12XX 0x31 +#define DRV_RNG_DEVTYPE_LL40LS 0x32 + /* * ioctl() definitions * diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index bd1bd9f86d..f583bced4f 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -220,7 +220,7 @@ private: struct hrt_call _call; unsigned _call_interval; - + RingBuffer *_reports; struct gyro_scale _gyro_scale; @@ -424,6 +424,8 @@ L3GD20::L3GD20(int bus, const char* path, spi_dev_e device, enum Rotation rotati // enable debug() calls _debug_enabled = true; + _device_id.devid_s.devtype = DRV_GYR_DEVTYPE_L3GD20; + // default scale factors _gyro_scale.x_offset = 0; _gyro_scale.x_scale = 1.0f; @@ -639,7 +641,7 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg) return -ENOMEM; } irqrestore(flags); - + return OK; } @@ -867,7 +869,7 @@ L3GD20::reset() disable_i2c(); /* set default configuration */ - write_checked_reg(ADDR_CTRL_REG1, + write_checked_reg(ADDR_CTRL_REG1, REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE); write_checked_reg(ADDR_CTRL_REG2, 0); /* disable high-pass filters */ write_checked_reg(ADDR_CTRL_REG3, 0x08); /* DRDY enable */ @@ -911,7 +913,7 @@ L3GD20::check_registers(void) if we get the wrong value then we know the SPI bus or sensor is very sick. We set _register_wait to 20 and wait until we have seen 20 good values in a row - before we consider the sensor to be OK again. + before we consider the sensor to be OK again. */ perf_count(_bad_registers); @@ -974,7 +976,7 @@ L3GD20::measure() we waited for DRDY, but did not see DRDY on all axes when we captured. That means a transfer error of some sort */ - perf_count(_errors); + perf_count(_errors); return; } #endif @@ -994,7 +996,7 @@ L3GD20::measure() */ report.timestamp = hrt_absolute_time(); report.error_count = perf_event_count(_bad_registers); - + switch (_orientation) { case SENSOR_BOARD_ROTATION_000_DEG: @@ -1072,7 +1074,7 @@ L3GD20::print_info() for (uint8_t i=0; i_device_id.devid = _device_id.devid; + _mag->_device_id.devid_s.devtype = DRV_MAG_DEVTYPE_LSM303D; + + // default scale factors _accel_scale.x_offset = 0.0f; _accel_scale.x_scale = 1.0f; @@ -660,6 +667,7 @@ LSM303D::init() warnx("ADVERT ERR"); } + _accel_class_instance = register_class_devname(ACCEL_DEVICE_PATH); /* advertise sensor topic, measure manually to initialize valid report */ @@ -698,7 +706,7 @@ LSM303D::reset() disable_i2c(); /* enable accel*/ - write_checked_reg(ADDR_CTRL_REG1, + write_checked_reg(ADDR_CTRL_REG1, REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A | REG1_BDU_UPDATE | REG1_RATE_800HZ_A); /* enable mag */ @@ -732,7 +740,7 @@ LSM303D::probe() /* verify that the device is attached and functioning */ bool success = (read_reg(ADDR_WHO_AM_I) == WHO_I_AM); - + if (success) { _checked_values[0] = WHO_I_AM; return OK; @@ -1005,7 +1013,7 @@ LSM303D::mag_ioctl(struct file *filp, int cmd, unsigned long arg) return SENSOR_POLLRATE_MANUAL; return 1000000 / _call_mag_interval; - + case SENSORIOCSQUEUEDEPTH: { /* lower bound is mandatory, upper bound is a sanity check */ if ((arg < 1) || (arg > 100)) @@ -1410,7 +1418,7 @@ LSM303D::check_registers(void) if we get the wrong value then we know the SPI bus or sensor is very sick. We set _register_wait to 20 and wait until we have seen 20 good values in a row - before we consider the sensor to be OK again. + before we consider the sensor to be OK again. */ perf_count(_bad_registers); @@ -1534,7 +1542,7 @@ LSM303D::measure() perf_count(_bad_values); _constant_accel_count = 0; } - + _last_accel[0] = x_in_new; _last_accel[1] = y_in_new; _last_accel[2] = z_in_new; @@ -1652,7 +1660,7 @@ LSM303D::print_info() for (uint8_t i=0; imag_ioctl(filp, cmd, arg); + switch (cmd) { + case DEVIOCGDEVICEID: + return (int)CDev::ioctl(filp, cmd, arg); + break; + default: + return _parent->mag_ioctl(filp, cmd, arg); + } } void diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index e4e9824902..e322e8b3a5 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -446,7 +446,7 @@ const uint8_t MPU6000::_checked_registers[MPU6000_NUM_CHECKED_REGISTERS] = { MPU MPUREG_INT_ENABLE, MPUREG_INT_PIN_CFG }; - + /** * Helper class implementing the gyro driver node. @@ -523,6 +523,12 @@ MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev // disable debug() calls _debug_enabled = false; + _device_id.devid_s.devtype = DRV_ACC_DEVTYPE_MPU6000; + + /* Prime _gyro with parents devid. */ + _gyro->_device_id.devid = _device_id.devid; + _gyro->_device_id.devid_s.devtype = DRV_GYR_DEVTYPE_MPU6000; + // default accel scale factors _accel_scale.x_offset = 0; _accel_scale.x_scale = 1.0f; @@ -609,6 +615,7 @@ MPU6000::init() _gyro_scale.z_offset = 0; _gyro_scale.z_scale = 1.0f; + /* do CDev init for the gyro device node, keep it optional */ ret = _gyro->init(); /* if probe/setup failed, bail now */ @@ -668,7 +675,7 @@ int MPU6000::reset() // for it to come out of sleep write_checked_reg(MPUREG_PWR_MGMT_1, MPU_CLK_SEL_PLLGYROZ); up_udelay(1000); - + // Disable I2C bus (recommended on datasheet) write_checked_reg(MPUREG_USER_CTRL, BIT_I2C_IF_DIS); irqrestore(state); @@ -726,7 +733,7 @@ int MPU6000::reset() case MPU6000_REV_D9: case MPU6000_REV_D10: // default case to cope with new chip revisions, which - // presumably won't have the accel scaling bug + // presumably won't have the accel scaling bug default: // Accel scale 8g (4096 LSB/g) write_checked_reg(MPUREG_ACCEL_CONFIG, 2 << 3); @@ -804,7 +811,7 @@ MPU6000::_set_dlpf_filter(uint16_t frequency_hz) { uint8_t filter; - /* + /* choose next highest filter frequency available */ if (frequency_hz == 0) { @@ -906,7 +913,7 @@ MPU6000::gyro_self_test() if (self_test()) return 1; - /* + /* * Maximum deviation of 20 degrees, according to * http://www.invensense.com/mems/gyro/documents/PS-MPU-6000A-00v3.4.pdf * Section 6.1, initial ZRO tolerance @@ -967,7 +974,7 @@ MPU6000::factory_self_test() // gyro self test has to be done at 250DPS write_reg(MPUREG_GYRO_CONFIG, BITS_FS_250DPS); - struct MPUReport mpu_report; + struct MPUReport mpu_report; float accel_baseline[3]; float gyro_baseline[3]; float accel[3]; @@ -997,10 +1004,10 @@ MPU6000::factory_self_test() } #if 1 - write_reg(MPUREG_GYRO_CONFIG, - BITS_FS_250DPS | - BITS_GYRO_ST_X | - BITS_GYRO_ST_Y | + write_reg(MPUREG_GYRO_CONFIG, + BITS_FS_250DPS | + BITS_GYRO_ST_X | + BITS_GYRO_ST_Y | BITS_GYRO_ST_Z); // accel 8g, self-test enabled all axes @@ -1070,7 +1077,7 @@ MPU6000::factory_self_test() ::printf("FAIL\n"); ret = -EIO; } - } + } for (uint8_t i=0; i<3; i++) { float diff = gyro[i]-gyro_baseline[i]; float err = 100*(diff - gyro_ftrim[i]) / gyro_ftrim[i]; @@ -1085,7 +1092,7 @@ MPU6000::factory_self_test() ::printf("FAIL\n"); ret = -EIO; } - } + } write_reg(MPUREG_GYRO_CONFIG, saved_gyro_config); write_reg(MPUREG_ACCEL_CONFIG, saved_accel_config); @@ -1232,14 +1239,14 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) /* lower bound is mandatory, upper bound is a sanity check */ if ((arg < 1) || (arg > 100)) return -EINVAL; - + irqstate_t flags = irqsave(); if (!_accel_reports->resize(arg)) { irqrestore(flags); return -ENOMEM; } irqrestore(flags); - + return OK; } @@ -1521,13 +1528,13 @@ MPU6000::check_registers(void) the data registers. */ uint8_t v; - if ((v=read_reg(_checked_registers[_checked_next], MPU6000_HIGH_BUS_SPEED)) != + if ((v=read_reg(_checked_registers[_checked_next], MPU6000_HIGH_BUS_SPEED)) != _checked_values[_checked_next]) { /* if we get the wrong value then we know the SPI bus or sensor is very sick. We set _register_wait to 20 and wait until we have seen 20 good values in a row - before we consider the sensor to be OK again. + before we consider the sensor to be OK again. */ perf_count(_bad_registers); @@ -1640,7 +1647,7 @@ MPU6000::measure() _register_wait--; return; } - + /* * Swap axes and negate y @@ -1701,7 +1708,7 @@ MPU6000::measure() float x_in_new = ((report.accel_x * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; float y_in_new = ((report.accel_y * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; float z_in_new = ((report.accel_z * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; - + arb.x = _accel_filter_x.apply(x_in_new); arb.y = _accel_filter_y.apply(y_in_new); arb.z = _accel_filter_z.apply(z_in_new); @@ -1722,7 +1729,7 @@ MPU6000::measure() float x_gyro_in_new = ((report.gyro_x * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale; float y_gyro_in_new = ((report.gyro_y * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale; float z_gyro_in_new = ((report.gyro_z * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale; - + grb.x = _gyro_filter_x.apply(x_gyro_in_new); grb.y = _gyro_filter_y.apply(y_gyro_in_new); grb.z = _gyro_filter_z.apply(z_gyro_in_new); @@ -1776,7 +1783,7 @@ MPU6000::print_info() for (uint8_t i=0; igyro_ioctl(filp, cmd, arg); + + switch (cmd) { + case DEVIOCGDEVICEID: + return (int)CDev::ioctl(filp, cmd, arg); + break; + default: + return _parent->gyro_ioctl(filp, cmd, arg); + } } /** diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index d4cd97be6a..13ab966aba 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -159,6 +159,7 @@ int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], flo int do_accel_calibration(int mavlink_fd) { int fd; + int32_t device_id; mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name); @@ -180,6 +181,9 @@ int do_accel_calibration(int mavlink_fd) /* reset all offsets to zero and all scales to one */ fd = open(ACCEL_DEVICE_PATH, 0); + + device_id = ioctl(fd, DEVIOCGDEVICEID, 0); + res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale); close(fd); @@ -226,6 +230,10 @@ int do_accel_calibration(int mavlink_fd) mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG); res = ERROR; } + + if (param_set(param_find("SENS_ACC_ID"), &(device_id))) { + res = ERROR; + } } if (res == OK) { diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index 2be0e881ee..8410297efc 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -62,6 +62,7 @@ static const char *sensor_name = "gyro"; int do_gyro_calibration(int mavlink_fd) { + int32_t device_id; mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name); mavlink_log_info(mavlink_fd, "HOLD STILL"); @@ -81,6 +82,9 @@ int do_gyro_calibration(int mavlink_fd) /* reset all offsets to zero and all scales to one */ int fd = open(GYRO_DEVICE_PATH, 0); + + device_id = ioctl(fd, DEVIOCGDEVICEID, 0); + res = ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale); close(fd); @@ -277,6 +281,9 @@ int do_gyro_calibration(int mavlink_fd) mavlink_log_critical(mavlink_fd, "ERROR: failed to set scale params"); res = ERROR; } + if (param_set(param_find("SENS_GYRO_ID"), &(device_id))) { + res = ERROR; + } } if (res == OK) { diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index bf5708e0b7..67b7feef7b 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -44,6 +44,13 @@ #include #include +/** + * ID of the Gyro that the calibration is for. + * + * @group Sensor Calibration + */ +PARAM_DEFINE_INT32(SENS_GYRO_ID, 0); + /** * Gyro X-axis offset * @@ -153,6 +160,12 @@ PARAM_DEFINE_FLOAT(SENS_MAG_YSCALE, 1.0f); */ PARAM_DEFINE_FLOAT(SENS_MAG_ZSCALE, 1.0f); +/** + * ID of the Accelerometer that the calibration is for. + * + * @group Sensor Calibration + */ +PARAM_DEFINE_INT32(SENS_ACC_ID, 0); /** * Accelerometer X-axis offset @@ -270,7 +283,7 @@ PARAM_DEFINE_INT32(SENS_BOARD_ROT, 0); /** * PX4Flow board rotation * - * This parameter defines the rotation of the PX4FLOW board relative to the platform. + * This parameter defines the rotation of the PX4FLOW board relative to the platform. * Zero rotation is defined as Y on flow board pointing towards front of vehicle * Possible values are: * 0 = No rotation diff --git a/src/systemcmds/config/config.c b/src/systemcmds/config/config.c index 9f13edb184..f54342f06a 100644 --- a/src/systemcmds/config/config.c +++ b/src/systemcmds/config/config.c @@ -81,7 +81,7 @@ config_main(int argc, char *argv[]) do_device(argc - 1, argv + 1); } } - + errx(1, "expected a device, try '/dev/gyro', '/dev/accel', '/dev/mag'"); } @@ -192,8 +192,12 @@ do_gyro(int argc, char *argv[]) int srate = ioctl(fd, GYROIOCGSAMPLERATE, 0); int prate = ioctl(fd, SENSORIOCGPOLLRATE, 0); int range = ioctl(fd, GYROIOCGRANGE, 0); + int id = ioctl(fd, DEVIOCGDEVICEID,0); + int32_t calibration_id = 0; - warnx("gyro: \n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d dps", srate, prate, range); + param_get(param_find("SENS_GYRO_ID"), &(calibration_id)); + + warnx("gyro: \n\tdevice id:\t0x%X\t(calibration is for device id 0x%X)\n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d dps", id, calibration_id, srate, prate, range); close(fd); } @@ -267,9 +271,10 @@ do_mag(int argc, char *argv[]) int range = ioctl(fd, MAGIOCGRANGE, 0); int id = ioctl(fd, DEVIOCGDEVICEID,0); int32_t calibration_id = 0; + param_get(param_find("SENS_MAG_ID"), &(calibration_id)); - warnx("mag: \n\tdevice id:\t%x\t(calibration is for device id %x)\n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d Ga", id, calibration_id, srate, prate, range); + warnx("mag: \n\tdevice id:\t0x%X\t(calibration is for device id 0x%X)\n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d Ga", id, calibration_id, srate, prate, range); close(fd); } @@ -341,8 +346,12 @@ do_accel(int argc, char *argv[]) int srate = ioctl(fd, ACCELIOCGSAMPLERATE, 0); int prate = ioctl(fd, SENSORIOCGPOLLRATE, 0); int range = ioctl(fd, ACCELIOCGRANGE, 0); + int id = ioctl(fd, DEVIOCGDEVICEID,0); + int32_t calibration_id = 0; - warnx("accel: \n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d G", srate, prate, range); + param_get(param_find("SENS_ACC_ID"), &(calibration_id)); + + warnx("accel: \n\tdevice id:\t0x%X\t(calibration is for device id 0x%X)\n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d G", id, calibration_id, srate, prate, range); close(fd); } diff --git a/src/systemcmds/preflight_check/preflight_check.c b/src/systemcmds/preflight_check/preflight_check.c index bbd90b961f..3e1f76714b 100644 --- a/src/systemcmds/preflight_check/preflight_check.c +++ b/src/systemcmds/preflight_check/preflight_check.c @@ -84,7 +84,7 @@ int preflight_check_main(int argc, char *argv[]) /* open text message output path */ int mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); int ret; - int32_t mag_devid,mag_calibration_devid; + int32_t devid, calibration_devid; /* give the system some time to sample the sensors in the background */ usleep(150000); @@ -98,9 +98,9 @@ int preflight_check_main(int argc, char *argv[]) goto system_eval; } - mag_devid = ioctl(fd, DEVIOCGDEVICEID,0); - param_get(param_find("SENS_MAG_ID"), &(mag_calibration_devid)); - if (mag_devid != mag_calibration_devid){ + devid = ioctl(fd, DEVIOCGDEVICEID,0); + param_get(param_find("SENS_MAG_ID"), &(calibration_devid)); + if (devid != calibration_devid){ warnx("magnetometer calibration is for a different device - calibrate magnetometer first"); mavlink_log_critical(mavlink_fd, "SENSOR FAIL: MAG CAL ID"); system_ok = false; @@ -108,7 +108,7 @@ int preflight_check_main(int argc, char *argv[]) } ret = ioctl(fd, MAGIOCSELFTEST, 0); - + if (ret != OK) { warnx("magnetometer calibration missing or bad - calibrate magnetometer first"); mavlink_log_critical(mavlink_fd, "SENSOR FAIL: MAG CHECK/CAL"); @@ -120,8 +120,18 @@ int preflight_check_main(int argc, char *argv[]) close(fd); fd = open(ACCEL_DEVICE_PATH, O_RDONLY); + + devid = ioctl(fd, DEVIOCGDEVICEID,0); + param_get(param_find("SENS_ACC_ID"), &(calibration_devid)); + if (devid != calibration_devid){ + warnx("accelerometer calibration is for a different device - calibrate accelerometer first"); + mavlink_log_critical(mavlink_fd, "SENSOR FAIL: ACC CAL ID"); + system_ok = false; + goto system_eval; + } + ret = ioctl(fd, ACCELIOCSELFTEST, 0); - + if (ret != OK) { warnx("accel self test failed"); mavlink_log_critical(mavlink_fd, "SENSOR FAIL: ACCEL CHECK/CAL"); @@ -156,8 +166,18 @@ int preflight_check_main(int argc, char *argv[]) close(fd); fd = open(GYRO_DEVICE_PATH, 0); + + devid = ioctl(fd, DEVIOCGDEVICEID,0); + param_get(param_find("SENS_GYRO_ID"), &(calibration_devid)); + if (devid != calibration_devid){ + warnx("gyro calibration is for a different device - calibrate gyro first"); + mavlink_log_critical(mavlink_fd, "SENSOR FAIL: GYRO CAL ID"); + system_ok = false; + goto system_eval; + } + ret = ioctl(fd, GYROIOCSELFTEST, 0); - + if (ret != OK) { warnx("gyro self test failed"); mavlink_log_critical(mavlink_fd, "SENSOR FAIL: GYRO CHECK/CAL"); @@ -183,10 +203,10 @@ int preflight_check_main(int argc, char *argv[]) system_ok &= rc_ok; - + system_eval: - + if (system_ok) { /* all good, exit silently */ exit(0); From a2a244584e36a0e9ffdb93a0dda8473baf8344d3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 31 Jan 2015 23:09:48 +0100 Subject: [PATCH 652/652] Update block diagram --- Documentation/px4_block_diagram.odg | Bin 91712 -> 94510 bytes 1 file changed, 0 insertions(+), 0 deletions(-) diff --git a/Documentation/px4_block_diagram.odg b/Documentation/px4_block_diagram.odg index e66e1b1cdd704ebe713a2127004488aed7fb2218..84566844252805e31620a7313d29aa34d1b36029 100644 GIT binary patch delta 50391 zcmaHxLw6;Nq6JTE+qP}nwvCSM6Ls9Nopfy5HafO#JGtw7Kj01atcH71YgOMAM9n?~ zg0dVKI643T4FFuR@+KfCqFk}^N?uJAI}^(R0RIg;Ezbl5FCm-^D0?C!E~4(0ebo(> zY_Q@@*fCk&x40Yh};by*#DTpclGRXzU_G9Khwfsjia9Dup}juJ=! zuKLO%Kse+)6Bl5uY@h}PK&bfj|H0}^fc#`p=yKv`llD@zmBVh(8+ZDT&N3_jU{}Ce zdKC&b^`m};2^e{g7V)){`opmZ2S{(*6CzH@*8((s#`P%L?7fNI6w}|hokIXZLBact zQ$JS@;KAxnl8KLcn+(OF(?G$J8|_0 z8h{m;%X}K&Uk5^X0WRjP>~gk74`11&16d8yCMazMpU&F12D*iAaE`O*?nk9rCpm<`eK_wf?m4knlf%nkf?QU?o^szE)iPalUo7=UsM%mewc zWAGvd?cs*JR+y7oCviU`%FU0f3BH20;MO4mg5|)v6WmtQ+-_uzKcwx_;oSPR&J`kX z4vQ=o5nX`~LimtFeVg;mv82-kvq?Bvp}qctj+h9fe|hl!iG1J(3v4o-Gc*Di&*llg z?jhcKKz@(-+>YS4)uPE;2-AZPpl+f-{iU>q3kuikFM@$o-R(Lq?0{q3NG zkkWd6&j8|k zpMEt70A&?a7#KWQ1zG_o2H|zRj#rP+&O%ZZ@O*=ShuOqSB*^l7Ai0^UuH6TmtbR0- ze1ZTxKc}C)h1lfF-ZDY8ePufD-Ff@eKx^z}`1Ca!%^M#GaE!!dPpD<}F|cH^ z1k#J#_!>k&x1(_W9jGfxOm6{#%^|$4zYRl!)oi_?bq0Jt)wlh_b|{`c@u$2)Wfu;K zCyp_Y)uV{_?8^k0*^P|L3#;5HETvjg;|nkLu6sf?Pv~c`>hGFTcecnotp{HWhN`x` z>XS24CNpyXm;>b<{lINn2GC7cY3D(I*C+%BQo~CoFYb#g-;$ICdDXW2A}?L(PK*Ar6-_2#U&5AG7ERSR{ELIHRkK#v#nu zYoQfW6=l2*_Ti1_c-khFP&$Dio#g1Mg)fd^AqU69MbUSr9+lI7wyH?wRi4+J%Sp-H zis$H?b8f>F-PLNlZuAA!BwydFwVvVE{F^Sf_;C;iG4??A)s27vWB722{PpKo@&zsP zJIqk867|H}4o-J=oe3QA)6UQQn{RVFS`nG&MHol&Q|`@FxlIIEj9)&yOlPp3kdZ!)H5}>!G|n zMwaozsJ00EmA2y{$)x(BBATaSGP9>b5kXhaEW5R@%HYb(1W_y)3a=?&=StRL@q*;{=I3T@@0X-*fb34GX_H+GBbfsdBmw*VDDNJtLv%Sr5R16cGA_5Z)+#-K^0(-eeo=j z0YYg2p-~6Du+RVW!{sfn+_wt!R>Ef$QZ<{Q-hrsL+G1PNunQ>z5&W(Z47e+|*RWw5 zD1E#TtNB85Bzm zOmwtmPp6;pXI1MuorScHJQFDJgxW^Sr+Knyu1%wclgpWpp7*`c{AGwWr!Ng4@gZ(9uM^?=N!0K)Y)8Uz^*FJedvF9=@ zq)neB2#^SPbids6*k zm8weSMv=krR+iF_#;StjYZ~N8fu_hK7r0y)!fLB}Gi_%BUM%=a4~g_oO&&8x;Qtss zK|q`vUBqqTT&d4lEO|qg$5LX9M-d#)Gd)dQ9odvzJ33$w@89|&5NlFTrVuBnp)<*b zbe_@MAS##ubt?nhbVPGvNDgRzv!`PiIFiXGVp#^|fz{z$Pas4Y5P#oX&e~AY2Iqbtavm5?s998xAnJ`uj?E3$L9kN$s%I$X*dm zgAU2`s0azyKS;X|EEBR91b0@Tc;vG0WtUo%B)0RGM(+{M=<#em0V9*F=5@%T2K!IL6JDFp^J$h&54yGH%W&zDk7guJm^p{I6K|^o~7#D zetKTO;0YFg)G6X5^ROtzCY#e!fue%19{JYMU6~^o57xi_QLW=!3 zvzUlw7;)R(Ro68@Y0zDpv56T^0#+pZ#Ww`v_}Xa(+=NHP`!y5sa)jPXme+>0Vnub0 zx&?Z?`B>Q68%sgEFK~ABT*=-F;B7Z<5jhTXgDE@-^}CLK6u)-L2l|#QF$7wr_*aXNL&crPVCDJjqDpM3th*gpHWVNBr;Q$68w^*I} z`tIWIVU@2*C;KDO7@!F~!D$uH@=h!=BjoIoJ64LK2Cc|TB87G0|D~9rcEK| zvQS!Jg7$@3oDia#XLc8N!(GjEivIJAfoHYuijbQLdfTuEM~DAMbrhyvV~r;(myHHt z-AP0J^J{`uz~fNk4$_!ONMGKC*q09PQ3qEH`q=JOUz-7^A9r zbJa>kIXSHEHE5ZED}H6$@iZenD!CwUV&6=vMGxM*oO`&CmzPJ_uiH0Sr3PSuxs=O$ z#g{BaLboD&?|#|pz5GM+pSV#Sh7^E)t;C%Q@^mvy37#JT=!y5d17pqzVHnZ zRwShq>zS}qS{SHpm!TqdR^UA{WYVA&RXU?mEc5T|{uG**&}tO<>`DHdcjkpH5;qtW z&%?bG|2|R!DQ9VxB7m5qMK2^n3989dh&riY3a;~UTa;rq=~5Yl`MTeG_z6iOyT+v7 zPpvmLE=h=u0(6>E*8oF^li&3r+z@+Ux$0&i=fz zrF^QrQxCL9YhV%%@b=f&V=H2ichdPQtu51m7~WL#&H3Bu%_&-G`Yw1t$0@0TZT|t; z=GT&FH3Y=8>j-a&>xESK{)J%e*S8}`krwNeg8{nc1KAp!db4k+a~Jhm()%#-lqTe6 z);nTOtp`t8$8Scnv){O2*DERd4@*NWD@_I@(Iy}Uj%5dwKsA4mK8ir+D_n%nw1Zu) z<)Cs@b6zERP@bex+B5@U=F`Qui%c$V$W>cokUqCnt`lysP;+LjuvvrC8)5TY}1 zyC9(~lS&YHI(W#>+7Jn+&Iu+W#*lPS$cEiv^3!>da70CPOgsWP>Cm#Ef>21JFEWcr zqB~p}X8qr)z_miDqaLH3=O5d;%sV#%@LGp`{fn+Ha-Ej^HRVOAD(E|;a|D38Ht5d> zKQ_aB16WIFU$pD)!PRco-_NgfM(dUGBNw*7@f*Oxlfqn8NFszRBa^9l{OhQQHpyYVl&c`SDLNp2Ec8SCTnOWK^rQdEG_!H+i7$L8bI|a5qE}lD^Gr9;} zY@vF`D>;7wv+eWk#duqfPRvN7b^vJ`^Se~u=~}_sYz#`7#K^oVW$~-92LOP8V`bn& z0U4FMRSv@IH%bR+V>;@9{zde(LTfoTi#L&0?kk(tFp7Co4K!EAY?wry=r0@hExyIY zx}i}8GLJYF-o5)!hwww#KvC0{L0`+MQJ-ls2Y=xjTB_&<-*LDNQ|h>t>%lMzv{|#G zz<5x*lt;6udSlgHRK{SbX1aF&QVWw0tuZw8TRW`b&Mbp26b7I6zo*)~1@#XU_YU&R z*N!b}%-0kq@yz9Y9EDUlZN5#vNAk7(Fs5Ll0d0&uN zZ@0Qdae6~hMxbF{$e&ALrt^64%Zym9C2|Gij~Hp>WQR8+>$9%l;;Gc)GL#jgDgE zB5dD+`o|ApLx7EZB{Ai-NSr1*!|mCms}~U}J4W8VM#2P)n#ctShfl5{%1OHk9zF+} z@lb(j02jB|Rpp`1a3E+v9mZ^`Se3RoY*BwsQm*@WSJs?V*Ev2Fg z{jpI)-DRon_Smszt5EG`!f zl8&>3?RKGE*{mu)rel+6<)FoqQyeeE>tG*w7nAvG*G^G0*l>Yz;A;3UHeO<0>c*Z7 zg9?3MM<~(T+_Os}d~`&XCyDuWmGR0!gSjB2QBzrhB8$I?flT7(C>xI^wg|uE zsV$a{#GGxdOs)$)Pgx<&!{Z_q2zS#?!OCK0JjHPIHw~6bGDb;Y?qAj3y!W7o38#H_ zIwvUWJ!)PCi$T3XMjOS5A!|wI`Q5P@X&*dfzI&Vy-vAG1LX4_u*5+&`?qfA z?<0}bH%S!eK!hAFqK*1H$e8miNp~o~-PrGKF)xq{w~_Cq@J>CAL>fH2!gcHA@2%NK z)|nRASW-1Sk?CTC99^1U7_Qeba!T2=0Z)J2xQW{rZycspTs#!@Zmk>hs-Z8S1MnDl@Cvh@(Wfff4%*=X(zsj$IjWV~cKkwfx&C#E)=m6SwqC_xIo=6~~Dz z<=N9~t^AAI!JA}QE_y3rO7L2OZdOTAIsH7I2H`HIb@!7*j&#Lm4?>x|5M9QAM_9zR zo@>wD6X%O9ye!Vv0s4FpJjxaDNsEsGY8Z%MqBTNRmh;3j$DcuKpjEWb#(T{ULcW|U zB-`N1^Jz#@iVz{AOLNYjXS6)p56cbl{<5Y3I1U&i(qE?j{jPK_=i>A0HQo8$Fr1(! zlOm+B@}ZTXHZ;Xi58Yxk^3tqUIZ0bkoRGbbgXPzj!wqF7d}ug)Rr^Z>x|}!W|CW~( z5`xBI`$!FTl#0!1foT1>0SDr?0klTWB94{s&lzjR%9-Tw!s4usb!OpeSW4 zwIDL<9*&5V%rHhV%uzdT>ORsE*pQ6G`|swn&j?? zF2go@1sfouXvj}W?&F{{Cy}J1#2e-%e_h43e+|^O((_4cvOplWcnl{vZGWbX+GEyI zKu`1rXcwy7J<(X#O-9I!`OX-XXD47Oh!K?C%x0GBfjF z1@nef*zZklgXi0sMLN|~=A-|qfQlLJ&~0N_pwxTgfEu)$YMq;LiR)2|s**p@EUYeX z4)1jU@Ao?yNoZT1;=>0N|AuG@5Adp2M<-teEO%iK{7hEbp*eGIIZ+|;dkSyT9lVgx zb~nj-pW&B9u-rw}6;&+~Knz_OLwm^g>3J30O-W$lru#h}5okmT&mT)Vv|mh*nAEDu zE-k@`fLhuf5H=`!1Xr^a>D2xXgHjhIFRKs)s>&?PPp)xs;Vnde*r_3aRo!|6f*i#S ztlAEk_Marsj)Jb8F7a9b=|q z)GX!m@O|^93q3~weAO^rFQ8tHWC_VaSC}S}6`Zk_Mu+<*&uo0Hvcp5pF(ai?Azj(Dv2&`|_Z$pga zgwiAIC4g6$%EpCSa>4Pyz(a;65Z|xS-_}RDQ*hv(@isvi4H6oJSlVpZxL!vEYFD=a zPlTps$XVjv-bf3~GqKH7ip9ufEQwal@xeclu}*x-E;5a1M$+VQ=Zl_q{2a>OFi%TQ z7=@N_0BydxAB`u`iu5S&a~-=wen+PN%(<NTf>GfiiRw;fJ`W86yuL4!ykctUFQ@Yd7gp-Lx0)Jlr-{W7Y~5Sf>u$F!3G z3r>`lf7*l5=8F8*);6)$l9d$^vFMVyk_j{G6~G}5MLbHE}ZRJ4&2OCiq`$uhL0PzIU+Wl*Zdmr)_d z6egpPGGN#v42NB>|5L+XVMO)xR*Hn`*XLRdgKvg+T z6#|<~CfDsK@t`5y7~(TC#C_cSjXY^N$ZS+Fcqo##_;|ctsZ-5w)?Br-AexgwtD`|( zCKcQv0lQG@8S#co75@9c8O!G|I?9v*R0%-Zv_}z|JjAU6G zHKfUD5b@~iwXUt@!V}eEA%7p$bYy(^shYN52A?g8UZV{1wZR?11fwZ#uPJGkLjGDG z$D#a*E#|OEJaR$1OO5S4Gjn5ZXF>sf?ym5DCv#K7Fml6wdCa6hN!Q<;e<6~kTEu7L zumWz_1ho67S*hW^J=)=Zhq==@a#I$}w{4Toi;jQq)jzUHHIN{%bY{a@_V$t*F{~=J zkZRCAeqTk#J<9ymp+i-hiA&kYIx4TiOIFs3-Z$mtG_<|TAXR&twhqn@{LV0PSeh)UqHLP$>$6#K(tPm%H;oiy`e8vpsa6qvr^ZE(}&)K>;qYdroA z`aF{hw84myDUTKOv|6~JV#88Beh>9<>9RI#FutQE_f z+z|PbjZGt&wfebv#@cRWpN{-;JM3o_?x1Z!M~(qEQjbznNv5=spSuo$<$}iD#O}B6 z*O9%{@&Y9ZJf5-}&cSaWT3@m)ql$3IF!H$nFrq7q}&p))si0mX6Sdlw1WV6)RxQd0~z)vA! zesHreet%!cSUR_n?({$DOAkoBed4hNC$tU3zWKFnP20h5JJ0VaQb|beW38~*?bKkT zUT2j8)g1AUmM%#5;(px^9uybc?XPxCGp12&e?nDmg4T7D6IiCq5`r0NGY{ zx@XqcYn?NUI(6%aelm=IZ^QB{i;zD8kj8w07Uz|jYM2oRn>lO@6gXx_Q_9yl6)`x5W(rgF3CXhJb-F*Xtn_-_nkzD11X)1w z;a2Sx&X*`Pj}$f99O$%cQ8c524(+yqR7d}2{aLIrE2foFrJ+!u;x|=ia)}mvTTwT< zWPgb#s(^aqkEC9_^R)bNJ4H(GVqjYJahSW9Ofo02fy6os_KXJKOb+6X5SXqQvevaj*dT~ulN@2E+a|q=VhC^|n#||l! z86WJDaP@wX7-3a`_CrQ61X5*HEpyb@^fdp$*bSlV<=9u){T>b*RKPhuj`c_is|a)Z zN;TGc6P&O^U?_9&59OuQ*XMEtN|aI+bXRU=WVjMkt}u4eC>sHcL8~v7=W*{!B7Ptl zkR53YLi2GAsg-<3b@v3Ev*w&)AZ0;R@>Z0C1d;Cv8>9BB6}DUcM4!WBbp|nh%;jH@ zyv8DN>?`g46J}a5PxUbx zLXVA`t+xxprt{4P9rah05##x~tY`Ns-GDx_(D=~5t+j7&e+Pe7QFoeo>Rnm=$*nRv zit2@q=@#tk6_U~a9c51e!O}!VbBQMP`?@k-Vb6~qmD2cMz>aCkP9>i=dm`110LXsw zp+cGBAy+La2#~&9?oSBvM0z^`T<)rD!_Gx+cGvnx9f-lQE!@nxJaTVpg=tVxxNcX6 z;bFrkF6~`au6O0zfWFD0Ne_#cb0y0^>V(P~1Eo|(GFq~c|3XcG)9|G;*0z;q$NHt> z35*YnA={MH#xww1gk=(gMUV&HQuXO{D2>^1gjkOxN{=^dE-fR^ON<0`btAwoL^E`^ zz$XJtd5c+ut==xPN!+dcdy`zoVj8pn-xz}#)9*i7lXFbR6$oHV504@$NC96}l2~>k zwPDAS*C_8em|TKD7Hv9;2db7$!mO8&b2xx7_zsIC_*w(0uOSbNv|*t$M^76*ZZc>` z(?US||1Lg62Z=#onh}cj)%(U2@G>->W_irkTGdOYmCmU5=H|2PavIi?6R6G@o4X9k zQp%r2!wZ4~xsT^yuyU7k^3AA5UxAt|j?-RLH_yyui_#HerZ;VnCG>Pk6=l(wPB57| z_2D7V_0yLW@^Gdt%&_ok_Hyn2+-Za(2UL_v+{bDsJMAds1|iY0I7MZWcm!jGcRgqk z@Vsstq&=as3bWhJ2=%YUN8p421(8@eDM?zJ8tX?J1?`akg$J^VkCaGHgBuTazWzs# z^Vw0wVsW#lgaI`#oqMO#VIj`&6;fl}VWWpED??84k0l%rJ=unfIZWE5gaGykG%#Dh zmpY5C9wX#sB$&=bt}{!?9U8DpCETW-C`Kaj%hup$JWhb0jiYkUV0-`v!^3fHQHYZp z9Q00)(*Yu=B|)w>o1d+&rqoEyhs$BJ$@OsRCE!z4_y97*C`r{mgkl-aSa+sko3GG^ z1^ElTtf*nU{kz$-C};4O=1ba^G*IcL<>Q(E^$7UsEAxCcd4AMWY2yrufVQB(Kyak* z@bs?ddENDE>wNlmqVwxwfjIs1@%N2tXP)e-bNgh(rIW6YSH25%}6m#b2yt#4$P z7(e=l(P^q9Zy(?I5FVQgrx25)!Lf}P+SWzyRlX2L8Z-U5h}Cloh0Rm1U1^dWbWq~z z8fG>wJ!4@x%0wtTi#Qpz3jDxT=bKa&?rK>y7lE^)0uTN-FUAzl;2T!T=t0i=)7$$- zRG@6_5dbNBUc!;Z*!A^!mQ-s=mpat?T1~5dDcXaC?MyJ>K#LB;|271)k`wckdco?w z{dmZ*)ALnwWoPAgStq@JLld$@{)ujpY(!$C5$O@{)uP3v-mVH2=;L{Jch z%_^QvYZ@d(goXn!EXD>l3K&bKTo?TSEIZjVdgLp|esfmu&&3VGz}4C4!2`RnOTPv3 zO3ooS1F~zS0uaDC5@3!q+unECv-+1Sf!5*}<65=Cq6fVO(9@q{_`>F_J^Yn~(v)n^ zSQ|<3yV4UQ>S!I3HsEk9Da~oVt*}6&`ZAsV*(NFnD)WGaGh_C<^dlxphpx4HxBxRM z2YOn z8p>TuDu^0*V7RJ3NMLush^^Wn+rk2l<-oEizZ{u_;qC&OS5M;2l5K(n#({i4_3)p$ zlDbZ_!5HgF`yKq6+hKre$;Z6m z`ce+@lE0Y4_M|q7q(lygFgG&Qh$WIBM`6Vhf=a|H3?#@N>R~Tx0miMl0eBbZ$ff8c zBnZNydHt`DTiSMCv5UU>`FSxNor5m3jV_{IYxS|-SR0gcDcUZdDVk%cz)VjRIqtWyGD{#S z*p4r`dj9H7c-X8$E|3Gkg4(AGM0*R6y_1tp!e{j8c;fnyhjcSV5x|Nteb{3xF8s4q zUIR=3JS((-rn9)Rg?AeM7uy6UM`|mC=t=|kqM)4%@IU9o@+!?oX5e#!rC}il392LS z#!Wz*(=nL=9QO|jR~Nc}PmlbWO!Vy^Pn3tis?}A@83I?Wj|%JkPXr966QU4;eUw9H z#`~i?9oOoAs62`nlX2`~7$9z7g)p4ZBwvs!oZ6mxl;pOfh;y9{s}=pf)^mDPX+ zh14w;oD}cAXapF8=a**-1+KQY1Iv0Z6Aotsr%r`Dn$Gw*GGc$R21qP;Ed;$b{6Dro zN89yvPz@vyq7%gMeJ`&0e0c}@y=E_0&k`a@=*y?4BUM@SIB6?Fqc{z>*4df;3V62d z(ANJgo_Bb#@@#kdT;0B&cKZtQu)YHSMY;~o{l2NX>VUZ^C;(-^auyg-8w9DNZ|(FH zLnfnI{B#WX>J_80wUpB0s>aR20oCQo7Q7qy-B_Tlr}T3z;OjtR(O8SmgO6BINM^@j zd+z8fM%K;d1DkehOKtZBp3*m#*i|IZcD-eT zP(k{|{P<_%cB-m|HWz1?MsJgBNULIQq^G)K+wUXCv*@JMamy#~AYRMR`~LbvsI|NH z!0+DFb2OIfdrwKPL+>!+zThK(21OkBaEL`yYl!^S!c1C25#A~E()hEkIlcKQ1(x_H zeGcj0yxh39Hp7Xf5%;aF=dVJb)m7}4uV8jg4(xD9z4GaJ?ayo~$1i&9j*sWr?dEz% zmro_TcS5eQS1Oq&Xh4C!m{PeGhqvwHp_@`3>X&6$#mEN$IIuVWlLh&)sOIW64VT0?2 z-{mqRL-hM}{_8+3NwsgSt>b2{$I1+M2K~N)9BvfOEhke$7DFR93n!h?9)6=;=x5Rq za?rr84DR5s=hpY_o7rslZHT$&M+MgX&TQ!@SpD*H;KyZLZ)T>zpyvd#m`YD)XQgdH zc|nMh-v;U82sidWi;`;~avB5D9P7VwdD^5|3RHrkY~ayRDPh~k-F$!R^X!xroa<4- z?t1J@+KazJ*jk}YOYrVTL3J%OTs|X69|3V#|6!{}w3z=*r;<;WxbXbw?$uy81q>md zeIBHsiOnpqiwR|sJqxqU+cW&ndkj)VUdQwT#V(< z&*vK4r6}iXd6LzuCGc|WHw*C;HmAJ;M6CV2bTA~l0wwxP%%1?Dt$b5OW!QPdPFawz zB}$uDz?d)SXxue$a%%QSHnxQ2!R&R~_Aj@za8{G*V#@!yz~!Iyl17sa+a98X#&|Kv z%0CDKQY8CX?I`oq%64Aoj+sNAUfPEcE@)fM2sWLBx_PHL5Ut`jV{_?=V~71r9AbA;eWsFga+9=jtpVU_7iRSr`h^*dK7XNpMOaTwP`(xsN{eM zNBn4$yM1)GDmzwoUig#;1ec20Z=9XWK2|x@w#0PD76h(xW3OXMt;B9{`XKN+TAMO9 zSHJMG2mv_*cN1Ik1rcVeQLcAo5BvTK&)iZ6~lHH4pe&Q1y29ANcK;@M>R>oTHH=vE)1XkJli4nR-P2a|$f6!|5ZS})Z z$vX(Z3c>)!QRmBM9(P{od};1I0IzNA9X%jr(q#zK<8Rs3MCZWRq&rzGm<9x`t^508 zZSzYZxcH$^U6xa~;BjGIhp%@{7BFs~W%DoqIrQAtt%kc1bx64z(C7Ov9US$@mw1PB z7`Q=-$H2(=qB;^G4dW6?C)cbS|66l1)EBQj?Y#x`h%PJMUgwu%cX5~F&s5LX=Eltt z-x}hwdfyZu%>En0l0z1vv^J{wn#UauU^v z;Mb?l1Snfx(F|ki+FD*up0s>4E=!)}18$-ry&+0u*Lz>IS+kq!XwqY5WoNq3lzBj7 zG5U$B3n0&>=-1SVYTiyp)R(K!HrJd@5We0Wy6&8LUk)mAih&@$coZuU4%I(P`bsNpi0q}}e<*@;J70|d2y zJrN42YeKECJBI*lCmmO#qzD`Rbhuh6a!~PVz0_!!Cy$rhQ&iGms#)-}vTm-=_j-%X zo*j&?Y&D1tE_w(Pmytl9oT%+d%WE17<}sw$Nob5sFdtx|F`+tTY^D`diMQI__&l0e zw?B-=N^sQ_s|v&IKh8b22)&B60CCgH&CeIxpQQpUuLe66fqT(e0Ut;#45~EP*7nYH zZ;u(#^V1hL^PGt%aBMsjsL!SzTp_O|hsXyQ`%o2l0OEj8wD4{g(EWN{!1HZY5QQ6c{wG_dYPnl?qT$rGLREZKw`ktUUF zPnlRiNPsVEF}UB(_I+*<=%nU)@jAw=<`eZ6OqYmszvf4d+F;L?$3#2}*>=`gc|EvK zVWAK8w$Y{xN?~{MPi)68c;Bh6n8(wE*?&JK^VMr5R{_lE`(v5!I$Pi|HG|953Q?i= zQ-1QwGv;eIT>gM5M$t)NmUQ@T-f6aCLcM`iAjG_Ee@2Dl|>X6JN? zROgeg|4}5dCm zAi75!3}@hxE&5{*n15lzbO^#P(nb=D9-nzbm$MdrYZ*LP7lI~9N0F9C;UZQR04Q{? zvV^%$fww*k?^^-X5njVOZZK9pzz!BWBaf^W8#JM6>tbVIeh;AULXkN)SMhq|CCq-K zdHul?RVhnvz5A9`kXgZXGL@lqC*T9mv5SS9n_A}IK%}X!o9?d`$|y(M713Iz$$y1j zG6P5g?+J=2Zn0g66B4>xTkn|;)ytxb1n&j8lP^1jDAfuo&BUvnI+Wa&ipiv6PdgO9Qi6KVT-swO zaGd=|_NHAihMftBd8-@u)FpJfOr3Tt+GCn-Z~TC6P(0rG)&Tu2%a`sQZ4F~rg0en4 zkC-lXtJ*=DWQ=p|>~NM6!xWa0y7jkyChXgpWOG|R<v=o_>pEI{BcXXIVUl$WaQ)v5Pk{|Dr34t9?8d4GJ( z#1v3)1{0|YItMAH^Nkk9{?bs zv8VOFR?AaRGL)Su1??9Xu2(|Rw~OKeGyYYuoO?Xie=v&gDPrt9o?5Ir|2ti;w&U`M z^6v{H*xv7MnUR zneG}92i7G5fN(SSP+794t##?EWAIm^T8-#88_vpwzC=Cpvv=Gp)R)Ao`6Z6)36c2n8wcLiV09bo5XCi6Wx z2iwEVx4)GwXNp4cp|rJvt1s^tkbnRi&iZ`+SAE2~4_WZ9D@kdIN~xlj?FXA~0Tmp6 zq`}I4zLwQ9diZb~R!zl|^_w3F1T7$B-lyMWP8%fs<$Haj-O zs=*-ci_X(wAU;RfH9Bc>d|F^kCaqL8=-W_2tHjU;^JBxv=)o)v8J|HzF%5S8n~Qhl z#i(`94XvCJq%RU2v_MS?iT}3Gf4F_zD%an`&6+wd=a<3KP7xiLSQ;aO0s_#WOI(zp zzNVcCcoZmhw5+`dQP$Y~w5#WcLJ6i)jm2K|1s;sSfg|wpsI?t2%-6{m^`_r$J#^6T z)`B1Rdz5NqAgH8J!QxWGlfkZkKkAr#@j}_NTC?!hnB_?}viT^sN8Cib0glY*4dmZJ zf6tSq%Uov81kz<*9?p&z zNcuQp|IjKBHQ60OxSZd0Ji7}z!OedFFN!>ZQNc5K&-ZzItGyQRdbeoy2kM|zDrBTc>?P8i6BEuZMu!fmbTrU zzj<_j>m|P{^nQ-2OamX@E25$CkMt14yWNOu2f-82Cy0l9Ur;+1|GL|w9jd?zxRwvS zsA{9U3zfcu0;mj6?Qgm-7-bR}54?7BbID$IJ)+YR`U0~UvzaL2bI`) zT+b%)TWK!lbg(Z)veY&&wP@5v1Yb*R^P1_qS!@$Ghi=|Jd70GjwsY~ASC;KM1Pa z=iTmOU4g(SjfJL0oNd%x8xcP#BrIQE)FT2#iajh-Z8n9Tr~H!gOK=%(rAi6(t{`D* z)c3S{>%f;HLz@2_LDY^d*ODF&8r+r!Uf(86T4S;1dRvs2Cu?Ga6lJ46c+Z+1l`(B1 zW;&6SuVaK={yC4=-S%d?+tt-IL*7z_7{1OENEFM_OtHT(KK^E!VG=14;R-)Xv%4{U z213AePB~qoPMum(nsnvAuHKLjm~=e!)(9R*6cPEo1*g@`4rA%6$X!%rpTeZ5@u|re zs;S>WTR^tKBcu`&dgRx?lYu@ioj4Z^=!IoL$E{&o-R|h*Na*r{(VQr-u1d_~qUXW~ z!mHC-NHx(Q{01F1&DhE>wEgN;rop4KpKd5n{Ja^g^2%sCy!J|$ow%~Ttq`5RXU zm~3DFO6|S>cgb)e%7XI~44ELWq+d~WvpC6#lT*d-$zECUy0yh)_^1Vmj#BtPQvNb1 zkEIP9yzxLFcyI|2AOx2HL4&)yyL)gQ-2KMgg1fuBySuwPH?Vbb&QrVle)`wD^{bi> z(>>SBba!=4cTHWQK%wO11-Ofte1Z@+xni}>et^0H{40Wj)k+gPcGV}0tn7@|hRDTY z=bn&+;8BuGe^(y6V4tn@EK@qN!RdB1GVNvjhW=Z%??x0&H-45IOy{-(@fY;Zm_i^! zLq#O589RpQ^=U4LqwVz*c(R7>U@WDqTA^GQd z%P-bcO4-h*ix~vOXJ^MuXR9|GuhRL3V(>aXpVw2N6%%y7ThyHg*ZE&AI`lbgm8CD^ zo7hW@sXK?Uu-I61rqgCeBUw%1E&ood4!c;w3=!F_M0&q9oPQnCQL;!K*KkM*A=9x=);6cwKz(pMD@(ebFsMhJ+B}@U?M+;$@X=e+J$@iZzy%wP}tnAzv^id7g8o zgv%ezHg#3W%981J5!`=v-a2$B8SAt7{;38_n#k3{1=w_<(+L(Pee%H4+Y9M{j^eqm zGU>P}13@P?_GEbOq4&g8`vjXt_0hmj)od4!9+}ZKwO-vV5l*NZL6HIpfu!i?zqC)aYviwfk!rvvGEG z5&LaI2hbC;u?|g0QX+s)${#W|$2iBM+qX~HX}|TuCdf^qjWZIf&aAHe72SMlQJF7_ z98F*>UlxrS{Cy4|qEV})`1_11jW+hkkb$O+M(9w8oj=0{7DP_d5mmQQnPO3%{EeVx zV+(47exkF3gFkCHdgUUsL;0L-QBOX z4Hy5FAhegiusWYj``h)muKorAHL-Z z*6_cMK|az5v_~DU{hrPe)hhmCh0(YeH*{`#k~C_hpN;C*s>KRbeTo-ITAlit24f{M zpc}BvwjCx9&l9AfFvi&?6;^J2gA!-K`V^2tkP%w@o8h0|0rInrG*;@ZPL9sBw)1XU zZ0OjrvAObo@a{~{o1QM@=Tr1QWIoRpoVUN6I}hP&JvvnpV*I(SX7(4NuA@LlPk-L| za7_^-NN}o^?5{lXn20VoS*lon+SUQSZ2*EY-LagVo|Uc=FMg{?glL~W`2R@!UT7Rx zvq#18BiMd#qMn7b?KK*`<6u~cLg@}q#|Pdo)aqkF~bFnhBjJP`V5^=@_L zMc@wJ+mfyUc=yV$S6-o7sh5?C=>W{I(HBvt)X^{$3^TB4&7}&vu^eK!bywzldwUaT zH5P{g^}O*-i4OfHqtJEr%ybG$V4wuEPC4S>_;+Gs(^USiD2T%3^Wd72 zHR0h}b?7R_f zfNgDZQlCK3h71;eqZ@j$9Sx)9S%UQj0HDbu;PHSL%lKFu-Py_?@)E2mB;OwxiIO&0 z?{wFCH;htq zt(Kf6+OV-(3_v z_DBi)XOppF=+1i=7m`LK+Zc|8Z{FWKsnvecJjZwYzz8hatRM(Ap@tZaF)6GJ+#UyYxx^XkfRP+lt8iw^qk_wZC`wO}EF?EW0_Tk=Gn z@TjvoIXd5rX?b_xKJpXPKPM}m+0Y>tzX&Rk^>*ta{z}z@+uwiJVo8bYwVO+B`HHQG zT69j>C`$Txrrd@MEqTwpRzB#Dtha@M7%M`|iDWb;#do|m z?HE$y&F)4EyhVfF5Eui<-`)dqf90)Jan_E+ zK1QMd)IMA%MDXVa@zJPjrBboh>sIp15S>iXs_RWV8!V98a+2=MA=Ji!Ly|HLn$^r^ z=UtdnOP>XDnELk;9)C~t7Pvur7vvR&PS15ntMjogj!}-(@;AddpPe&Ewx9Kd+RMuY zL6DA5<^@Tqz4D^Ja2N3a3`Yp^SmGcZyUoDjjP;pzg{7-66}SGbf7C&ncX@> zSE;U-Im3aHHk@1VTlejJAWpX6%-{{PGF^nVii|0pu>|1HbZ|F`FOeupI4!^yAGjqG`X zfPjDy6OtAz=hyZ9H+>g8Jp4cDyFixw_`rzu3InRp!|O+ZGv#f899p#MYN}8=yu4oo zUpW0JDCsmpFhx0}-`m|(%U5v%5=kdU+itRkTN?7UTYYbqQyc}+31~Y-OKO)Yatp>1 z*`v;jQT}+kPN6qeUr%}?*g^BrG|TQhxzt4>kK0nW-meD3aiPSnV2xsSAFvSn_XD5? z^;DgcLlJmdkwX_QiUF1?>nKqpWO zoz4xUNK#F-16;-#BkJy~Ye5m8a7q=znOVGKYHb`mOY|rDr&+BEno8*1zK2^|OU=|= zXkMht-_>P1WGKu!YQdm&n6rC@(*g6F?{ioO<}wxx8eDivGYNM)g~NK^rn@$2&tbBU z6?)mDsMb*X?YhjAbEq|ozVgDNVi}+-)MrzfBo(me;ZnnQ;Ut=N3)9MSI9L#u_~HLn z+BiD+M6M%dm$?^3ne9AjR0^iGxKzq`^UE9;aY2D=Ha#h}aXZ-5E5tJrO93kVy_&JF zx2{}Y(&!sTEE&n_%W8P};Nn6(;;a6k>QjBBNTl-q5_>vbu1Uhzw)vTivrn4nZ8+lk zK;-nr>W*)u&(_-6^WHianW8jTNt#SC{!pLda2d#YhL)B10RI#D6XY@^1cVR_1jPTV z^M(Hz77!31KS3N|!+x6;)$2kh=IS=v^GmeOf*-;vs+VRGnLk|P{WHZW?ChX=)YqWP z4HYJey-qlh{&eZoAVYnLjR&*3b*(bbm!mlo6Z$V-g|waN#;M0~ZLztB4;)RH&J%bz zGVyp>3f7;XH&e~(&Zg>u zEj{<*<=!-%vBY8Pe!DhlX3$rR%Z-j;&zaW|tgRt^f)V{Br1+H`f9pQYN*((te7q*7 zRK#XNq?c{VxZ6!H=5j=QuetRNU{-&QGumS^kf3|^nddUvSPHd0HVI%fcu;Orc2sr0 zlX7oW)1dne9I>#zRoeOWTUn;vWuhx>;uHT2qsNbM6j7Mh{zkLWIv|$v@S0YkhzHFA z8_ftezMroS`%JMnY^JGLPH!{v!hX?x-Xlrw^Df^u81agKH%p2DKHl$f{J{q1o_r#e z0~`Hu1F3lpQSycSM$ztC+4D8JRu;0aZ3t1q>Z?2jTsfKkpf97O1zvrW5CE`dPlgtW zI4>0h;ruy2k@F8iZ+sqfmIA}^kV&fNJnRO8bi)JpZkO2(1(SRw@a#p4 z+d(mNOTWXYI0m$F@Y8R42+kZn+sQy?nD$5dn{NEWf!#1@LzS)&ym*)PZCCwy49?BloZdbSYQc~X2_tL) z?Dzm&wb~k(U5*~WO3Wld)dvWQiYfteRb$V9UxCF$(E41!3CY$|G)i%O5bK=z*Jt3t zhvx<{M;3E%3NBJ^FTVRXRHV&&CrA6|n#hdsVl9E0t$bAp?(w~E=16XkLU+W$Z)7sx zK2@7aH#V+)7HuJ9!;AeOD!YU)eu9`CTz&7(Au;wvwkvDJA>WxZ*t1BBE_Ch z%OIy56&;MrCoGE7-=1bAa|TnnXmQOw{b=)C5Mun3@c}%>cFlJ4+^mRAQTcI?p30TfNX8CdZtnaG@*RWM-vC=FsSD`xf&>{ZJc{UcZ7O_x%XaCX{j+ zc;rRgmiuH(c(U)5&_Ov|hf;~SQhkI7zaHl{5k%sw>(KTy8uS2L3H$+UfH^RmVfB)TWyyu7tv1h>-hq8T8ue6Vm zjcuVsd()RLeavSsxvn4Ac{&2!h08zOGbr?65OWHc3!bX^4tjai&YTh1)_ZN$rYVUG zv(=30hW!gT+#%5n!hZ#{hy*^gli43lQe#Yo^_&U_Bx%R*suJoTpZ zDYyZRd%xj;y$J`Ej^udiZe-VA8vmr-KPl|c|1+6tw~`(bz(PR$BLDxHOhIE}#6T>W zNCNpIkB%NX_m9|9Y32njsh@qK2ylschU~bXi42VvBtAk}pGT4sTtaRVp@BnO)84`p zT+a)RAKrqWVw+Sxg_U_j8r#y?HFfHlBhHo=XV|kTy&iOUG!w(g3>k9ZM76tik=>Ju z3cp_6={mfOh3Ra(qxRFkU0(w)2f)?XSXQQoC-*t;I%TffB5{=QC7XAP$V|md%|@$B zT6jCrL%!e9LhiaJ)2rM3yBjz<9Lw1=z3tiJUBhh1;of!e(Fw4-KUcdpo4++~Wp%c> z5V+B~$2PfW&$#7bZNsgA%{I2_z-4QDnsLBc59d36Kxtdbk`0sT={kI{0kl`gj8SxS zG(EtNjL+Y4uXgW!IS=T-OGW`NBPT>elmft<<2xxX#iPLtBGFY?U){q{=|ZiSi&edN z?!XJmP+pa(Ta9*+5;4UPCY{ehU*ys$-;(?9*OQ}qy9>AGGBo@5ayGNo32@(2IcY_z z^Jmxewug4MmJ+j#$8B_J;5ll9t;Fj`lg+b+i(FNQl|{aa$pvxLz)11-rz72m zxzFK6VYlq|cLQ~aKpK+q&{um`;B4=8y|N?j@v&Xp)RwTro4R`!p#}UKHTz5c54Pa2 z+uQ5yBgeDccdw`YVjyw#K^Yr`Rfhmsct*{pHg@#Zi@|dBqpg(`P;eFj7tsQxt%1jZ zaf(!jzJLwd)#FO0#XrpdiO^XQ{3ra}_@T!QzNaQk!iI8TAwH4caTm4|*IVq7MU>~Q z0Q;wt`Uay`wt-vWhC^7#w-Hzio6S?G3bk$ohq=#oQr3y9Zp>Z;S_Nm(9~qu*qb1>7 zirH=(Jk#Cz>+1q{K*o|-fY(PtT)5N6t&`C;*YVU#=B;Lr2;Qujn}q)2$Ry~kf6W@qVEMbilRO3`|B2fE}qLMfns|G8ISWEV|j(AMF4 zWzKZ?X)g?-(ZCKid~o6hgWh+vt?lH>aJT6`75gG~WkAm+mNRo5z+I;xi-yr%yWP>{ z@U)xSe4yW0Q0mM)+NQ*7)4RF?U3uEXwQTD#oIq!C-)(9W{KEHo9)nOs*l_hLSvOUe z3wh6dV90pO3^c9=K2w7+I_VFMH$=RPiQi_CfST&AnjoS|9)8kXEd<=zk# z?x2%b<@4+Bk9J%@3;u11Kd656Iu`EJTrz%=x<}yj2};Wxk2)ULL@6NyPc6HNxvP zCo<+>uWeunXKvo>FtaAl{5`qkwhcWxH|ntv`EVSCS;O^Yzek)vayhz%4Ay4P)O5!EL;c%@DJQYZQlGb=e>=Zqep zWvj6r`4E7fiAdf+=b_DeNYdFqmjPzjCX8@d>R zIbKd3Retz*Vl%EC6JB02VKZ(2+%M|7m~Rq;_E~B|3zNR3SKtEA542?o_GYuy`u5I{ zF$PTD=-8Pjt&TFu52Yp3c=2-A2@%^DtPSWdUC?$pTzG~v8a%%WJTRUOYe@(|`wR_ltM01~F0EyW-4l46YT54ztO-wLTO6u{W{P>netnzZmi(4+iPaHrMM_Z47kON|^t;5$I0oD;hck?oK6VO0)>A|$bM7(gk zLo5F@)U8b2pS;Y8R|S$I0>N~cG&>EwYb&a2gS(Gykef$RsBS!zJzI}EyuD+*Jl?Xx zAF$lqLx_8s70IvEA#@J1F}5e$)|upvvS(JdpCMO8MfTLsW#cBcesFB?jOD7TGy%4x zL-n`oR|mFbEY1<@4B&^!?m9^&gv1ltIgBd;?STDm?vI3dKL|q!M-O#)!0yuzWDh&K zPC61VFCb$XFL-=Vu~2K(=FXQ_zfoF>fp=6)f_mo z%kzR2VF0D=P50jQN}?$G`~wK#o?+v2gW2$b1V6i&K+&tJE3!nKFY^RyT1*Uwawf6JW!#%F%muPwi-^}xSXL;YSd>jxHIgoOMb@|v`MJ={KkfaFZfC@=+m8F(uy8P>6BUKZbi8sr!|9QRF&Ku@UA-D*>^M%&n`?V=) zOHX|9SM51u+(JCN9HCi zu~&)UkWaUgVRaEQIo$YqFU)2!jDr4tR!YUUs8OFADHuQVA=**m>3pcrhI&I)ZEnqQ zwtN>~WCdzBgJwBl+wr)k?tGlLwasOY0EFlU8Md0h{Pn?bx-O3103hB*rc5G~ZFjGJ zk=AM_4Q+|Ju*ZqT>{$hnW`v(r1(J??cXoAhH0q4a1G$!Y?g=uGyQ@qF`!-{^m!-Q0eF5G$+)m=78DW~`9Z72(O`K@%21~6wxTqhL1q+)_Y9Bs+`Dk` z$b|GrmJ8`wY9=dEn`ianD*fCTTO6tPV+GmYExJxg#*%ymxz{7Qwk%h>yB<25NH=Ai zPpC1%5}C=wWW~;N)6tXj(D#Gd_I4rm?b)xw3AWe0a0-s$VBm)K;AhG~D*SI@-~E`{ zm51lTH9PBzIum z%+##qF50>{pK%%kUJ&O} zA#K5*<%3G?m1dB=`8dg~-N%KqNqn`uv4l28p8c=0@j!IyADHtJw*Cn;a|`Oc{KiH$ zlJrN1mo$S5+-l9MOnPmxC=@(6u6xhMdbF=XPfove?OM*t=M0t4_o-&D5`4Yd4>xfU z)CDE2!n!`xfu)ruY2=+GWW>N8b|W?DwKFUCrqx8w=(S23`&d84?^mTJjU|{I;!c}V zvl^uC9RSLdQ#FeQ0@wCX_k9Ex%q33G0v|FB=XLkv`7hYTzim>i_E!66({-2#ByX@= zw^%GYCiPbOvkJ}y*NP_7mm6sdIpv(cJ5c@}FRiIGcld~ASN{illyIT{YUo4H-uCEk zka^qcMRQGDW(0IjWm8nKYF)U{aMNBV-Pbk~#2#QZ)pCj>joY3F{G@x}lzxD@<&e2{ z-pJZzFxo^ui2fa75aTU51y4bM~cR-6jO!^nJO_(fKfb#}r{-#Nunipt2PhQ7D#HCeU1 zelr1fQJ0Ej2CfpQ9@FHShmm7hW7eIh2*y#WC0>jA_kZ%z<$_Uv-e`oJd?^y3 z`(~kA4Ho6na?#!YoRA?oo^ZbURJ5fV2YzvZ;I?4$BG6(xF8Qf4&vF!1^_e%Q$s+CK zA@S9<>GIAYfvb}|itN;VNo3>B__bw{O<$W-NhvdNj8p8M(eGv5D%O!9VSJAu)@%Vt z`HCWj5<1Q=F7(k5)Fc=zg}oVyDb&EvyI!A=sD^7 zdQ@hQyR11-V>BO0C%#T)7q7H>tPx@oNu};uGyl`F7J#kAwR=FR$%BCL_{+ zMwgf*Qx_o_N5vQ=$5|)?N&Z^m?Wqk(M(;_(?Ah&>>SgF<=mi*h8Qe{9rYKbWarX0Y zC>rb#{ukm6H($z)@NWp~0!n?Hzmi^=B~!x<^^-vQIH9)~J*YjXJzUH^waMmuNcQqb zpq0FMoPBSk8S{u_g{)}LQEJkUKjlAW|J39hr9qEj{jDK(VTQc0wa;puRJdH*cQ4*{ zI-Ol07J9Ind$*KN!e+Gw%G~>YUwbIFryPcxOvL}DF|?N@5x47Yx#Q-GoA9$6_h!2h z%V?}`CrVZ`_O}3@pSwj#LkfKcBS=7%3|3qm;z=K5p~?lY1Q58?xaLH+$-|?;N{NyZ zkq|BMMor!$9E_6vygy_HC*lPDwZDWDSTDnF5=cVyuYamh{8R1jrXyJIU-uIXwPH~J zcl&DN2u}$<(g*69`pEs;Feb)0DP-?`3`k4=v?~M9%P=v@z!LBlrk4a}`0~FV;Ov=1 zdZ7>08~pEtt8rj9!sFkr_6HOV zYW063ktA>lrDw9&^Z!*mcJ%)!M(kN+wSP{X7X3c@@peLQ-OZ)->fMs4)vRYWd}wL6 z3Phnn3LHSqm_U`3rPPi2R~uN)myRdm`-F4>MSWJO2t*d@9us9NOX@BIi(jpna#E4` zYdwlBv;Th3!?AeP0`fhk7Xo*s_%%}3Ti7MBgTc&gWqB)Kw`vm{q^WsH#BK(0+RO4* zU~)TP#5m^y@-`A2ETb(ZMy#m%jKq{9FI#}eYFH$zw-$2p%Cn37he!LTk+4s<m=7 zKi54w_j)Xq?(OK-QFgYb2^(A4Po+MzO>HNP4&9db9UZ&B?7Rf&T^GHb@4>#k?fcRk zDS!BEs1TzxnD<1T=#Y3#T~XA2ywmaLxW;j=f~%>RXQlX=yq-rktj}V-@p|<-V5baN zZ|F^qNI8M{Oe;68{Aym+YbmtW;G4LOZxulIgYDuY-5-}%YppqpGYzmMRq)KUNh)$Q zt!d3J>}qiZ&ie!;V=&w1YuU@Q{rvmWU69`moDJWdF5!l63OgH5A|$E~J5`{Nhne~6 z{z!t;Su0MilSDkrCq%^&XEiPyEy`TLW0ks%pmzO8paSLMXQOog5%p~#igVka_ILK< z2_l6B?tVU97ZlmZl0w?qJ?O*rj-Ad-q-Tkte|9oVbvL3IeHqgHYfwsf)?YTEk!F;b z6+>(hBgZt^z0&l`l(Q%2sB~7A)(pWe?up9`5*B&;IZFpY#TG>UGkkcudhY;co>cUa zjoF0j4_sZ$3gIh4iW*!uWrkMf=Dy6N)d_myd*OztHVucpJc{6`Wa+E}&j-(a*RW&y zi<=)Bf`#HrUj94`R~HHHSEOQe87i(O%LzD#_~AqYQ8TTpnJJp_AnUbw`z>&Wq_8%7 z5u9lj^cnk6o!;)y z^TOI=L*s*@(wufiS>fZgrqmqs!O3?W`w*MvT_SIiv;IHpKh7QkhoB;uqSC+i@cmY> z>#1&-{nWDudy{=me9-YC?f&qzMTdK5JYv@lo6}HZh@~Cc-HCOAQ*;gN7H9W4h0k`z zsVTe%99-Jo+32RA=CT^hTITAe6QS!=VHzjy#zsa*F0#>jrs?dyRrUlEImJfL}ggaLvqe*A8Cp1|);U zgPU7;tRgkj#{=!8(cJ)v;o6_xbXp$+t%}4{=AN1`kK|F<@h5H2+Q=p;CpQRhqiA#N zq%YsN%42D-UXlq&OkgK+`1YZ?n7f#}Zm7C$ZhGSz{0~e*@iMWw8bk*vTzJ^Ih<{*V zT-L*BU$Ybc^r#<1B!660i8M@w`;)hzu-~H~9c?6HT#*JKk)$fq15AAW!{Q2<*|g_u zq7me)l&h3%b5xLALTKZ#8JRZdH+uPS!E45}Hc4NeEbjoF-(>nmsk30NL@^hgYeWZ< zsC=7s<2t_wa%h*86kg&htfIT?S5Siv1z-rza`qmSQTf{58*|2RketxvqNCrQKgZMa znPEL~-~TkQUC5B81g=j{i=TXnAnrTEpY3xkgrd0Fwm+gN@A6cjO6Dy#Y==KI4U;|M zN32pCTay<4t@IUW)5!1{HOHJK?ubPy4+K?L>Edl-EWlo9xw75EIMb3^j3zxu0l*rX zZx%Y|CTpt~P{|yhoztllvT~lML~fJqe7P2U+4-UW&D=p>S|}9HlP9%s3M;z0&e-eA z;fk7;7acLFGD*#mT=}!tN$wnM&n^}2IOl560148rgUTlzX?j=FV5U9QSTS; zPaTOisjh>^>1-STwr@+y9feBf1(5wfw{CZ~Ozhf3^(yl=x6n30z)P}oR=GVY`R#m$Tkgm9!H$4M*{kEx|pcoF%7vP z2Pm3L;%V`G*FK+}(3yh7F`xa6BW}`lo5dtOL^Kmi)>3|;`5S9(FC6T`_?R-@`&%1GL+UmQ5);afL z76Rd2WM;OgTLm({Q$=5J51anZELs_Bd}klCBnqG;CJM~#IWgpDBpkDPG;$Mdjxye& z=Yk{($_NR{(;Qksm^cL@V^%76&bCJ@A8;Fx0(f^sSadV2J2Dy`f8);z?XOisoZ@zo zwXpX_AVgdc-FUrJ7FshkpF}(lIl82VUtm=JdA*bI+udm}SS4fB_1O#@Z9O$0-?0wx@A%fl>!Z`RiGM|r^{>D%o}VMu`NKDbdm*Vis{m^#spVv zz3(~cuiNagNqn-4>RUf_YGT8wUD`B`mdwXl-lExNWSbWK@IGFl8VX}^Jr*Y6QVGil zCVYDzks@fl0<QOV z`Fv;_)k73nZv;FPxB6h)JZ`96W)}EOWxoA})_J>SXxwCcIWDId8UpoAx7yyU&`;o3saPpm&1%tTs^sOFEP&Q8Fy9fSd6)3{Y1GcTJ@RlI*D zziw{V>&~nB$maIbdcmy+M{Epd@s)`kX;|u~Y{gn{c?R@9-@DDqZIO$rXuON*zo=?; z6&sO;*6DY52BAgj#_PuG-Z|(g)%z+LNa89p2T+yv=Vz6u>-?&0_Ekcd#V{iiviyUd zQmf>vn@HVRl`KH&TkBd#*n$@C@F)&1U%4P3 zoNoR=q@RH1yG1Oj4R4H}w%fymWPxOXyp@A;Pv4IkD{Ow_JQ17y>s^4V|{A?nghJg(p9R>>)>qN6AMI@T2BV^P2yn9j*RIL@=#t z-iY^DbvrDw%a{vXg;pMtP0+%2--vUEChU$_bkC_{!yq>O`vK4bPV8Ukk%2*pBBsla zBKFO+#faWKGhCI)pV_>7e0#Hli3Z6U{QwYZokmKbTh>(zR#kjDj1MP#;3lHcpbj#h zl_V${p=_o5yNpoO^%$DT1u6cs4i)2Coa^jA$orLB6D{&f@?TGJUy^H-lW0bgxE=ye zp+@)}i@&|ICh^<{cF`{s#lgoT{3Qb!e=-w-pZ&Waiw3LtH{dSlg~*QCN_a1vYshaa z`IiTb?W#+xNJ;Om{F^2L0(ni0K#UI0ny|Wa0*|xc;_Rgp-;`6gWQD7_aS^DXsBB~~}^mYguJ_ajwleM}+(f~~ZFeM#S!}rd$eG&M~Lh|KsB0l17HNk;)R% z&-urWsXwIeUtz61KdK(r^IVr{+Sm9cTfOK%x5)wRt@%cQc1sy4!laQ0qdIIZ*uP z{9}5Tv|QQLQv~>mv+GU#Q1U;m%p3=~7`}CZ`yJZt^K=O?WB&qR#?q()P8akipyc-Is{4$pq$#>p~`ttUIGIW)G?dNShj#+^%XJtRSk$FTT>)wfHB@woYhzTXd6tKc-$B#%!EWXe1t?4$M?JiFpZv(9O zRlwWzxyI_rUG)|BC1TXd8-&%pxWp|;SxWuR%ycedDSK>Iw)YvGGk7!N%%J$78_%G+P0OqdU|HGP5v;1&)Y?)0L4i>wp^!zKwfy}3`K+QV zYUo??ZD7~R0D@?Tlh`|(-_%aQ`NFM;EKnL}_$oLsmiKieEza?=u(cydBw#5QdnAPx z$ARGpr3Al~hdw{Xq_`E=upITfMOQ*-&0GN11MHuV1$A7$-%C7rCazQ(zZwvQkPetb~%}@FZvX%zUYChA}z}8)@~A^#Ad+5REzX{ac{qU@&!lT4Y3BSfo%2iJ5R< z9xhR~aU+`I2?&bS@4Km24nMDANgWkrfh6XNuNy|{_~{w5kJ3(31d@uc#Ca*1D!D4T zD!0CB+ghRhh!Yi*eByOk0_eh=07}QI=95JPB$Va@D9w@r@(ZZPk_0N#4zzDcXIR3D zL#N%^dfoBx>cHA~G1i~n0*Zo0)=uM8cpa{(EPG?j4NIogH~lgp9lW8Kl}PjwX!F zIv|rI`IiSxk_Tf@;0i3y&cbP`PK1pyF(jt13g~SO;_PK$D5qF0y2VQsDdA<^AwiPg zV-f(beun6CwE6wX&)&nH`H)gObOwUA_-r7@DD#(0^i3=bC)Z_vV!gDvMr zNWIv;qp_tIs!mO+AC#N2*5+CS3BK zaw>h~4cJY(E~fR?&V^M2-?_d-65Ok1EObH$_4)wqwpXf0g>uLFwNvd@aOQj2)!d~*;ydvB z>O8TeV!Y+)B#{#O$J@;79e3S6@w6 z2hm%4_c~bGiM&W%T)?)Bo?uiDPTY6tE|2QqAySI4$4M+ZvO=30?fOtv+zERYVxy3C zw_BT_y^dE*@NaI41~MNS9~z(2B5$CyctfWOa@blv*>#+6He?ulZ0D31Zfp++^Nmf&P@*jfe&GH;ruuHZ%)_-uu3h_dDKtI|1`(Pd?uC7fD#fW%?y!_c+2vR(Ayeu(ffn|8NW^#d2Bn2AD|Kf&iv|>ZkYm;g6 zuValpTO)j3BBKlXr7Q85ySQ$P$y$AM^i+iTi+~M7+P!t`=JAa~!Yalp`hTz#C^&3M zcSxBpt)^3{8(`}H#|!&lyzBnJus-St5~k)~lnGFpqM@>9TEK^$En6^St0cTG!e1S> z-n8sU+lAb{`%XV{Z*}<9`PKQCXMQc4*iM9w*Pvigo1=}R|0kb)1o791x^=U(ti)gL z`m!1w=Q@<@eyuzsx%X(5@dsaAn6q5v`MS7H>!#f-Q%70OMS5B}W)cfH!M1cx+nQ1vCF2yg!FQ5B*EY|T?h_rPcC>ji) zRQ_6^C(uMZadpmWX?GKu6#wfvK)`~LtUl~@oSrzSh7SNUNKka0EE(dU&7=Rv*b_#J z#{CiK*;M#V2tLtukP!VJjhNcG0OgmJ7oBy~LG=G9ds|{ZpPpR3a*Ku-yQ1}@|8@NB z9M1qa=l*1EcVOGC`acVfFqS+~_urF|+yuJS{#v3ZFd+|fDNxgt6D(KtRrXc(omcRl z2VSK!X&(`q)jko&K6FPGP zsi}Jkx@1td6Y}ItLazS$BC*({twqNauoR~DY%qzLC~pUW1-;!m6V7c%zNq!>vc%wK zdT1JbLL09F+_sr@wbW3!D6k{yt+fuq<%&$J)l=A%<%^R}?Q`%LD|e`!A5fj_D{$txX8-cZ_k_N>RKdhR}> z#)NRicVn*tfw^!A5a6Wkq^9!mUdgAyJw7>JpcO+yT?DZi6ygsyv!!*@=lwr+2Likq zhPH2o1Y##!Va4F0}AQ3YX^mm1;=SsWq0YvVYr%hX|@?V{bHoi zUH(DD>|)*9^ZaGlm}^&#hB}8lx%($wf3tI`@ zWmVy2Q8bMre~NUFZM^MWoLwDHe*I6*AQ89E3>sVJ`3!wZR8mALWtK-Q0XG%vTFRWl zD_pnNcNJRh8WXUO@d;=R@*L%9q>Ge`l#9ILi!vkD%sMG(K1JkE>0_I_VaQ7Cngi(5 z&7=~QgLa8VA?ycL=^wV$y_*29tLp;b#k~-h>{rD-(Lj>6Aq)5_6Xxh2A_zwj5NQiC zj>FI{JRdUIAl***o6*PSEG>Qf&;ru&LA-t?E2BWdcT-iX`Qqy@POF#|wlhh!ToKRNBtQXrlUA)6`epMtp>jETKg$b7|t z>o#;%bkz(PRaU?5*B9-v)rDs4jT1CJvL_h?JOsQ6c)XJW35Px2ANBy%lgtnxUMXJr zEYxYPiCCibBr~?Evt0ImJHYX7mKx;ce3y_`y&Fe9OF7G{G+SXeye_#}M~$F67RX{r z20b`<89U)4Bz%Mf-6+~<2}gTyqh$@KbiC++&3snzf2BMTcC3SMZ#L-4wLOX3o8{+% zJ>^^;`^1q_1@%?&V>(BcB1pGrI!-FA$)Xr5MJ_A6XV)ZhyzJiKVo{eOLd9c6GZEXo zLdj66Oy^U{yE1(reKwG5Y}?yV z%f2%(5MLD3bQNr3>n_+`_UOjKV0WG9&~?1uGIY`E@PxaAP6!tbT8!G&r$mNWv3~+4 z8Ek`7VsQd-0&(`%aSEBv9m&9NMNEmN7&|8RVbX=b-(26NOw3lgfD3oxz#$r!7dI8^ zy`?e?>^YE~WCFqz%oNO2@0Mu}oyg|_Gjg7J8T!TWC<^)VxXfYFBj>ErJ0O5*JJf|B!OCvzZTh)?~ONd57=DPccN#nVY@WnsD8G^D)hk~65B2#|oY2_WK1 zW@}lN52MmCJ+Tir8)HO{FD~REWRdBS5IkrSfnygbN%<#qk_ulyC9G*r(D5Gw5{~#W zRZ{#~CL+ zNe2-j4c8B=-V?=v-HoAt8)WNJk5pHgEei!m`%iaXm|FqH9chSeP>gA72*L9hS#=Q7B>30TB*b2CCj}{Ttxv}BvX{H0Ui+T}o>HE>#>0cDQT3H!c+J zh=A2@`d>xzk0SPD^Rn_ha|m6vv7q^{xNN3A^6(rNX&=(Y;dO-54u%f()f&$153D?I zTg9w>67H6!;vpUI#oFxi8{Mw1&t!o2lM>)n!fSjzAj_Q8vSWE)%|4lCC8igX`I#*IY1LC7yRy zk??IDC)lsN+z@G5#VRqB$|p_!cT;-Y$vBdB`MpSGDuQ^@9`^Zpkp`rG>%SH-_`R zIdR2z*2Y&>v3o38YhZ`-ByGQisn5&9T?j1wmlq7!OZ^IQK@Ys$(B;T`@bTJ9$7|x|Yu>^;cQPsw#(!)GS^9@42>Gt}bUFI!y|{_SD0WDEnErF^ zltrV8M|N&1Jza6(1>gP-158{@^yhv1cLAHpO_4aCefR~VJ;pj<4QdTOfbER6o6z4>(xOh@R|yTP22yHOsU<41Znn)#nf*3(tLNof-d= z*|?yUteyQWq0zL-XqHm0nSYaKUT&}ZP_@NX=%k>!Vo9vBi*6fDrF`(?89?#ABD%E# zOG=)mRZt6Eqj?XPyE*0puIwqj2a{fF*_995o$nj%3%UA$q4rWrmwH+@HPm&&7ZVl} z7Vm=>ohrP%G=x>tH?n9fX4rYj|F5>*0yvHySk(2JnVFd>j+r55h?$w0nbypViJ9$~ znVC6eW{jDcZY4Ll=RdFBe6?FrOX{BKR!z-Fno{vuD+yt4QC&TbUM$s~9%(g2HAOWa zY&Aiw7|f*Dk1s2kuEU~jlrh(0(P7FNb(X-|Wu|POo|zz!f{gJQDbu@y`bXEtusjIp zK(CZ7$BtHQGuS$;v3XUU8pzu%;ek^*^TBLzrDr*o^DJQo=XqN_%}bC<7L$A{nywg| z(1Dy$A{SGhVaBktMYW3!v}BY&_;4vxlBE{cT4R&D{4CT~6E!W0sI^uaO|S(DY2fp3 zmpN8ORATP}bbENV${M`DDj%gH@C_J{npO~NyVai%FnFLDp76`IZ0wTci!iP*uQ0C` z6fRZ8k?D;Q0|rdZmQs+{M6>4S%Q?IT!|ahgS5J%EUCP8f7%?9?7bY=OLRkwk1mbmL$Qq^(kxVoIwiyaFuxNG3WEnG8 ziRIe&?623@yE_}*(lk9(O(W;;CfRb`Cgz@pd9*BPzR!EfDI=y1qh}SSfa>Mz7$l~@*x>p_RJvX2MOXf2=C$-7WxILd5ve!o?J75IpjkzgSi2_i?khL#zY zg~pFGg{F5$^dG<-T*dI`(Wu)yo`2XbqZ5Ue`GYqo^qTWA$R{@%P!V-`9Hr2T0O zSb(7=dll?irhHFs8aU6+MA1T!CRq*hYD$|=K$D*8J1)4Q<@RR2;3bVc*ctSNy*(N! zF1WiV*1K8uvVjrYutY8gsd8PgtTA=%*~u0CQP+qpU&Zolqf-b?262XRq7SHxigF-PP|jlhJ7I|g zT;f^FN#S}%X=AdhQK=@Z)|R#H8@l6Z+x$(O((%k0&&9&2N&xhCSWlX#^`t;iFXvqR zZ$+pNqX2K8>x6rj2Xlo6Q3*3tJET!wV8C=E9rb7P%1OZ%AxsMlYPv?uqpDs?p+r(IFAR$)bf9S5QEA~(dPhUe!*|=z&qupJ7iBEsi|g64ULRU^Bg@<5@SEy zuQ6wnHKPzuo}T3O$4SN@M^3xQ{63idP&IpRWCg%gD5v1nN?)pkM0=_kiIw#|-Mulg z6CB+ClDZxj&!rJED6@@^n>l6#B*SW@v(DI61=L8n2XQd@Q9OLpzvB@79+{X}r2INp z;1QpHVU*`U5kB}2xc+>@yemGbMDunLT&w`|jK8!C^7f)eUnrz~*(I29FaK9Pd=RdG z7km1Vuuzh9gCx2tfj8{`slu89nuiTOP|N0BRhr{B%c|!&{NsD57|q`zM2ifZf8x9= z&?|d6)=qH?)Mz(}U0TZj&UB@G+4V}IoOTao)s0d|jz;^7xGKu;(<+wP_29445u#FU z2(YOQh^bkv%n1Y@Cv137os_aUZ){T3ZDa>F&1R;~aAn;E2ZeosTW-gRLPeIX+WZWUp~A?j^_ERIB8P z=lqL17f-rGyOKc`OE|~6EJ-ES>4CEdzh0kDo3qF=`}*bY=QFo>0rci^30*?M%xfhX z*rbBDxOG#YU^QD;c0hUN9t9gwO+D zq+rQWss6%rSc4MoMmw%Vc~vCoU#VLyn*bVIt3?G-+9>H?)p=r^+r1xAHP)9u>l4A{ zACW|)=qvF=vLBk1t%#dWx?e19kX3u_IS6NVg<>Mu;iH7Jp8v@(WQ9sA&@k!XKO zom~nW-~nP#);w-Fg8s*U7S@0Lr^n_0W#=FJtn8#$a={*>o2K*m$ zc3l4d2^TB-zi?#;{wG|xc>lsBNBo~~MMwV)*Wq%z0Aci2(@>%m?BOab2LoP|KI?my z_v?WuHL0nfDj>FAeIM-FPIuVc`s%VaxBfp$zddgUz#^*q}lsRZh$5uMeyQO() z9ojrB?PdgdfGf!{+Kpbc7RzC*pSGaIrsuON3R$FGeSLlTi*>>fvWP?_QCU`z4JbKJ zu2Pr8YA!K<`rAR4gana?pPBy?4#~iO-5KZGfBaJ*^v^%=GXL?<4DmnzKvMq4KNfL+ z{d2(v<`l0tqO<4B-!~&#<_#27(6@`1Qn_B?9phB%` z3F4oeK2D9+Z81&S#S(L}zeL`pSn)+;v`Qu$?vYKDUev|#&&Zj30YjiKZE%fTOBE<^ zf|(D?NDF*T3_kl+C6atx?PB91Ex5(PeAN>|(H1>z)L4t&c+f2wn7XH{J1KBeW+F~K zws*z?*Cb{-73nMCSvMq!s(tTQ0hzyQd6gM_Q2H&r2Q)#u59?7njn1O7pe3vFpj4KD zR3ubmf9__$od&;hm$OAmQ+>yUJZZ$zkeh`R%O^E{@|OY6R*mn~byaGWOS$nPv$w!E znZd+MTV9;kPlOYmK#^a(9owuU*}hVgbwXY4*I~pNLyDqn=8wAqFulKtMeSuen6hv4 zb9znlx!HtNN*Y-05eZ1#OTnCnMXDmg?Az<)$fQU_Ci*1c27&Cbp03?jjla1D{~VsW z$=dz1$}3hL1=&TQ9Y>@h#Xcp=+a22%u`2Os%M~58k(?xQ77M{+C|q;HLXd&6*2L6- zRD?c5T>HR#gmh=a0chKq2o*^V)FEn)(-05Vbo$?!+CIxaD`8c({~^n9>TaRfKPx=fU9_rtHwkcXFPE`Nu9}7cZx~Y2}YR$ zGDaS!t1M~Epx-kEAYt+PWXSEP?Wpa9pW1DMX4#!)-O;60RLy>`XyYz)s-_JhR-gEP zOW%N@DCn-g7X|Q{s8>9ku=}eRNsWaa!lI};V92Hhs*G@_N4ii})7sFJ*%X9TC<`Yx z{vLAn9Ie(A7;>rG#c1jbqsyowW$Z4Eys5U-lB@8$soH#(nx!oz(X_yEpex)Rr2#bu zH3$8b5B@97;zoS0YyRbSlMux?0Vzc=5h!>=E?9>{e!dA+_HY!)M`}(kKr2Khw2LE@ zZvzo6`&tt6TCykpd(pyMf_{T9PGjY3i7Aus9MQ>Zi4EcN`n;CoWP($Qe75|DvO?D+ zSNi&f*K}%1D8x^O9YG9@u;+$J_LHqzUP;cAy7}N1M8x`=fU$c+jTVR&1j;u9AVozu z`vPcW5dBlCQ?UFd=L~(DUPN}eK^bkv!ZmbhIazXcpwFucqtvo!W2+jzY;>gXhWhTh zE2_N>X25>@JVsXU+8!Ai45Q-(id)n3?00I>02uiQbS1f=!4=U>(uS}(QpLy^X@(Vn z5|(N&>6}s0hLu`G`_Y3YseOB0pr!CUektydwQ)15wxbzAE^RJt?lDUK@tIhKR;{MC zO`eOij%K2r#y9@P_Et$~wJ>$g{DQbBhaZk`8T}|CXY7I?sw{1tk`1B?W+-Esu3#EA z4XA3&A*c16Ku-gQwBJ~9^r)4lp~>Im6sBfVBIYxa$x&xa^+Z#=v0YEIlG){nCDl!b zXJgn!hsa9tNj(wBJ*tjoYa^dQWIxwKNs`~(Ty+sYBqWzJNsW`3R6k%jJl?Qj2` zcr-nK-I3MZ=HGWmQ{ey0yTOiZ@$h5xj%+tDN+w&U9cDwSWH*xIs+8dVx+eWvmE;Q0 zC#$Wj%&GLNEJML;hSsW$U3iLoqkb!Ye;Z{my*YBuVjw-|EQZ%zDIy3Tm@KU{tX=Sx zextr+lnT{u2jd2z%l&isj;t|d{C8(=)0yp#uLJIAmrz{Uuq9HHRKhC4^Rq(pyUzST zwkQq&NOs!84}*3XLg$z(N|~<$NB^I5r5b*m*pa2qxryWejAV(l!@Lz!VcG@I4)F}6 z+fvF`N!du-l>hLPQlbA|s_jZ)sh|sIcW$gF`K#}7Hg5wjg-wM?w=<+FQMY@s;#=zz z4fn04Vq2)6?{>iccRW)x>ZWk@vTr~h6e)Ai$-+xGT-?m{Ced`9`_qiZnwkjsc zmZ;>ImZ-Rq?g#L!U=s)9h=GxUqV&yOn{4+F@CYQv1~cg=1tiKmM>>B7!g+Un!_VTr zGb~pU7*n13!LFdNip1khm6ljzEP`696c8{qrAqHwi85yVeqd?iIBozSf;;gqFuLM4 zGaFsraHa%&HV87}b>+%RQiy8x2kE0Rg;e{D$ZBguY(mdwwqOVhyDq~rF+x2@1%zfx z{q&dfIEvg-dp5wg$8$uJz<|XuCv5e@(<2ti1)Vs6V2>k3=ay##4}_J@=%N}#JtRV{ zg1tn=gAMr_51-x3wGl)?n#3!A#fsib1kL%2X;K6QsW2cf4o;8M2j-ReH2}VOP}3lM zBuUUlgL*qvwzD7jgodfq)hYsn;0G(cjtsjLJ7bm_wgC0N>89jFM&irh$R$t``2F36#|A zK7csF04)P)!ba!B%Jrx0Cc+vEd_QJ|i@tw@+dG15?)vg_k}2?ARd7cmQ~>I$eOOomwWzfd-I?m37p<)S z*okk2-J3kcXpbN9L=x6R)SEmNC{&x9h9zudIM!F=OyZ1%wSydJwIy7F31`F$M>e{% z%*mJJZhncZi((8p1bIHYLi7@?<`TQgoY8EX3M}bQO z^`^L+iM!D&^9HzOcTWgDxeBdb6+$4`n6T*t2}R(=3Bl-mIp%2xz1cK~gT=Rf%tQR3 z{X)}DgbQvkCM{;*e*>|Ag-`Y9{5S>4n~ec#FvG4+O)dZsF0}x7l=HG8s35YM-Z`~HSQrl zk&HIsa(gb}@0*{J4Sl0B@e#Q|Y3?m}gU*5H23S|%KYKa)wK;k2cysxhcQ{JQf8`6> z=WxYJI$B=R89Hu~xtfF_FmO|Q`{u8XoJ?>p zRFWN4bEmG4&x-+B20^K~6`JY$^|jv$bdxIChPU34OVE?*F+ zNN zMM6OQu*gnDzKYEVJEw9+9c-fe-Yiw|Nkj9BrNASg4&$G_INgRGZq?jG8N_38YN3dp z))1*+8-t%2m>}*@2H&|uoDhD_>3#-1hdYHm42JUcL+tM3nj9iLlr8F?@Ye%+{<^e> zhNyw^J%jrKdj$RHiTHR)agS`pcU7gX!va4GIuM^t2CAwbAxatXnb6r? zUSW$zJQ}qH({tNaq{#%71ik?l>w5|wWYt2>72J>SkYlaP?iFZK(v@4%Q|ZeCRbq4>+2mK|EKdyq&iGS`MuHM^M<@+%qR!cJyk?vN+gI49C8rH7ZVuIw@V2^BfGiw+s#NgPJW`_5eIH>Bh{OC@#*Q&iGhBpB(*e3xft_ zd~Q(Iyl7TueJJdxD`G99RmtjvoHw;-bA^`!A8dF|bio0LWwIX6Z+-4KF>I`Jf(OIR zCET!6DBbR$pBV=i1UtUGj7^K(ie=x*9;IVzNkXM~ozRLgH*VMmAm-dDJKA~S@SY6S z^gA%k#K$zoHjyYAG2G?@xf(j!r>PKDbbrKbC+qT5MarnyJwn(a`&WcLn?}8%BPhAg z3W^|r79bOQxub5mh-|H^OL0jp~^@GLTm=8kV1wj)UbVW zWtRs8`3EdX6-J>o7Jc}O0Var53FKjahUsVI_hwke!D`^HAj-o1QpV)Y5@JKbu&W{h zHxDr?6DC=z3ouGb6~b{Q3xB|?7K^_IV8{oD`l1;Pb)Ul_jLKt(2ZKg~hwFfQ!6ak~ zhYTf-izMkoRu2gQiFTo7VlE&V&Q`vDX}9bmDdcF#o);VJyenseK=m_a0pZ1$Bpz%> z#uxzMq=ZDY2O)By#TPG-=brWR`#MD}a~tgT=w1PG${5IUf8eJ-uEh*L0`i2XHGg6@ zMa!wmR=-brT0s{9^lr=n#MIBnrTo`R21>l2i!&V)1 zCnE0v)7q}W?A%alB^;`4Bhv_z36@;P6ekSm!}iOaQS#rMTHp5oV^$*BAYF5~E!lRU zZf_dp8En4*(gVYcQ9acXL@&|>Se=w7@V)XBa_CkExTa7W+;EG!_BuQ){N11p0qiUf z?`?BR$-4ehhqd|_ zg1ZDhmfHlz8-c^^Q1K=i{^MTA%dcs3c+eE2lPypkLrxxO&(IxmX4trHOLK_myw+5B z`2bR&L;2@hhMsihkV?2fAFK`tp5vpl!RjdW;FpcZq>2W5TKLeOG6KuhU=sL8h$@6A z8is7IUlkyd=0^BS^hc&X81$p0I%^!;^aeIAMs z`*vX>R!P*nfh;C3*_M9E=J4L=T;iE@Z8X1%OG{7)KNL4(5!Ri4%?TPW;2ox~L5eA& z;TcP>nd3WE7^wAJQ;OyGv^D5`8zB>}raeu8)D;`+H(GbPb)N;z%2P+ti?rq(=71jn z94+f!qnqZm#}I>@G75#7i8V|PL75Rcu_1>ky?h1PrSz$CV-Ya_PE>E*id}`het9_U z5iaueq=kp50q+z7mrujB zZ5MmCk4%U2OyhPN)4(!DlI@X*xog^D!r%rv7NIlAtvN@z_q)+29#}fJZ#nhh;fm#E4 zs^)^1KaHXT39hZUskPqi1s306V|t=Ja=A^ry^BIUXlKg&VN%uD<8iZSxaX1sm@74f zQsmxon`HW2(hbq+mILN??886W=KM-+xMalUs(apH+RJ##3NN%3Cd@&+QF+P8ikSS( zQUJH-OPxTTIuP1O%@=$pOps0R$vaGb_w{OxA1akHx&L$`S_%&*s znGa}Vp&p8nBnx&Z8Wz_4R0h8Swm@Y8&Tfe5WSh$o2orCMqGvKj(%Kv)0-Xsn4)tj| z6&ikfm)#Hr*_6-_1JlsR$g9`@#Rvs-ml>lINn7~BWh1paQ%w_`8e*Xg_*7C4UUH<5 zX5fcmKshMpWd3>FMV*=4`%q|?-=y zJhF9k=2TD`#RJR|Jh>4UEdO1!-Eo9Laieln(uYtjG0I76<4dZjn9VONdNJfWTwEN@ zs&4}N-Sr?&xbTnyBUr>SK#Oo1a8)Vh!kuGZ1A{%>v+_g54NA zu}Cc25Se>OP;&?;dJ^jW4R}uCZPFsdJsUpT7yYPTom0-C?5oi90}4?~p9^j?KZ92E zM%^8>$0LTY6*UHKXdKSY6I&Uno5IsEXLcm=PdObDYOX?eYP%DlLS~D6ap8nkiI_H5 zHh2g%6>XU~s?DzMeZ4ul5bbvB27{#G== zeOAI*Z&S9UvX2)T4Xpf(t)4TFhfNJpsQ1O8TwmrBrBP+D#M^~tAOVsr_GkBX%?}>U zLY%tHd_6EJd9TZZT)};G7tZq=0`hFFVA|VHo>?)t@*nsjr=TKWu%F(+nvuE>@~KD> zCZD%3V34g`t&_Qgd6v50fedy`p>FTKgbKacM z10AzFn+B4=)+3-c?K-7U%U<(jWf_SHxL=l}-Q0f1(A!Pd9*wfLBRr)8*a}gJTcpDm zr4n;k4B6b#489Z^WxVZrUz`=n8@`>|%s4t30~MOgKo$Q8*d9wrRR%CwM0zuWtv+b+ z>OW(8z15|fJ*A&0nzSpKgH;%`5mkHCseX?Y)_)2Ej!5{2lm8q2>#ZKu=xC+I-72lr z+~!c?98$qLbeK2AQl$Ax04}k~+;BNCR#tXlMVXE&LB~L5Tj;FKuxLRU_8Wf2u*$0~ zTTmOVDmal?9R@r`EP_(|X3B8OzUSVI)D)}OOsmj+&}#45RyZF1``pE^0cG1cK)svG z_3t}jQ&}mWDch;q1z_4QFxO=X*JZt&nQV`y5L!RjzRN*%C_FsAzbV;$yC)2A`@@4% zfLnT~+bZG4y6)Cn>IgB;#6q`ydT1BuaWla>m5bIVlzy~7WK;GJd4z4YmuyjnLE$e$ z^#fC7UJ`ryta+CMQGp*8>_>N!;Z#At5kLuAy!)Jd1ATD~eX&B;&c_whbt;sODAsfs zbrG>DF;r76H89g3bnzSUsEgfcXt5i2MTql^vm~p>@ZEzZ2gnSr1&2<6m1j70u)OGf znup2nmGxRbacI?idAwz2=8v}rDaN(UDi^=coX$G}*;Z*{-u5&e=Xr8*$~1$;c6(zn zc%bRQo6Lk9lM@vi$nR1T=Cq_{tW(I<9BHJqV{B#aq8-`S`;>Rp#~<3k@y0sAyOgAZU z;MnOc#=)Y-Gvb}tk2lgrmuTo88Qh1cWX#P%(@7BX(v@hIX^md1Ff7JphgYa#I}Z7% zsxY6DR_;htIG{@4cbTFQDs81%4C}=lkz@q6_Mv>}O1FU!w>h~=?fz$>;5PG*@M0h; zJKA3s4eKWL>rO$^nTE`_-&oTq64N&{enL}dQ)*Lc^Al)SuA63k)`=XpBqP=8o`@Lu zHDP%s_=OtHWU<{5#mREi0@zOyeTM9|Ku;R0+D84%AJW$$ac(Xutg#nTMA+=2E%KCdsYw^>sA*`*KAJT=ruB z_SM(CL4;UodJcPq$|Iw@7ZK}oBiL4@0bO7)e@L6er3?`m4$HEsGO%-C5`(^*8AKu- z!LzT2$O;u7_qG&arNFALlGRPY!oY0`tgcL2L~pkx7n(rYxvBjY<;bb`@r?M-*Ur-4 z6S&2X*DOk%9N^Bw$Z*tbWE5wm0f0$d)NT1uM(}2{FYqj+xK2QMoT`*Zb<9M2#|QQUec^Q5jGfvJe?uAuoPcBg7Ot zg5G$jFW-KcUEX?MbDD)&|IJtE^=v!RoCxq$Rup@WiFWoUU+oa|Eav zPKjOO=)R=9v|8+W^uiIGvf;xN){#Top1j}5`@-b2{_u`1|NewZsZ+pyo>ARys+;a@oa6gxa}+q-J(IMQ z!F)7##rU8X&#ZxI!WGb zRyu1V7`Dr5a5=MdgI;vIxA+p+<^rKzVVz#C)8kHKtkN(iuIp77Z_}_cw7OxPA|nPU zq1auGhguhaI=aT_yAl^5xdl3`w5|Jy{iZtU<8w9gXai3TgU1WP8Nu zC&zwPrr&fJzccKq{h%=}QiA!sh;EofXX`)goNQpjCU<8`K3yqu@|u_4`j(e>WMt zp~z?uh4}nZSN>CdowzS;4ndJaV!ed9T$(Oi1$*HHsD@gnHDeMex_l6F_lap#HP<6a zRAYg6oR?Uu!o#|STC3uqM=C88i^7sHpiOfikRz?<&3o*Detu1*RG~alXSQN`?|2ce zd_%LPNB90_-V1*2lrc$|b84Tpl9yO0=xYv&8uViGH=_U{Z<+N5eD9TosVS`@>p537 z-P6h&;1-%V9J#hxz!pWcOi2I$1R-(sFDwj*?@11Po2= z#S05eptHthj2#{iyzn~4eCYIs73^9qryk*qZWAZok?m$!Fr`oxj!{^WfyLBtjHK~r zgeANUM}#-YP*4vYtC@QapUh@fgRL*EFsUmCfgE+Q-MKwwk{`ByT**^#fqGd9Ff$L4 zwdhqunB-1K5kf35SK&8KYJ$~^9V2Y*=Z^=bQ(t+XWx!rvTOXkNu>DGjHlhqi||M$d!qsS0$?8EX#8@%!lWz`<*N|nzx$E54?d0)+HpYylz4{ex>fI*Iwb*|h$ zz83JuKRY`g&zjO#=#h5;Qm#o;1{M5CU33FUc`5$Dr^jp4z-}SQ!RAu>gR^g5LdacT zo`uBZG{_{aJFYvf`_jD!x%fjhFlP+{{1dnd3v7NiO9ou>@wmn(0WH`g{R(!EP)+{(jOj}H5%lnlZd31dhOnK&^iXp_=~4BYrR^7EtRQ52XEER z{Vre*>Zvys)TXNvyLXT8+YTu*0h^FY_^xW%#dQUyU-u^RS}L9*Gn5!UDKXVRJ~_ne zQ|yL|{-C4vvF5_WHuS|@l*49lIkk4+Kp*?&gz45?+dKI7A_x>;;KKSmQP0wA=vQ5i zD$(<$+lI1}_WQiQsZTB#p*c#4U|BZ=DPjRa5<)pV{aWviAQTHay?idqWe-k*uz=z|t_oo^PB?)*&~tcZ&ew7!Zu#@r zYl)#oi}KWE_6Z-%9Ne>@mg%VDS(%>@}L4{$c2WU!5X{Ttm6E85Soa zISnPoLi9}FKgg21;E}32i>pW_>vxb=~H%&W7LuN|r zTnRL^Mhau5JIFi)*Vn&rj8ekWixsaMabIH3YSN(6M0>JEg*L@-Aep@0-K^0*j)8P= zIe68ch|ib=ut*g1?>60jc2cU=)v_#>yCQzm>;7OVmb-Jv&ZD`PLQa7~G0#w$Ktu!Df?dksH!bsM|25 z!oIFP^CP8olq>GZ(4MMqd&@xcbLC?HA>KFcO8nXGUTaq;e2e5ri*I3)3c<5Cp0?!u zY@d!HRZJ_B_@NgxGJ;8ETElKsQWz`E`a_UgDShV(HX3Qsgq7tY2PYuk314As14w`s z3}`WZc(iyx4Ej3qEo1B8PovOh8T*6i*W~EjszM@CclA~GVSUhakrz0x6C~H58P?T^ z8nZ9R3*GqXfteWZk)!iu5Nkcoyk}H>#<-J|fvwJC<3Cjd8E@j!D|SCNH(KsE9gH1b z%hcP&Zs*=m&qz^7Mav*eQo1|Sd;EY{Lt>_3wJ%3at?Nc^$z^3@t{yY&A6Sr0;*NJm zR~KtPJsv&aDUsZS?UqpZ50P;2H;jyA9bEg6ZrM+$E=As;5uG{ka7>B4f3BDBqKgPG z->?$#%Vzq$1x|N9dGVs3b^G{}%rVM2^YB;x_T#!0a- zR!mQB)`nTCB)NK#ZA#KZP)87`BdDY8-zpC= z7hE2tlCOl*?l&5jj9)e=aq^@cO+q(0Dms|+={J=OJlN4oNb{7;Vr*2xbha2MZNDHZ zsIi1$ggy9XGa+*kl#JR26ZCV%xCo^Mk*2_IwFPaN6;0DAWtBpS@uf2SjUWmY<(ar7 z!dwqhjp7=$zjcF?0&Cg)g9Hgc)xj2`!r07Y5U(Y^KH0m*!Sn=^sHi(BEZk~P>T$ten3!4a1f~=nPcKN2~UGp(U?Ave$t%+S%Pr& zUoT_7B9#^miVivEo-Ejw(fsivPSrVc#4|+EX5|w_1BFyj0WjVsPrKAn10@c!0AL|U zC2Nu~rV!fm3`>;9xs|JC*)P(tyNJUOjzd9fS2aaf<(YP8uV!Wg z4xf zMY+X}uNaIRf#%KSqJ56KMkGi-F?}PUoCyjwRp2rs;s$==E6ylr`Dh}M<8d@hnuKx0 zq~U7&%wb}ABtmqman-++o}o2OshCYR`NT1xs=s? z3#|<^f(CdEOMavcbvtG?>cS5@Q#cr|P}Xjy*0)d4YW(o+fuG8f#9Y!?B}5q=O%jr- z0%IX~HE1~qT{ky(HKnCuZTBb{8SZx$LUjf4-*vW7qkfN3al;W6rCA&~?LB>yoPPo<#)OMCNq~L?F0Xp1Tx5R5%lKDxz+l zWi3`xKl5O)#7NSX8WC8uuEv|vQCd87w|Ce(<7NJ;B4%_kr!ry~BW&&BzpzfAJk!Ak z{(9F;t&`-wn1bSCSn}tNPDeY;l;Wh z)Q3%pl|Gv-O=L11Le9a2r!TG{5%uunxuxcd0Kpj`5%@5B23@_dnLcK@BJJr{U9 zQis)K_u*RiR%mwi(FHPV4g@a%97+4#cJmItHd_LvxW;(d;~fZv3gwWA#gs8l!FhTn zr}3Jp3jHJtUHZG{eY2bM060~s(P2M8qXl=_a3P8mF+s~fg z{Me>T;-D5J@`a&Q2Wh%tf6GrEz!ts~3lpjv-6RIe`=M;nHp5m1lx-rZp;dqd!>j7R zbiu;^?ux2@cfvimOkc4nZSpX-wMWW5L8Blogf!zY3p#jbOas`ahwUqkG5>0H3H*O~ z;2$%;1vU0X#+ZN9Zymchu}#an{7-}60JdqLlfxFw8x#Ci)*9;i>=vLRFg(GlB^|%- z{XJ-8GTr*-K)24ur|90DXyKeJ|8VwA*4SB{CTz5NDFbgo1-6I1#MM&WtMuGP935cLi#gqZ5UWJ*sd7dsX^jN)`zV+GGb;i9OO5BafC^lv=)yRc#Bgb$t z+!rIXTmY!|%IUV5_w;gh8og!m0d~GN=4vN)IIXzNr?t?LJiKSf&DyLk;Bi0uyt?=` za(S@|>8ty8lIfAcpiwPkQZ+`hu!eN-S|ec(t{)qS6;NM$p__~b-gE{x-{&JiDTkZ!$HTDyFWE>Fodn9r6!@%{z`y^z8hRvKE+ivy$Gc{D~tI zdrxm~ukWib%Yh_Flz}7qEZ@9QWaOMny<|2J4PbQ%tGW1ujHTqLu2-||s01nwF9aq* z^noDQAdr4TAY1c}N1YR8E*YKTp^7Dy^+YmP!Zh!zF7$=D4qhQg&%RC)Oq`u9?+#ZL zDHTRba*Usc3f3>uC+E-Y>L!Z0;=A%Rr9&|#p`UhX$k-qsn5C)C5n74Xm!(dpDTZfI zt#9xYaMHj`%lyZJUPEW$9HsIJyG=;>|6hAY#?DUv8n)T$NM1S;dg zL^6xo7-ZIg*Ja$c-mA7pC6kkvASUYD;hbcFfAHXV&9h1w~qDfajh0@a15(zC%~QDB1SU!26eJEgSFJ@+R=qWXZ#}- zdm2&}@elm9QMbESjS?GAPbVB7;MM@GZp>;3>lZIV*qnP;PoRJn6IGR&TUqQFmpes~80q2%4EFd`$<^WAUc5oYDJaTW<#ry6rR*$-@4IfBLf6W}s z5(vvaa~r^v?D(HSt7rp955!@eZfK`;9`y$c4q34e>B%%YiI-(Ql z$%hRc_a@Je&B@iWHoLB$`26RpV_pw8%O8N>+vUxlVC3L9fq=y7!T#NQ6a*Rs{P)hY zb-rycabRaG+)dsx09D98sd+^}AMEc`YPt@9?O%c<^;QRf{wD{g3*h)4v4t@J9u5o~ z{kP8FDpP%Q0o;FzTXg}-e-bEqfY1LDB&p_l0M!4}H{d+XYrs5j{a5aAG`mcZB{#E#MASi)=7}?o6o7g)4SN-2h6=PJ! zTd0cz0r~x7_+Jh622*1V0X%=MW5N)C2d1M2NR5&KFs9-d0pNLlTiv(8bB%!o0ihHD z0fGHrjT?0UAWqI6)+SE>KSopajR1I{-sY)3MgZzR`u_;(%~J>d2`+!vI5kg2Fb2^6 zPYp_oR5fD&HSurT|20SS-wcSZMQXM&fZFVR+%RCx07#(zQZ zvAKXQ?vX60&Bx|Uwdp&%&}}k2{eK{F(3b&NDMX&AtgUh1a`&#PYGbXf%lvJr;AVvx z^rsK88Lyo_qd@1Ur<&Kk`o2Oo;I~*bx_Cs>HXb_VupcG0ShN_`{}*)drt#v)FD49R zwNHoeB#M<0h;!JLXl`$7@0kov|p6EqLqcepWW!7`zr?cL4o9{ zG)ilvgMU%3?L%1;4PX23Ik+?{%~TsHZV<9z!oyXoGofstUtKPDVdbz4%0?^%mF{73 zS&vZl7rfxE%`<#HsfjLX_>8OYuM=(f$qGF)vdw2maoT~9qnE*ypr8sZ0I4SsNw^82 z{w)D10wmIq8S*zoJd{Ml`uX`S3MbOh5TDJ%k0u;){{{FKJ7>KC6csm#}q z7QRZK-C>S@2_b0=|DuD_P@)M6H7($Wg!2MSSMq{UuwdUevH94LQMA(8$f4c35xft< zp(Au5+^_;vnT0{PWV<07x4?V=(TpaligRc+PZ?8}TDUQ3S5q-$osl3Ki1BDIGG_4B#^31Z6 zaC2o)pyYbwyDDY;>?H$yv+o=c(s$I&rO5@gXk?!QV&Nt@RL0u9P@oisB!dDk0LtVu z$|5Ip#PJXPcc-=a4If|J0L|%s7o`h4x&UqPlY=>UROU0tJUMZ|5GVe}+O|_>P2(e7 zPs4yF$)oh(XRKPO!9L}=pTW%psubttPJDTXLk!;>z$DUK=vt=p7KUM`$`XsPoeYzj z;?~%Twb9G(fxIDJa6oP@92l;gxY4fzyDt#Qg?LvLeOX)z=UXs(6nRYlaOAF71M-09 z7o3pAyQzTyl=_-CS5#~VC#*0sJc10lSqm*Zv0)i(-F++`SX#XB^`2xT9t`^-xBMc? zXbYbILaLDWxpwViFG9$Y*d!4UI-Q=ozL*Uy?H5dF89O;&j|2JkdYc~#SuSNmTsWBo zPVr&NVXnQ=8)zQAIC~&UUHI$2=>OQU6Ohs10ey=Bu91LJakD>^O88Y`gxzY6p7{$D z(XedkSV#@OZod!`PHI(!M)VXIV`G|lWrjZQKD1v`G2%GguRIt974OHCY~mhQ@UuW| z$|;?wuOHwei2&@gP*aRx7?F|^{b^z0bc()#@&1f1F4j926NiMGnBh%2b46YNqPIRu`#QhIy1dtxUVwW8vV5*!%@TTuq$y9 zqetDX(+^AQ?*WQv1xwXPXd0uwqcD3?ld2COiNORbXRYu~CetYZ@0fE*7(1f%+}M7< zwOOGw1?cN#Fu1lb7bPj`6tf#r7IK28So962ZycUj`?)RDI^EbJ*@pTCiS~GL2Pwtu z$-`9O7Y9aJu#tXNog7XzSEL*-gi7zb9%d#wtrONP=~Bj$0YQhy#|&gW{oN{(>5~md zNubV4sqlTP-pg;psA3Jss!3~RQ*ED0mUOy10VT+xEMrZhi0W@P)#S?s;iMA=L;c3i z4i#Wy?>}dLc+9_Re;1sk9-^x-hXx+fS8TFZc3&+lAQ%C^L_C;!Swf{jEMLL0-5M$0 z(3P2!_2Oc36PY<2^&vD}QXB)#c*j5V#>dVF>Rkw{R6d`I-n_iLs6CWH;d2jT*)GHs zz+c4P4`ee8D<;-*b~(0CMlc)u8=60*f`hwwia2KtYxE0qG`inBx;~>Aqan&5PzO>&SaQ+xHb#2K0#~LUmAL(MA!> z9p|u<`yBUl4>?bpk_sW-BqhqL`szg}@Wa}=?tQ~TrLW_5$Y*-VwWhVES~oGWFt&_8 z04a{bX>gA zXfKtEGn6yD&}7@a2qR(J?z2-Wn--i!c=baBy42J9(@V7+F?r`g>KX?-UiWk@{Y^B> zw#Uq+f6bNaV5oXs_^1M4c6xNhALj<`qqDR68-A&@$_KVKJKMUozPlNUcNW zm}SkZwdgSqQ990*)b?&%3-fDD%p8kz;ry+THgJ$P`|r<>~k;m)p&{=2l(pRGEJ7Ec4&M%Dg?^qW$+4AsJ+{6S=E zLs<#C$<%K!);JPy*210Pg?3Lr`)gmuJm%bdteG zh&UxD3A?+EJ54w{w65(=ap^0Liwo&a7lAPP2aZbO6;E`!0=J)Z_&d zcDX>JlQ)DgC!+Ad1j@v|NN%pBzA=j$1VV?^!4r2$ICLOEXM(*h7{T(F4SN11lbzHd zWHT_KO!EF}ZIqJpy=FPv2|pfl)U^|)h(6ZPmW~G7tPx6X)DAx&(9u+4PpA`kZufTv zC>!q1){!DFES8ARIu;){4GPCdsUMF>FNoKyb27c= zY#b6YNJYCI55XBwA_JG&61v+Jw`7o&svT$QBEe`s^Mxj~39|_az4@cn&hdQkhghRIUz3r>i)4_Ps4zDuw4r zpYG54d-_v%*7ry*eMY(}V`p0?@1x9>Z3Mmq;tIzd;l{trD?@UQ>=uux92uH`j7d|h zB{yL!heFInyYI_)>@)H*8pdXauUn<2H9lrNP`G~QX%beYc#7pEb6ZAE3XTG;v{d)} z3ztxg5zov8@cI`R>13HrCMcF#RuRuiv8S{u@-8szh!`-b9s2Gedtzr>KDI={rv&xi z-?ft~t7k;!)S5DTNd3Mh^kfH^GS?59XnrK16DEXZ9KH0<)o8Vy_g=t}L3S_u%rK?l zZfs_X`=9(bO8g$C#=;$k1^j;ARm?iCw`Lf0H}h~X;Aq^qy`-^1F7PH2M^~des)*N7 zI2f8=AkzsRd!|$vXFElg+dq9zWE&i1%@-f5C%#vB>_AC*yF0FzEG~fd9t6I_HQ|2L zYW%s)g-O=YNH2TM6M3N1$6qOre-FTQRhuY{Q$A10 zT6t`K?K-q3ST^L_c9}V} zkUPRd)J{VN-P~QDLQ=r#TalTm?=7*fRN66qET4`4o{%Zx{y`;HIgVoHvpUOI*!Z`kDd#fNr-QTOq8CGK`F@PhF>zbnWE|aQ=5rAoq=G zisCL#Ax*t%Z!o%gIac8=P7hYfgSfv}V)^Zo-X)I4ZDsh_ECE=LSSN#^+5D4v`qp%x z&Fu~27x8CL{^QyGFlKROA;1#S!R`OYGpl?w&yB}-Qg~!ld3XE$B_l|RSi9ApkF*un z`rd=JU&Suz*Z0-&Y0Fs=ROPg~L+KJ{|Bp2+I`irqjR3km_0lSdNh$Ly$hcYyeBnd` z#O-|{`h4)BMG$EF;HC(Z0Ff<}hOxtKb0P=%VR%rR^-UCaMH>+*{(7mEOQhADx;M<` zr5#i!(1l+jF02M|vgshUJHxsn80unh3%{W09HMj&eL>I%SH7punT;cIc%cq^RElYP z?3QEGtq#a5^&k}0nBd>JW8$|mN|(#1kN`1gu8`J@aDZfd*$Y7!y3lOstIeI3=cC1h zwb?^06;83d%27&(KM6G`Go+rMVNEnxo?<}HdOxXC^iiNhs+1~l`mui(GC#Ctzlkqa zEpV3rWg5CKRfr>F?YV_t_!4j4Iiy>a8`$Th zdAF#E3RDSrGE=;_C2W+S+h(rpPy+~h)Xxcmg+LlIJ`B0bCn7tfyiY!bDts#}&CN?G_AGi~*TGX&C9b^yC z7)ut~uxns_t2%3pM|j0_X#9fq7($k3F-fWc2DrDx$7uhwDYcWnT=(k|3s(I@)Sb40 zNeE!qewyUkUhKm8R#MyfB2O8!6!HW;v%|^lz1z81+Lx!9gm5rjH#s6B65186ElGB8 z2T1s-bm}Q;*475$+ZacQf)GR;i4fDEz>fBlqq$UV#Maep5l@6)Ms;ivo{jJTz;6@= z(BpjE!wL$4UWN^+=7tORVi$73bJ4nr+9TC`@uClOrRsXw^vG>C_*zUnUC06(=U#@@y>c)hL~Inyq@}h=^K!HUqiSiU=$j>my(67s<)Isv^Ob%+D`ea!FgaDNy@q znx+6!F(!s&*bEkFUTd=F+@@@&mq|1Li8b(gvdNgCe^K?}Tww1!TMj1Tj?H#soL zS!){ens0>Ip$gj2*xf9SwmfPMQao-Z)uDTaplXWLcQX~oj3zSWTw?fng|L2gU}2Rp zp!<|CP|~-Po4^D&Q`Q8h%p9#Q84$>Z&d4wE0GMlP&W=bdcXiP%Q_<5kcc=qNbNXje zlcCZ<4;o-zSFI`(2mY*jwdIZ_nufNxLDgsDGr_?#@Qq=JDF$ewlr3YZBP2c6p$~)u zuRIl(Ml6pf-Kd>JwCmnVrT+I!_E|%vD;&8FZKxpCkpg?DG+pmDpok8`_DVh{b1||% z@!c&>bp=QIx|+v?GX)Q&S0BLepIU9gF~0hcIuV?AmjqLq_ZNZ}>o3l3e{lsw3HSUS zfriG}){=~w{N*XU4=cm~Ewvh?)K*qg_8Xm3o7wifJnRzU*nPiy`cE3|1m_oW6$0f$ zo1wcFY8(iWe0ap(sUWsFc=eWauO6qx35A!TTFx?L0yZX9%c$73IU9I|sZLOjWo{Ij zj-T)wa#dJhbNF`>KyDE ziyl8I-kQ|kFW!ezOiY^_+sQWN+wYDUNsDif(a5ug2}h|n?m;02qWrqrOOyufEx>b8 zo=UG!px2L1<2^%MIN-3vl}dH6kRQ=LJm6Krs?%I$X(`-?y)$x&>8kF@L8PPNjVLL9 z(u%>*?tOq%1x&#mWxm7D7%FpyA;+D=fN~JZQX(t=m z`=3>PQ8m?xxmlJAFdQ=M7}PBjj41{9K9v*{m~Lz{^FF;l0=+rJQQxhX5!3d$sdVAd zJuz^Snb(^&__ucDIpVT26$qdPhv~JU?H)FxGM{5Q)WMj0SDTZ3yquwq2GKRelsgrB zz6Zr}?1*Vi&0IfK%BYHELGW2k)`YQDzXH-+2p*>4;)~F~Il10xA(dRbV~F*BZq^<~ z*Sky{&CM1R0kV3}Or5RDa`jjkDc9A<+!uzW<1UF~Qzu9PR?ptDo?~>168|WwNq&E? zLB}}yHpTKA>8yHb?UDRUMi%Ru$!1^S@^*qvuL=h1xL7-G70%)RbUgLS_6P)u!_^QR zFBgvSCgUtaJ&5PkI`_){v)eAuS5VH)?uRCtiH-#I0zf}eQZR;?#MC8zRkZ}&cbe%a zC)Lp~| zCMu`bz*P{Rw}@opM1?y$VZj?tiW$>NPLtA=VDe?+zNY5c_d*?7#xSGCsULscMGXT+ zH~bOz*XDj+d>WxXsPE&XmH2KZJVFp1Z+(-ym}qur66lf1DzKTj^T$FYkZEIwEbzmp zZTQ+~gEA(QgJ0`)Q15J3bOG3tSg+3xc{y<21?;9@AQqF=<9Lp zjz~^kMo#6^{<$I&oXgv(!Ch`+A5wA)Yj5|`v{fk;7=ab*_|VQ(@(7vd6qfbanX7Fm z&N2zTn2w7!1Z)zHNV**P{$M?P3C9$c=h=62H@gx$$XeylV0?5Gv?$|njN5;qso^aH zlB}u^mM=RvGTgB+ z)D9}Fu&S0oU}CJ_#gR?z?H*3sd-}=yp?=D2ptdzED=NG@z>8v(<{PP&9q!*r*wq$p z1h)sPC)FQ*ed#r7jWJdlPF4YT^g|D~SG{t=XxCxLs;d;>u}g#iMG=l`N5n4xOFW(k zry4o3GI5S`9CDF5+6M%$14 zoINVfogK|Ulk?b8GhCL%@%L)qPgLhh6xy2=FtUuT*%}Qt+ka~~a`_$8{nK{TSsy+Q zR1Avw75TQo<9Jo8ZPYLD$=(5IxCPw9Tg$XmcpkVDy2eMD^--hgVs!AnchP<=b5}ir z_jWrpE$nyn16MN05oVZI=&3~Onll+t(-R6c@0v-+wS1DNrX(LxGM@j5Q(YbHe`MV@ zT0}y%(N_9m?XdnIDOptZN@YK#->2362WE+)s#v!k!t;v|y$tik-Rex>o5^MLDhW4) zqst#N=k4W7((eklzMFGz`b$NWMIMqUocSvJma_K^B*gaSPX49x+r>XW0=@!UTw?L= zzbKHLPk1%QtP?z6=Yv|231Os^ugmlQRY1qZDRdJwlb5krgrQAFfssK!L?I7I3d;W8%z$q1pcHLGIw6!1c%COHJtRRj@_c*#H=WVmiK(@m zX%eYe3CmMYmNIY=%oSkKxWmj|U!(F~v)g{chh2LF>a9X&f@L#1D2n48IZm3LG{qBb z=oG<_Xp9;OaPDPb%ouwwi2T8&-}|=P1yk_0<7TsK&8jrZoX!SF^wnUObhW%Vjmr3Z z5ku!WD`cC`4Er8s6}U0jG+y-HYw-4`54zOR&-GHmrP6)Cq7Ub%y&rxmg{hw$y6wBO z(s%-ixtr|BYSE&#*)P|B#2TQ1z)Y)tp|j_?-RQZR1L+{`q(e`YkY5?)|lau}B3^PCimNf}9!z7Sb7%}-KqIRyJ zp=PHbb-wtjMM2QWtk4nNZJ&3#->aCvFW>&V|4+EhoR(^0ym29) z#4+bT6rItC6vu9pAeIUlX$~Qi#PD{JVmG2f^Z7Stg{wvweVBpT3vw@~tsR{dL^mPy4ZnOr6i|~ld(k9zKknv%tAueP_ zFp(kNU!``u6rvD}W`E_Oyr0S9by~7*k_8?QF72sDI4IXLb;8NE1ZnVdYG1JJX@$+^ z{c8jYYKh5M+yB9_q5Q3(D%;xlv+dbV@b?w#!75|(iqy0F=j#d!w{vq}c#}U}SA{H8 zV%aus-*B+k*P9Y2YX4Z_6A4arY0;%*|1dmFVZG60YMwW#ymSd)fdxjMLAkzf<0IH# zfQR;a(HiAKO+)Q(;>I80y^l9%W)Zsg#oC~;7bK<$AUHZk1z0HQE zB-z~N+maK4Np3R==^Cb%=-o7X646R+KG*Ci|DCh)2@(u-uuE91A{$%xkH8CS1xD$1 z#wwf%mf_-U(SlNvp=$1{W9!d`1r3I6>em8)mODZixU1O;Pi@$bby*Iv8fcp(rP3$> z5)$FO@w6@GpIRbbL&G$2*HvALYdQtFP(w1nAk*12gWHwe{VTLtMc7Xk5NS zBlf&&-yZt9&+q#Bv9js8-4a%-uZ}U0Ev&eJvaH#zE(XkXMx2G^qESu0PE*^+1%lD^ z(MnlXmW|@{`g6F+k@T@^ByvU=tQUHqEAgB41n}^1D^>v(Z!`3;Cw+Twf9NVXBr9On z7%E1b$=Hg)SxwytDBQM191~If()XPooelPydkb#7qv+qng#1HQ1d7-$m#uAN(LhDI zUCq4p?bxPzE9u`)0XT-5dBtGs$5vNn$#CY5?Q{8rf&lXtx-9`zOC#B`*s}^CcdVHD zS_5$3=9MAyEjQ$4=ooVgahGFaIndF<=|1fmqWE;4{kE1Xln^s%)0J%XZ0R1N8G#@y zPvkoufIfDr=4V)T5^d$i9VF4Y7aS=%S{)Oor&}9)XIYa2eQsrMgp<&tip3Gf;Ze>o zhS`X_r`3w5`?OWq|FVnHwC@J=cD^TxxVo5i*3x&J@ZU)C*_MbmB!@<7G@Ojv9z}t& zR(c)zz8hWd3g;lcVQC_M<#q?_zvtB9&E-lNU?y35kjVJAmuDe~ylx9yEFvsbaz?Q{ zDD${x^{f8}zC_=<3skXoDCGRJZUyuB7XXL3Lxk_Hhb7Wx6Q@GE=o_oC4g z8wc9$cwOQ<#KBEUYl(xymr9QIcV{n-+{1h<`;sF{50~%4&ZVuZS{k*wR52s2_^Z)= z8&r~8^H%$zCLezwAC{C|8iW4^X1Kb%3Np1k`928s*6V(82M6IRqyQM4^gIBL{#u@u)Sp&wO{>b@*h-KP`>}d$7^Q>Q~ z?Wmu8oc@ra0!_h1PA8nv5EwBQLbdyr_P_$w}q6o%E1 zKG!k&8K|EJ$q`;Kq6`BFRAoC?HpLlCHyo;|JUV+X=^2NSMgy!&t2vB3dJ|#`(FdT} zo`wX8qH&-QRgApwXVL3h=U`Pds@6^!4Vj&Q^qtpG1G=xSn16ymk)^Xf3H)!o@ZuD~ z{b?F)O|r8=ZPN}l{L)HFuPQ3S#pub#YtMVtAQM6fuEfT$-^1E zM5;fcNQ@+FPXHrhTKEW%$g@uUZretC(0S{PbGZi6lgE}oPGINmTDhHzeVeYqKjllS zmee|2O*!(|uFDBcr(qNHm6s?#EEsVqx?h<<%^5TgF%A^e!U`YF$m2Lqx;idf@~iue zx4^*1QFc~%YLY3vB4wq!!Z{cnO`O?8gf~x+3JY#s3n0OpM)a>G2x*pqB040_5nl_) z)S#^qJ4)KR2GuSiy22wQfHoly)lL_4SSwsH4PW68@}vV3NqL0z8`rGE`_v^oL3G69)V4Es)u^hP4=K;Sm|{#j zg!6|JcL;<^gLa+ass3TeI+?Lk4h?9oS15A<>@a)v3{Oh$bsX&PCN+8a&crng3_5jW zL5KW|S1&YSgg3~;J1(l&3F=9z)eP0gog9uCE~xlJ3O+OnKN^vMCsl#1$F`Kwml%(vJs z=T+ubH!CN4tT@6E!><%y2J&7e;xjPChp)b=-SwFe2rCwDk8pKuu>F#_aYYRl7Iq(; z``L}?4ZO)hQLBvZnoH37jtgoYH$?;X-OI8Q&eH57c*3+WlD=5>YnIL-<|PBie7!kd zJKj%l$yLt%vnMre8WbD;&wY$0q8e)kKyW zx6zI3%cRR@7FGMVUsAHAE9cDizcPtt$Ny%DFA#J)hT9<%E;@}({aEq-IPDwb{RAru zGNJ46t2>h$jg?-z`j`=oXzeWM_3`kToF??ZOFyczPqSMN8O$Ro=K%!xB|0t7wQ+jk zx;w!|ffs7e2a76n)xTF^CtK~cXy-{E93*rY#j)bA?>mn_PU}q$Y9h=sNy{?lJr4-pWPSiBFx{eBM_PsWrR!p)xz}UVqYM zyP=gd5VAe!;Pp|alMlee=iE{f6!H`Dx<@OpR@GwuKNB#6EXaQ!j;uAm&n=ucLicC; zOc!%yAHp>zI%)-lL25Aihh^FJ-y&%ffwN`KNsRQ3z_YC=G-3wJjC>I{IVr+VVOna}0kyb}P!4#DM$GdTF@Nk@-xu~PI7T+9j>JuuLlD2jv*lZLaL{hFa3Zp;ey+(FL zg--)L2(iXz`QN`>*^cEI8M|$w!0B6gg=d}^<+P>AD`2aZo|dWmI#~@rUjKb>H{rWm zSlE1?+6x7=RdNs0LF`-?c?BJdZFyQ5$|cCNM6E^b(Ug4nkFYLwhe& z^^k;nZegRclUB9f3D&e`YYz{m*MSRsa#DRMu+2Ia^9e%?c1-_MvB$SNyRR8pWFohY z^OOUu!Jk5Gekl)fs=VX3+8o4EhT9)Ne93c)PqP8OJ#9y97%R;uLv`l$*snw!dOI+n zAmPYw+7Sd?R(7JUP5DaWqiUKF{Uy2Wg@4909UMrwb_`T19US;;gHoMadA(9!Y?8aG zZTPX|q1+gJu+hY#qi00_0yfCD4ViU^cJ{2dc{~pNobh~Oo>Y$#-~h&0vJy?d5M;-z z#3#VRJ2iG$w*NIZ?dKaP-nS{Fs((2GvQo^k^wnZyM;*Pb#l{nbAFY#}JW#%=7r_cG z0J1Vzlt0>kHw1afrDSmblvLdx@4_#$P_y3!wt@MUZS%|(93;#n5nWta_?Q1Ma`@68 z=_<%nv{mTQ>fF&)5}gkgWTqjZUUu_?dL1Az@cb3WKxrN)pFgk4blR(_z44QyO(4(B zmdC^B{DWa?#y4>d37vFaib<=6k73nHbCChZvzdf_)GJq_$U&w!buA!Vm-^I~+Uj52 zXM*Xg*HF{CbL9jUr|sqARAK)S3yc|dTy_gNY?C52PJQN#&Qj;oElVwJj+dr#21THy zsikuhYj0%Q#)(GK@(*0UZo5o6Ez14GqcVj)Iv;HGpGFj8!wI3+2pu-ZRR7?Cwuqg@ z_$6)+5&Jizl}}aO;+>VB4rMd8RmMTexKt6M*C$ljMG{}o+at70)h^JW2hfmKoQ%+- zk2+RisP6I6d%XxW3`K!-yO|mHOc%)VeTmzFZVR{fy&sI+6?|AjPuDB@Z^nKmkxTDO z{bER_5G%HdAx2#_d~78s0<%iOb-hHMA?>%J)(sQYpCSql0iI$a!k>pVg|W4v8e{4HkqaMl z>NFJYlk0Gjy@z&>YgNa*S9~63=Rb6I+J3x&9vQ0t@Njh?pwn*8ds|c5rZN74>(+!s zPdcvPVzrHABBaUxoy&V7tOjmRWAMB$`W~j%@#q#FuR0xrE+x2DMEH4A6kD|nFUGc> zy7M>ITbVJ~@f5fl5pefV$%El-3Mnk(ee}6JkN>_|e^Y@v1CV1L)h}b3b(0VBU;~%T zpxeU&Sl-3nyFq_vVaQ=Uq#I8qmqW=&AHk3wue%OXbo~4V3ZS{}1KyWs17yS9hs;Oh zhxc3c%W7&?-R&aGmARPJxg@c5ATpXGqOa@i1T^VXJ!%1bvlw0xm8gGj=Z7-|WLW#I z{|%4mWMtsleYK5~jV+VVSq+XHAHEMTU^vm`j;;jDmSc`foY-%&jo~n${2ne_vfR$; zY9B!rclk6|`(>tV7HEvq`26#|H!#9{1@5}Fk#mD*r;AN=Wn)99>for!{b~C{dt=5Z zmybS_9m9qmn{lGp#j(u7o>GwI5k{)xn|p{u?H{XbQ~08u65p#F(E;C5;-pg=?{s^Ui!gy3 zc|WPh=!IJen%M*^Fhol5@DY}Gt)$l5U$B2I8NMF`LmixKG%sHHo7=M9I6JthKTcDSY$>KZy?t0@zzrVtDI|M+*-MNCu?K1Nv z1Id<9KX7-LkcnZW3Y%mXPTGu~JXcXzhU(a#3g_`)YtyMyKjy5S9#=L&d-I7!ZQFfc zN4+{3uz8E^x1H_0cyA5=k{+H(8O8;-eskwGGB8Zq12)%#znc*u4N?WG>f2Ca_+N*$ zo9K)nWkDmPE6aEKV12^Hb_`m|&&*r4BjsEax%N5|UtRWYd7R`AA|-Z_cXMw@$EwOpLf7VmwH}jj)z4wDcs6f~e10#>PzdjbZlibHQ zBnkZYfn@e0o3LR!q&QG2#^gB07v8aUD5LmR>W!d&U(nD-JsGn35+i4c+>kRqF|%C{fALTvkMGZV^G^?$T%WDy%^d_VL>dU5O8m zMDd-i+0gG(4^q+~c;O8-MUCMnU#SGCXI{EKu~BV4I5kPFT5GF`LqEawlEQ^9hlwYdP;(XFS=I@6<6yPN4>t(}c- zGp8aRi&GXmoUiR8em~uJd@(D!GPN8xfJXb1=Q!>8&>%9aEd7bjkZCEq{8f?jXLLiFh^!5JPt4&-K$Va2fy&fivN$!TN)W=jt3C`sdf z=-{TOA0^mu73lf6I`s%|@e8xh;Qe!Jl={)$ZQhqDH*C1df~zpr4#OiP#e~oU?h>ml zg2s#vt$A7(`1A-E0c-f-%!Q>7yo_T$Q#=6RR+ z$m*+jm}q}vh+&fH66iVh4GWWW?tGdVS3mdZlO6F~uPp$+5E^wEM2w5B(e-v$AGvT# zvx^KSc)|rrYfX+Y$wa$oo5q7pQ!Bqv2}|SeA9)O9uRn00%*D$O#W$K6lED)j5to>5>)_NkCy>sDrDkOG-*+g`aZE zZAlB<4rhU@&x9G6Or9G2zc{tjj8?1KZxB=$HCf900{&$m@y#qOEDVMr>1b>7!N~pk zyB2*$-I`ryM~GdtO1igiPQAnOVP1PZX~Ud2NtP=R=mpK6w%%<207N}0+@;ctzxK!V zj-{j=)h6?fe`3mu;$|SNY)Wl~B|@NlHeGopxbg-B{3~Yiq}nE>f|8uH7~9TX%Sg_K z=r47YsQ*-2AKywFt-e_Lll~>lKkg(1!1e+EnN(}ahvj4*&&9&F>nB(grEL#{!_g$d66 zi%kIlgsWc{ge9*!pkrfM`&iANA;&S&_l7)Z=nKzr$WzRgq>ziJENbXPRZ9N$eWQ+Y z>B0qgzY0ZP-#7)635ER{DS+}|XaRz}$A6PpMCDDKi{IIhYIJ!@!tlcix9M=^(W6d( zyU|(2%jszEnXAi#cF^_3RCDTm_|H;35i;6EGfjc3n(pF(9^tikao2XhGg?O zA)Y&8BLO=e9F`Bo=Ihrl*a{>0+uP%D$1_RFvBfyWAK?yX61jj< z_vLFCn&|y-5?OOfm;YJ*$T;Wz=k@&jd^n`XJfDl!64`<+@8>@6{{0ys;8_r!=){8q zgF78Bg9O&k-;X7i-}PuOJoYq=n+F+I7Nn}9YF4K$*m0Ha#?pP&)-4*iN_x-+k$COxqCW7GdZywdbRHnvv8H+odE4O@n)_^e!D#ZjcYn`xjBG=QlHeyS zMvA$iV)tXm5$$7T>xtHH-@;og>DJ1A9_H*0) zXgB)#1;@6l|3=lkx{@}_`(Ru||MS|B`Jha>c0R*;>D*#XyO>r`+^GwUznGplS7b$eVx6~U+Z@1^DI6F zXEpu2PdB)uo>a7kDDrC9W+JerH5fZh)7|9F1mNVFD^MOeKE^GH!3I35S;EVUloY4$ zJz8=aD-RndQ{9g6d^G;91su-hk&MwmFZ`PYkti4M(lAG!t`vlBhMGtMa_=nc_=E9PLrSY-ZljQ&nk~;ou z)S6n5n;kc7LZNNuKF3O2!w$Z3GMcB+58>OjT66<>#;@a`K29MY&+Y8*}s!~upF#iJ@%3=Ft#o>_9O zX>q-_gb+n1xM6d4>cND5x6aKLzmeM}zIHxd&%M@+7Z{O-=;p%U8q5GpEw-RE)n8nu zRDFm`XPvHj$Od0CxcT@sURcpq4P~+YgRm`2ITC_yIb7_dJb;?7uXT4 zKxz0F3NW(o5&&NRWYtQ$!J)NLeEFGHB^uKxo*`Bq&k8bRY`D6*H?N~Y0*8L%Kc%-P zn8kS5`JL%$@p%H>Jq9mfR0*6TUfD*|q>Ph&^b9#bpoA?R3lQE--B}u*Kxv1mK7=?p z9o`?5(qg{F(6v&NpvmEiP|n)`LQZ^6IF6~F#l=3-oq*P5 zYR7*(4Qg@jo{3LoQ3jrGPgloCorsW>q=18^Dvb=^5dHu3S;QCR`!+vwapl@w{c$>W z?rzTM$}b>V$z=>QL3eVsYLaQbD$j`E2G0(@cDK)m%oadPx6@VP{)jE0>bD=8aRO^s zaf^wBfDS@7@A>NZo&UZRK^zVWFXM!%e})cpnDI)1#W-k7#?|}b%R+h%7*Fz|#o=fT zgEjIjiqa)hOFWG)QSb>z`vx-A{A?*LS8E`jU7iM^Nqcz_^ zS^4=U$mKOexL&O^RAKutE+c-*Ab|`wQFZHdwMXuTIOwz1%Ne|>zh+PyVEH)s68=fFCz7gi_HOYIhC@YpB{FEZw6x2XLEUL& z%0V?FEtyI9f%w0Nn&|#e$He#aq~iUH3{=O?N!oQJlJQfl$Azo!9u_Bs6w8k8;?fbI zK47MYYXY@-)(BTeeNeO~B>%S$ABZT}ePDz3c(zfIL z-sObK1;?^9e}$O-=yO@)o}ZPxmy-xS!>ez$` zT7A%qzD^}eryTCHTx}^sDW-@HiMtp6*n{mP{DI}HAiF9WCu91pTbx*YkTMve)uEUi z5fLFrz4y7dKKc4ZWA%Q!uTR2?J*U0H_gLSUBTac&andW=Vw#z$&C1!u$)y!IE{Y=E zYZBz;ZLfxob*4-z$?7B}5;mkLV26$MzgG+!5C$pHf#BE{Zp6tA7N-f8&kjSQZo)VfZMZHXcNxi({ z9B&6AVgIhWUV~pAzP1SFsqtkUo*GQJFFNb<>)yvd@M$`0Fq^vU1u;NCqp2vx2E@_s#V+i_)i38%#KIpds};YqG*o5?fgH}3}Mk4 z&xbmpG68N@&$F$xc?%%57~D--J2iEnEWGOaEcbW#uBtsqNT9*5+%aTHDQd#PdVj;2 zHVpxVi`glChKx7ri6L9~JpuYxld0A)|9C(LfA`Zy4mz)J$I}kv#YE_Rk@mr-t8S6+ z=KQ3W_u|yX-eiW^K(q0PH9vQ}!u+D(brC3{zg$o^p4SK@Ndd~Hj>YFOPzY+_%Oq!1no)U>jD@oNN%XcORQFqspzl#CI5wik z%xMKx5RCBB$VHri1PgQfbClE<1#_cJO=I#9CD&|Tb2d!Y=?!86jWVA;+@oZ*&^j$G zNs~BITLsEcLX>bSMJk1##(;LbZ^;;CU>eGg#v!eMqfT`C^$gM{!MC)&`+|;QR-&B_ zxulldD%DkXcYbTj$XFxOb^M?6ht@&lj03AR@YlStZG_B zw>92Yf~)ovsVPIH%1}$A84Mxn=&uF)){c`ynLxoV8(j{ASTt~t^&$Ifj8Y1Faf1yT znHYwSA{*?kvF5!S962^2gX0k7B-8c>hrK86uGx^U!|$h_Uiwk(!nr)PqoKE{X>dOy zZk+2M7o(lk=xn$ncLGu4I$hIET@U#~bJv5WRF#xoqV~D`A3+l2JYVMNE-+ z2GDYz!fh*C{kj`mJClEFa&079$Zgd+;?Mz6bV3otsjI1>Ic>galhOT`_LVWnOrI;e z*Yt|qLDt!%O8q2xo3riIzZ$7GB>jfvQ@mppkE#Fm%nx<%bNvpq29{mBF9mIA#C!vm zt4R)bN$v2=#!Ae#xp4Z1yAb*ee-^S658(A@0py1=SCvd1E$^(u*Yznf8^WFGyFl2| zjg4|Mj*mwbDlc0dsvWpL=S!~RmPW2(rBbKAjKVm9cY>n|DP#TX(B8lQ;Y#Sp&gIag zId~$6sMund_4C|r0D8~POC(V$q^cSe`jh0m#32;H*sx%arP}QOW96=6<5;={fjhPn zGgHirF~`glGdqr%nVF#-Gt)R`W@ct)cFfEaGxO@^=DzPf?e5!uc7D=mq*7O%nse0Z z>XIryGcq!=F5t`$6QF2BeP+^udVGAGW5Y2OC!5cqnWX`MAG0HF<#Z_uI}nYuv(~g} zNtwMZwA)@+Q^H^`f!em>VLT8HOp@@AaF?r~XH`5E%osG0ReMpVH z4c5}$%ELsPRSJz`)6R8atMGbbf?49VC_4`pf~Q|56%M7d0J>jEDa-sjXx`;hK2K=K`$*Cg+~{YPy*d1x?Pm{Z*P%-M(mce!#K$m8*eOk^UF;FACI*#f}-c;LYQ^(2D- zZwJe0g z1~i)cq+`Tt zAT|5qPGtETTLagv<)G8D+k}+^6QGoD=wXqR{u1qO<8NBd8#<9n#P77HdQY)=&)KNz z_vWIJ-ojhfWqGlcnO7zY8MF(V~m_9%W2HDQ_uRONzTCwBaW$$ z+P|1g#&YA!EGd!R`@X+X2#wACd-QmP7Zsv$q4HaJM>))cZ0M8YgDdkR7@%eTV6)~^ z2KS8Q%|*D$OLcB;bwp@yv7vW>lc#`cr0~3(ei1aVKjsi)bLq%L%9b`&(TOjqS|weR z$`oJ2yzK_w2~D}&+c3frjIV8M_^Dbcu){g$<$<*RrCy!#L7!uOxxIP4(9ga^w`mmo ztIENa%w*l$_eUgoDJbYgbwKlA=^Yr@7f3L$zt0(ghKBy@oRR-l6YyYQU=Bu(j%L;- z4u93fuDXK578|nnS;e61<0Kz4QGzA}S|c%;W+EvJg6+wT%mQ3tOdWv);^w*vGsSMF z4{45U!I&^}RoIlSPBjP%IhF*lah;tBbpc-)@VPYW!A0QClR8tn3i^~P zU5Io8_QC;KE{Pg9ExZ$wK5pdY*0R~>J0BxXbDZi=bUa*P0O`qE*3Q?Q9!*!#$~>Hb zyCz*!s7*hye>L+Eq=Y_WU-zx#2dJeaD+a76D_9bKT&$7LByCQNFKeciYGms83grSA zGySaBRM~r8Be~n!`V6qD2gdF0G8;(o1;*67jkaH5Y_g2P8T4D1xtq;;FI(8u7o^l- zl9*^aU?7z+nri2btG!Yyyw3`W0bN&DRc)FLKf(n(Xk5FhUA1Ri{gB-Di3elu9n@I; z8YaG;tLZj&6}F`8hP$UV$(vYc`D+Bwf%SV6gCphfJABZ` zwxUP`{>uh8_(Oi!dMBm7gw%C8o-HcER3+Z}ErxrB^k#Z6_3ku&$ir$5p0gr=*Ci9R z&EKVhpT)psa+u>CxI&vhq3BAek$5NJ2T2M z=Dw_ep)h`PWK;_4N9c)teUoM>)oL_>+Cg#u0LCj$(`M~%SqCRf?%>WC)z4x&)Y_!t9@VrC+jEo2uDKV$xh`&}E_Zj_hs>;*) zL#LypU#E`=gvPh^a%rJW0tl$`gJIn8I?!_XKh-wScfmGRPC`O(oavU8c=@ z76?&@XfI?rj)^r0DLT_ueKy@pM;T)M&-Sr#7TD~c4xqkj{Mi23m=CKDq|#}_!EL|o zxNUY{xNZ8iNn4YmpgODTlr4OF>FBy!(1vcy2{`8K2_rtMTOwF0ob5eWC%jJ5K@R99 z9zd{_j+;k}C$;r_f||P+8{=wbR%i*_i^r|1zv564`1#QhD*F+s_dLc#+>eLNL{sw& z_E&Qn8%{NpxcmdID|D;=TVNdR9ToRMH)bHe!(&D3W z`S^vFObWB)i;Ca@2GCrtrB}zNw^~qri67pbH!M)CjUJm7;hCh81Je=Ev>8ck{eb|~ z(O6mDEwOKG*{xP+@^-2_g3_>z#R$%3Je0>ip?J3J9c*J*8xF#2frnG&2L9F?t+)Vd zqEmUwlG|$})jD6-xTVny7IG{3P5Pa|X^HFEVvFj`Z0%GzN(Of0;Z6mbCfeKLmmK{x zgEQ`sZZdWBrb97G-~hjv_QqJ&wm1`q?<7pe&x|jJZ-^-xllHD z?yxr=Q{o(wOkkW1ZieZb6Ou?jxTt3dT+)%*$eskMc-5SrkVRdzp-a4F*83z_)UgkC zADbo1CCFZI&_rkRm%~_K&{UDNv^d8S5Sl4&2lZPt8bc`pfvZ@)3!<g#7&Qe*1>Z_~ByfTIa!9kVA$1?bzX`?Jf5= zJ#u=U7XvD9%luJ*jSbYx&T@*~+aWRbSofjv=am%1{Pjq8 zq@n9&wYTL917`J=8>`zRn-`lr@|nd1z3t_fzQq=kp?$TPl#8X6wzjA-U{NQv?a6rh z>F2d^E2e*P+l%wc_1?sco-6l#=48_AX(bmro|}wK(O~h*|o4AU}a;D`Lp)8p|2YC56YCu#L0M zO#8(yM(Q;{sA8k?VtzS!-Eh8?TKEX+GT5G%t{7kACx}W=?!%q#IxYG1Q7PfjZtqCW zg5iw?do)&J@A7qN99w4p%G(aDOna+#znw!dEZ*k;z+T8ZBAa!KsmJS z%B;aQY2|?Gqk|%ay*fb8!raq?Kjh zUbyZU)jtkB5259N0)fgR^(e3&`xGxA&Sc$8Op z2jqukm|h3=4aoH)wJDs24s?|wdET!0dDwwj-5)kU$dh~SBb0mOn0+CCjau7weFQMe zrQq%~>X0VY3c7S+<+`38a*9j7HJ*|e@8JfX{nR%LGJRUsKfbUD3g=-s;~ncS ztiKh0OC?u+n$Xm;A$YhP*sDv$-Q6@eqrU=*cQwu^@-)sU+6HQ|?zaZkAYVvciKK0k z>CY`MRvzd!NliZw8M1-NcEWa^$@F)0A>>>@|CG>iwD_@dZ)j$JaNqjk#&M(&}26~_oX z|GHOl&452W6J-k|gucG2M;Hr}xCLR0tCY}Z9{0uT0&sqiM-Vy)kgg&qbjECYe`w@{^? z9q0HM$-Pm-i>JdPy2nKB6F$rlZyT6=k28aC?3pvd<|s3jQ+;NxImAHN3+g+@{WrHF z0*V*@pi{jcR1~B+QFT#m{w{ux3t`t)Iek?Lw#T|bOL<{*$=_}+)-~A&sV0IHgTH*8 z`Uwut*=~f^nhH@URL_YdB2f=Cp*SDEEz&yuDyZvDl}R6tO^0LPRl!l3R&H9f%xzp% zoMza6E{b+U%#;NmZS@TMupUps|1D!u!zXPG^83CNGhQVBi`L5P3n{dbqi$A1!oA?L zkTPmTz?su4Co$scF&)~ES5CTm%U}qOn8@L(l`JZqZWG6LuVk^Ra{CQHB-ZM%Wt(AB z)$H_Ro6-GQR)9rPF)IDM`3Ur})IEAK+#Qu;^qeD=Gh;UJaDOJT9UeN-S0z#;zDn$i)JNah$Ilp2_?uI63@yJ||r7P+kadwvhx+w^JVQ{esrCP$LOF zUb=pbY$YfX^TJc0{nBgyWIYqdklskHb%`2J6=1AQ#6n8yR7ya3fhu~hvSa64 zzc#l`raex@FF{mx!Eqrzuw3jo5T83Gm8=?|aY(ityxn+7lk5ysw1sR3jH!p7)ynx6 zvE?>#C{}yvd8E0kS_DM)wW`PPcb}3YZB?o6CyWR__G?alh-hQ@l>n90EWdmg@wV@? z#W8s8YiyTo2GIuC*mDm4@|k-$@<1DSH+?5ZoKFIF)(~Dam=iKOhZA^Q`E;^Dfw1oV zONL_Te9}G3@kb0mpHpx3BZ2Yb-GT4S-0FG2w&5ENu1a{3aMa+@sB%u^SCeHBg7y^l zV?#*W>{#+;PA|Z1A&Gn&e?nDaaus5_jM7S((_ZFjV5ID#q|g#;(|Th$#H&_I*LVDL z0CzPFaP_z-=A^*71}CsO#Qo;Me8@I*muuv_6|#iRp3(sXajs#@9FfBtCc~ejg5LF3 z$#N*5zwsmC)x_|r<9Wa!A#N^6Z6xyi^?M$ZQ=xt9u;ZwA)lmmZi9n-(Q~Y+stnAwy zUx+fkVfml}!`Hd*@<_~Q+r60777WH-nx6$CNj4U>3!w&UISQd0V#ccSweV(Qt?(VY z3`Vv%o&j|$eCV6Zng==7FfGwko21@yv*u3vnfD}S4H(xWW0)MD1TCsggz^~lLel#1 za;^Ly?E~krZ?rKanv7um`Fxn1Rihl$Yo693))Qc@B}6sK;yl1=jxxcQMcEATXGiwH z=BCW2bo|~3Dl8For!9FkHVXv~qKl64O;nuMl7L$?y*SZX;sQ?`#S0h3*u)iQr{@Iu z3p1F5tI)|LG@X>!V{uu|df6-cy#+I&=Az;1^ogW(W4Xu|ll(I~|IK`FqGDE3Ipfn~ z%eKmGT>9Hx_;KT~^Lo}5oxvfqxMju3g34MK*)dKZK5Mg!6MT2lu-e6X_Vn&*orZ=n zRABaY4p)a{dhVOJjGdj05ZA@ZNUFivdwYh9j0$au28IL=H(#kY8&dJm%afhXE$3xA z=%Lb;F{Q%o6G;3HO~>A(jcv7 zqX%#nuMOUZ{t?ds}JNoT`WMChHTI6FmP36+o9+KS^dtI#9wVD9EQ$VKP zEDcLGh@bFarTi?FB>A*z9^Y{4iPda=oQ^o2v8(Oe+Ke&+hN%aDL0h_O-d>8MFQ82Q zwS{*|u4&TJ+@KbxIRO3xnA$#^YNuvzgBF~|u>l^=Ey`{0F;v%u8;eN}N zHL>Y9m6Q>s#OJ!YTxuDk2KWZ6JAey3ES;=PDkE~64s^zuBk}{E%!*{JflOo^k#X$J zG3PQBKS9tt*z%#Q*C~1z;@e|2)luu0Jk@fQP$Q+|+k>YQVija585|btSNg3E{%yYC z)3amsQu8gXIj_ljraQCEK~_xJ!T=wUYTNp6P}DGnyiPP4ReCpwSAa4nsewI| zM9H8f_V=CVP8ifW&@s*TF@|?57|mWTRhXBCFqhMCis&N@?|-B%cYL52iRzsRfHKzB zJM4!9y}7s%BS7WstPemOsDUnK4)PUr#@wp&C4lgaI=gjqf@lF+p5;7T7nsl8yBm-N z^9?rMG-*6@Ii|m_-t;ALK?rn31=#S`*JRO37tVa&gW4*4>T%esDQ%!WZ%o>9-HR!@ zfxCBGKC=up`FLcE(Cbo-)GK_Jv%`o9V-?ZnY9PCOtnAzxr4+#Z2(Z?4x%&4I^=?ya zg0Ba*r-+mgC-Xn#T^f=PqWd8qB6og!t!MX5uPY>ooc1(x{hUbla_u?%G2n_C5zLJnU|%)+aCf&z+n~y^Q}b8DX^2hw5dd>|^Ad zN&Oi3G4LZ0_|f&mdRpSZzb*3j)Jjy4 z269pd4xLNOJ@qmM z2Uzs#D7P;n#A1@fEI}4QN!njXrbVqk9rBKCa;>cNBcLg#)GxHpYjRz)NL<9W)U+?K z?BthuT-IpzrDO6Itr#+q&Ik_zpPKL-Lt^owtu&<=dzi&I47Bm)?Ms_npP53#?1gds zJS<8Yk{PraLC<=m?_nju9?U^x`aA$@0G?}#TXuAtA~ec-8F3NF+GQuzZ&H>QN$S}&t67X80m-x*1&xMQJJ(KWH6@hw#e|mD$ z3(SJK|0ij^j8A$WlQZ5hnBeK$06chyHL2Fg*gGBk2&&}tx7kVPGIUWEp?GKo@p*Ki ztFsWH;6I#mk(1r!(1re}CJjKITyf7v{v#Tn+8<7#e+&H>81K!47|3~66j^({JugPeV(?m$g|hVN zUe(ke9)tO7J5FF0%#990kGwn6pDRHuhBDf4Js*?P<^Gh7FnkSRU{=V}f1hdNe{be| z`FC?7o5N$uv^dQu?Dd4+n!9W3&6_1bt69&_@S(+>BG9@a2~Gm|3>J7vS#sT&f3<<- zTM_;$ZCJL7GGBo6&Cy=XgcAssHU^3P|oW+v}S^?~B0~sSa z+LlyQ&G-fr7pD;PgeE1`at;^Xct{DSox6Q{He3f0m<@)q`7U&TtjhW_AwLfyXyV3& zf#Lj)9m{ZN(@kEG4MQzJI=RDs2%MqgGWn>4?^UCiPebKp7*ujpQ&z0QY=QJs|00>a zoTtHa)3xM|ZqXwm`4|j?E{R&>rlb$G2E%C`Ry)+D7Nhy+O6#Y_xKjVs=HmIKhWo`> zzU51k6vSvc)0$iW`t;iuMwR-9*~kU=*AfrpkuFoOFfKOTQPvKVXMU($q_xP`V!b2s zLfGZ7VY`4u^XGg*AuuX3#{AZqYqZK#{ znZy8Fto_%EhpVlhnxkQ#)nLlW+PPAGC*C-Ftv9jxJS<$jlvro(AQb<(_YET+M{%f(! zR*0EW30sL8=F`ef-qc^3e?p9ZxYqDLgT96Vu3eyB48lxP(ONsgOAMu+y@|yJ-MSpY zH2Hl^d`*1!4gB{LLO$8ROfaCc@-ettM<;$c_pCkl{HZG4xs2mFwC%ZbzDU9lx8b0} z8Eb_T7ppCoge1o)7>}RSX8+3MZ`uAl1c^fPNk+lHzHyz1HQqMEHe=;$0pvOQ7Y-~} zzEe|08@~sapd=h39BBkq2zORAGM&;_Ldhf*)kO+c{s|0vzPMP_=4i1QcAFacz97X` z>*eGQybH{to1-3(oEZgxj%>E{hQzD)Bs_$*i0Ghg`W}Qt9;Gp3p0T+_PAF-7V11w= z(qC;Zk2gpDHFiv}2waKgbnQp9Wbt}DiP@_ZIezKqO?E8DJlclI9!XGWa(hqKJoJOS zHdQ`8a;xF2%eOmB#fno-{cY|a?jY0GVGOOZtKc+JZHfYbHnz+uY{f&=-72V@ zFRiGgLpN;cv{VkyJ>Q#>MVer-Tdpb=aLGKBsy*7U)RNlynMG<-qO|sAV?Lx+pxx` z&yBEGm3JtoYV`d4Is-4)WMteRdvy3Fy^^pcr`?d4@b{tLjvc7y)=%XRPdEdPX@C*HcJ z8o`D$zAj|Pqe+L&0IBNtLqq~NEa5kAMybb|0*UGs$ior)ir5sf_LWA{Pzu&Hetws> zr2)fNC8BkxxTzgtrG{m@H?!o$J2*K$KEw`x_8~`G_T=UBk?UwjxS!oRuHZa~yl7Dp zO|dR$>5ISuS;PErOJ9=S8QkeDEYL4!+R3x-#cgx3i&!5v+G6x>;j^!pvt6)!m6R;I_?c9<(wXypG;Tk={N&wsNig%vCXIxybB=(poip2l3Y1?$_1MGYb5s&fZ+%biCg<8h4nU_EK(;Q2K%dto+_+1-TM1$!cGX)mpKy zq}~pr8c0-u44O4|Tz{}&T$zi0mYBM(wE9Z~!O3Hruo7+CZLRYBWL%q_ zympWzg$L>NY04&E*z_586ul-iYwKWI8sLr7sVZ%4-$FGuA){+azPXRQV0wOjUAc79 zrxD#pqtd=^sWh_jEL6?ahE>aX~oBSzN*qjzF^9T<5n@Xj5Qm}qmBICK9 zKdCCYD!D4Jmg=Q`^9RCrM92-v40#9*f0jj8eMeJH4u(k(79AQ4S|TN+ z$)d?RO3E%S`Z62oSr}YQKb});btd%kpcMgEHzo`;dW(6hU5=n0pdDn;91ynYd=s^| z@m~D=PUG?rbp>@L9dd;YeCFlJ(J1$Je})(BD>=aUSCCr4TE7SZ&w{!FBaR!o0Pr-1yKnz1yNlYc4*NX ztA;Cb`7;}1_nVt$kkJuopkLOX`3P#AM@k`D)>I2t*uQreA547zFcFOczMt_ZBSq2p z!B)1vOBqRBkFl9lgyN4i@EEt^92fsVesYkO4H4kP=yinPr(>;1jY5O=TLw7Id;uD} z(5|eY8m;5*ZSXUqEtM<#uL#j0|Bb}o?x8mtFUX?ULWX=*{1LzG*jEY1=m@Ne*4JSC zjDjW%&{HUFte%$CqpnKah}pOZ-n?)IXHW|Xx}okfCnW%HK?m$KZ5_ zcqvq0WEA!;dvnc5^Y4o!YLdsJzW3J=;ZXj;fE-mXvh4w)m~t&(q34|9_`~(PYc4U) zdJ0QOKX=}RvPgny>K~r(A|g#aZKeS-oU^$8p^uc9e`B~robTQ80PUI8+&h0_x_BJ> zdM*0!El601I`JDWjUOUt5%lP*oA4H=+S?u`iKe+B)7QKS@4=I7pshaXdRVRSK?t8r`lIQS;|K09b)x$Xm-~c}z!O^5=RdB-4^pFKq33Xt z-yCm5TU1e5(Ef1A=SV?Z!Lg~RAOrCubD{l~!NJ8Ii8yETa~d*F3nlMM+^Pz9B&?pw z6lOjHOx)k(P~zBq?Ov8%WI7vb$CWTc;RK|gY_T%qn9yMMA+eVFNJ2B?GUGC@{jYV2Tcc9I;D?QM2O zv0Q1PycC%BW$92$<<1$!#MZ$lr11X01~Z!9^N2jm#Hbd>$T>x;&$qrwdBtx1NC=V}+l7V!~eS zK+DIdwMY;4MFNkeV%eHvL9J&(ignMkH+u{Jht~qaQlqKuLaW($<>ZRX#;m5}1o(QH zWbX2hr%S(D6I;Thmll{YlltlDbJg_Tk`ZnhQr||RIv_8Uj{}(5w@hA&b5gJd_GxHj zUa74&aKtFWGGQUt#@Tflv|lRTs>WnXfsWQU0(Or!b5Eq_?wNXiftI#uoR?f>&^Ip} zbteO7)6cBvRo>e;FSJ-2pcsxK^2g=lvD#LC;!niK4@4GiCyM+pRb7{_B>Gh}xC{Zi zFIT!Wdqb!{M2GrkU)EcnSnoNp$w6z3u6Hc&4(I6)-<%oUA6H(IU!g;P#Fl0p0F6m; zhDo4mM8Y3;cjtwS9@qA2HShfpDPHuP{O0%jST3c1xcH4?2pQ2WDB9IGPj0nme%-8% z7n3*oF|p723Y%a!N^18LgI?TK*vUnMN#rLb8jCHC$hU+{crUx8gvppniZ`5C61Dez z(J z=0#Cyj#m@2DQP>D<^);t7cuw}7hEUnMfxuSN1#>uLSTjtiIP+IV|>bx6#Apf;0klWCJ_)9%jo7s)&} zK}Sl1%=w3}>qD*_($FB7KN>m>H&;jT^)8PWEg3zUCH~Xm(qo8HG|u?I(6--&??ww9 z0`psgh<=i_!#oV?cAzPd)N}|8h|z@r*mj)jq2}f%``lUc?nFVNQo2&Q(i64PlLxjs zbZZ=a=dLZKi{m~T9*u#p+u*ltw9f5qMNovChr`J`wHG|i4>tVI;)&9~dD13&as~yq zyyx4V|COQ>Wn+>7hVDBLdVK*qc`6vnE>??fanpp44_b4KmlpDz1fEw|!TMxu>>>Od zJUzG%D78ap!1y~(cm%|kIT>><|^UvkZ#EJw2EUO#H>(`}REow9?mnU5~~7 z^J1N*6u?G%j9i;l*l7eBzfYqMA^bcd;_9`BVP5CU^3MvW!M^@_6wkW)u53Hzczd&j z%9XhfEr=;mpf$s2>4|2opUJzuTsBxWX3g4u#%>7ryaxr2k7%cJuVy?C5y^!$S=$dq zT4*nf*io_bXRmky8Zv(C`#2`6Wv5%SZ_hRn6+j?i^W*%;U56&%uvVtik-TJlJo~LI zzyE9c&uTb~1Mvn;)(=_%0%qG1la1u$^-r@MZ0!WzB(9#1uAE+vA3gc8-$VsHsy&A; z$w%%c<84TJY-;H1Lsb{ClMF7YMt0d^%c`C$07UO5Y>-EcK)gV_!0lIoZ?g=?E!ajz zJAk<`A6yw{&4P-G`qgp4q8D@Wn3V?j-!S5hEZ~j{!s2x;{=P*- zG;Ai^t>*}D7hmw6!^thbcCO?IcFO$*z;^0%%=AXKhgaKhLA{*(_;qZ^@}BHXkJ~s87V| zlIdOQIl;YWI~-H~Ct3|~4M+UM@vd8jAxlEHSIC;bQ6}!E+{}eiSw~dzQ*NuBomw+C zlSY&O0gQ1WY{@8vHmv4{$9lq;{Y4JKOsDh9Cs?!g6Uqv@_(76X@;_wk;<0?<>sG#` zWyb$@+nf2#1)`2?YBiVfkiJ0XACU#<;G8f5dJSWH#+OB zuW&LSNPkDfi=XPHmhgR{wn-cxxD?oy+E4Z+aMPU=089NuVCi>vrsZ^Ifh8S{t+h z%x_LR&HW1<*Xd9$jV>H|w11Qyb3w9YNO+`4e`q8AGyh+DcDMG32rh7=eHDxBZ%Q`y zS1s+$dXLBTm^q zCYkj z8N9CSy?Hx!VHg!%b+V^kU0`d&&ZTrFNNSvf0D|M=A(1_i&y3E*e-A@;W=TR4?bGY@*3DMfZbd)14` zLbZV>$Ltm4lnWHU@p&v*m%3cxSzl3TpPk%T6CznQDdU>2@jpu^vjc)0zum)js}Xdo zi6NI?wP3f%5B)cyX$CeChOfd8G3wmpEjw*N#5Z=ioX`SN~)^>8%t;Ln2~Up zxMMUt@Un-$OQ*EP{Ep?_^$@tD%-SW2oAnsqbG;bkj~x&4O0r5Gm`ckJ0Z(^r$xRhb z4DIuzzM_{Is4S=~=}0W*Q?NKG_i|$^I+CwSml>#~sHNT*l{6N5b)VjflAfemZZcjq z_xY`^CJD>eW0c}ktNkDS9}7YsS!EZOVOpKZfp>-K;2SAeItekGe@mMSDPq*Eohn^1tos&{ycZeQa<4HXH1v=6b&?36J|FdpQ_Vd)yiw z#zf66_+c8Gd)NT}7s@l1z|kqWLI)!NArTxL5<6QLC-SIyaHE9xU0kqcjTXim>Cw*! znEM{0SE_WH(y8-VdSa1k&ejD?2h#f?--%&K**HSQ%nW0-FnO4PBw-?d)S0|;Y-+$Pf>2YMc$9o3< z#f9jNBOSZJ?0Pja8bQCtLjg(~3+RqQVj_RP_V7GVXM#O2{2E?*NMiq=GvUZaG#E9vop zI0aW;EdGZj6~+Zdnb)lv2}uU4elI6;S{#i~fhD{a-CjJ}EXpj(EIz}ml}P_1>!h=Y zD0Y!*#g~2#3U;)t-!Cf`v&~ajj4%%yNp$HA*)kB%Nu062vc}Bq7usYyLBvPbYMR3J z3Jr=6iVwQB4g#wCLQFSI>&EnPq3{M44%m#D;l+3)OS3XxD4`$OOSRnuSsdOS2MxP5 z1R%yCa=Lu?!{Ne`pYMxMAv~AYv-v}rV&LF&-2>w@{8iyu>eH(40hVXSPW5Kt zIm0K`tb$_G;5Z&Jx%guq-llPiDP&%h-<-td%6Oi+s`I;h&ZMn7@`4xfh`5Ys7A1P* zVer(E;p}6jUG0T3njtXlJF!XR*;oWPnJPnqA*?nSCe>-Rf<@59Gl?*IUqJ)nRNreA zKLA2tgndk}U+UrTkiK{82i4|m%l8U!pe_uB$}?j2<3xVO&qVhf=iYx;@VPqyW#5gtS#!?nX~j(`gJ6}cL15(Z*TFDY zb42bMr;?^ttNY!4$IIaj@Q|?K9Nbaw@Bz^HER(}mqwBEO7^-TfdZ7jCS1^G|1-96& zDAGd0ROV|8W5fMQ2n3dN9p`T}t4I}X&_6RBti}3$!26`G7Wfla(I!%@Z4r9l#>|$x z+b(WtrG01@6SN5_txkr00r7_89Wpf zx4xR!*UwYUc_>_g799v`J|lFs?~io=SFF~*oFy1~RmH zx69TJ)Qp?8e^UCj?=s(e_BXN4vjU6`Tl{{8SFT2hem-}mrmRtdZo}VpPMA1P;bU|< z-p(1`&^;5cA3Mm={w!e(y0^>x`Z*SJNyXc7qze}2Z2EH+4SC4qDgIbPjC(I-(2GAN z@p$th*AXA-LXen+{N$;}{0BdOS9dKXE^F ztmLAS{O2VYYRs3PQ<^U>jjz)!dNwTCrUY3`Sxi|M4>?ug!`y-lldesjDsftLXydtYKSBCge&NOorx9GJR!7#Zp?x$tbyCS! zp$a9ZSGUH#QHv`_Zj5sJ+Qi5;XRST8RP@VvOx8C=LQn+IY$EwooAPW!8eoGJ9B1pC zHjOxHKZ_gg+pvAHjaajE6*lP7*Tgf>gurITz!0sIsR?5BaK-YJIu1iwMqNf-CO}y} zot>~5fiAhijWe>s*)Q0s)ne}@{plFbww-d}|11Sr{}-`$_1jZaU5ivd=DKCMce1t6 z#N2X30TG&UHP*#n2@LJJWa+z z-xPUE2%s@r!t#nEw~39YHIL<3sLvvID9-`6HzWa1HhuZS!u+kQ+NX(O%c|zJCW`Cw zBy~c!Ah>4e){bB16dWgoUZM+5%*jG`t{K;+j}&Igxymu$oY2*0?F7p+Wa&5;M(Jn> zIq~pi6OnMB#Bpa)^FmZiJC(ZBZU?4s3bb##7tZ>uhE2Q)S8~2>M<)UX>?>;&9Psre zWjL{9fGvHeT8+;(d{$$=*Qe3S)?^~+e3*OPZMz*!rBZXocy+GUoR(A4GrukU@usmz zH`TT>*wXqf&fP>Id2mtl-hjF^k7b*ReRxVkhrc1kiduF}AoWA>N9aIncdG@5!72yy zo)XAmz0@m2u6-#<^t;@jK>6nwwnRo9^rCI+G=VhTlWoG`pP?)nzY-=`%mfzf4e}+t zQ4*~&mzv#Q<{N<5k&fsa@i|hpC_`R??>CJw->sA&Bd?(GT%tPDXjSZA9tX~E>PR3d ze&c%wY0Y!Y*fG?9St$(pEsd7#>5=Pxce?|^M*%%RK2bhV9=J!BZ!9996+sM02ud4s zQ^1$zj^l>}-0srz@lhesAkiQ*@FDjbG?6V*&sqd+J@8eZku`73ZwrZjk^jb<%^Ack zwmM(&mIl|zFfslqI?BYFGp9t*U$tiUas4B1Ijq4W##cE?1HlRQszTI_dTcm^Z#i_v zv>)sFjDRkd6qJSnXznmg!Nl(5RE{syMp~J|yH|9cioV zPeUW;BR}>TCB-9%`lxr1XSBCKe<8b8G^R3wdCy6`Pe$v|rd*oT(R8iFmWn)r3#|%^ zPz@3Ao!1}=p^SAx9gd&3UTAnu{h(^gme={kaRGSNRBc1#+NKNg_G?0vuvd;MGWc~;YUnHXEe-ABd7%%Xt^$ zw>(Ajo> zT!3FdOR={QCj}k!_DSiYeD1mUhX@|7^7A2;>mstmm)*A^k1cb{lMZ zA^ra_BEZQb`l~IZpF7viIQYLB;Cg#Fr|t>zBv{snBT5qgYYV1TfJYrQ;!Gh~WE+3l zl}TTaMfMK@wJ(2#mChWo|Iq3Ucii}`Qs&zvlk+l$qrBEMw~d|_P!~8J^kg{qBz$h& ze5J@WP}3fb9AiqH(%`26o_%=pG{l&c8~ohB|2)tCtZ}E2(c&CCbx|K1Q}#{!v5Gxu z6$+`XMS&Uj8bZD~klh1SX7s2^(k3nrek^ZkG;%Q1rB&=Ep`#Ot*K^LKh}9ygJE#HhT`#;NlG zAht+2A2Eq*z~Tt+8}Be0{)PkY`@^TT-VXXgurrDw$43?kpjj1A*l}SF?8&*xxe(uB zBio80=EsSFi-C(7nLibSFde01g?u0XY>F>AlU7N1a$uSY^Y6qn{*(B`SyYGrGzbxh zS_c0ql`^}$eD5-%6pU|#Lb;+{>4mjK?C)SAGbh_09q7ru zPG%=3Ubo+@yqj8GZ}LH&cYQu;1O#a^I-@RFF~;%X?#xF+g_Jq3$81MZ*h@YU7e#7W zVzko7qeT)|s=UBepQDSu-5(eBzHC*l1>Izr>l*dkRUCh~a`LCBwkqkNhMmzZrm3d> z(VChrL%UYQ7}9t5<5-sLAKK&N6sTuk6APrb5L}dkzqIMH7vNq#;G8b!;SSdy26nW2 zzh!}PJKuvbAVm?&K2TGt<9hha2sBln5uH+19ivH0v=MZ zWPdg`DKQ!Lt5CvRXr%QoyvX09hM@hCaJ3NJ%Z;%|4RW2y6;NYXppf`3v{(iLS6AKxqC<+3y`$cPq*3%1e<0osiX&9?mNc^rjr%>~@b3{RqyIIs~7>Sxc_c?BQfrXkM(1N{+`;n1jaN#LS^slr; zMEsMM|2^jad&p12Fl4BdfBY44hzrfA62%2i%H2Q?%JZ$OqQs^K96KFuxGu|UyyxR7 z34o6BnW)4^Ouh8--Gd7jvpPQT-JpHvasqsuUasq1gY9R`(UOIf(dmY6e_m_*U6&rp zUYHF7)51JPn{V@Q3DjfSbTfpI_l>baHDCe5>8u8KS+l>AK6jn6vS}14l#ZhFHm?cvNeU(vU zm6Gra{5tkmxfJjHRW1;qeP}fg2Ep%MP`=n3Q z^lh|~(V+QZ zO2OGv-9>eK<@%hp$s{PQOLLyC+s@4qYV=!uE=O1~d8-P}o*RE%GxnT`OM6HG8TaEJ zVNaj&oWTb->Zu!dZtW>M3<7~khVsB!n)?zDWLj-{B>CoqFb-W|1sUl*({qkF0%m(S zt|h$sXZIUv$cUUL%2%||F>@h=^O2PP_roQ+PVk21yty>@uj|S@M|jGl7(v%MRZ&DJ zkSAIoACM1trwY2;Cno%b3r;X@a(Z6w$F{h1yvW?b?)W6!@^v6*WQfh@NYQGHYEWkK zn<13s+HV?OHl~rbw$Q+xeO{D{>mM6f8qkvKXig(MD%I4vpZ?GKJoDT7{8F?c^Qk@0 zW&h7+3riPD38q{bg9D*4%AE}dh+}>VTsjp*GclIqv}`7}&WNuDPO-l@nKoZW!9r|a z(gl^PNc5n2Y<+FmY104(LK>_^+LaUcIOa~+MetroC>}Oo^I=bl#`_f!pf}H@fcHjF zUz$rdKd2mSouj#>NDB3&cIwf_B~wd6Bq4MW_EKy(K8?jeG1Mh>ycpDa)~%aBrl@ zn+ij@&mdVXALUS{FW^pU*gDFqh#D`W`hPJ(ITEm?A^Y8g!|M$y*_O%s^M zQ4`Y!U5Cjq6#TK?$N*ES6?o|SS7eauiME?JfV1R|xI=1bi>t4hR!YajHHZ}OT5kWO zWI@<2OrXQp;k6L7lnJy98FjeJnxnky&`ofbXWaR9(rTKrbgV$tbHf~S(lIM?)Lb@i zyWX_O)!B<*PY%Lh*6t|loO@#00o5$i162WT>KPZooR6&&gIvP{cYjq)_+s5tg(Yj?Lc!@%g#+osz>T2mbh2BzN`cCYhHCFL ze@lXbDw1AC97&AHxgp&Lzs-S{m-sQu0#qC19Ey0EanW| z4C)uxI`k7FnQRL$`O6e;d(H59JyNkWkJ2l*hQ!`+Df-S)t=pKAN*C2pBD=01A|RI> z3$Ty5M|e3K@BOG`NbH*5;Ck(|JB?ft7?eu+mR2xPOCrlm*%Gx-ysH-b2dxVE=LTlQ zXW3ew8mQdQ_wj1{xf$6i+>pqsVRj;dI_QiwU6~5G3b_j3!3y6t)zkDETA=MCN)&%&$0RAvTMYam*eL6uGv^q{oKIwv&;)dk%_d3E2BcVRRbD z8%u-C-&4)bX-?^lg^9=Ae0unk2-Vx_oVwo3!9S+9>#KZ*MBQbQQs@Le3C>9_>Emv_s|{jS?CUJT(xCdU+kgvK$@fN-O9^6q>+4nB@ob1xKCty;P);yzMRe4BGQ< zulD<$&leES3`QGmsR!JJ79pWw*#f>sSu$-IG9JdE!i3&&YWmpu*xwE{R|!cy+>a&M zuzkG}70N0V_2xxt6^Tq5pH-Ac#Mhy#9peMO5NLVnO{gUACK5wl^I~X`t4EIS?Ap_d zgoM8t540ff>kdC`wS0-Y>EJ$J#n{il<)oUXorcg(zw&<4G=f(4ehQZ}^$`DOXq!MH zxPY%4TyP|?a>#R@2ybdiE%R6gHl{Ie#++rD6`B3e%$gZem)7i;YVvZq89E^<#pLb1 z!u{tKghF9%GM95&N}(ZmZ1^qt0|%g3DmcYWr)e=PexwAiG@Z9&(FpSWM)D9jq zlI(MjY)R@Nbt7xEJax}~;ov`MCVwsU^fgxYtWNR zqtH>kQqzp}X0K>|Ptc=$bMMA|iVX}^zQ>*01@@7?b4v(bAJS9fd#AtbImWD8yvRn) z?cL55morM2)r0k4645Xx%)%py(E_@uH;()WZWm=?BF-loBPa|I&OjrOqD-<7{3i+n z1YvwCqADK>E}B*$VtG7qFBb|__I6m7Fxy79;U>0Xru_JMiRegJv~3(QOK1Bnk=nnF zkjx=7eAjmmB2$ptb|r%+nmc2Wp@ zC&|Lh_WIfZlJiPmiir+nyR*J$fRXKm1h%qoJA(D(qFy}p>tm4^dTWn27$TNzKd-bB$Kn|N-%_B zO^2_0u3UP*$fR5jXB5K6R0t#5j(dvSGKe|YX#mW@yVD-IjW2?blzI0*0ZQ|D$gioR za4B}y3=l!adfz_ngj1L}>puNz;LIB0JxVdUphJ(fx_e^#(~VAu{0*F#V@!WAvNE>) z@!dz$f?ff-whcX6+j>m`~Lc?WWXHu2J_EOT^3Y(5R&vJCr z`Y~;ZMO7ka4c}rwm}DcH4_Q8yt<9r*dMzdrM8T-8nvjJ(jN$7i!VLGTn}FruxZ8rP zC;6$yAgO1R8gzdu^@FmSax9C?W}w3~8<)x|be%1*t&uL0w3#$Yw=`=%t0B=d-NuAt zgsiUULbZ0YFUEwc#NlDbHjl<&*O{UD;|8nHC1%Zt1hsyJ>{9Rah_7kqJE5TG!VUGc zQS9^}JUM3%1jbz9;s8Q5;>zJP7^zpIRK96KNSJ( z(8k<>L(8I6%89jbfzTd&pAI)gIghQ_H_h1!?g_u4S@@2k11?O%0*=OH^`d@ZM$SIv ze03syY_URs1dog+7m0zgN&+AW&lki(p84!dS0F3K;wa%iC5WMMg~BzHVr!)hr1ju+ zGgxdLIn!{160(n@G z);8t5>%i6`Ay*&;_dHk^54PSz+-3~NKibCtzx!f|CVh<#|EB$~Hkgto50`KdbO4Q9 zH9m{f3oDnS7jMWElmMk?iwqwvAY9zq3*Vnxq;=LJrJ>hYqF+j7jp2kc=pBE<7CVJ% zM$iI@?DImzFAdm@2i3z;nz3s@cKf`F`Y5}V(#RqUy#ZvVzv0-41BJkFHd15q!z9h7 zp9(l*`-E@}hNPW$?Z;X<)7jw;vl4cT(Xf&Em}}BwBxHi5o0B3?e*!|;NzLu-1rf9` zn_sFhm~A(^&_aQcpo(^KKPnwGH+*ULsPO0XFB${c>Opwff7K&i6}7U~cx$C6Ak6?TY>+vcGN=ocOGxSdb_;U% zy`mq<$*Oqzh#y{Fs2FMnaLuSvQhG>tR?E4t**$&WBnDjgGiY1jUh7zB+j(%1RnDPS zM=8Ot<*&FIcFn{Slr0BP;A2FpgQ=E8hA%O-GyQ2-2!w}%X-tzNptV=Lg$f7hiPbz5 zVCSRv^T-77Ym(z6b8(UA;m(vZ{e}pJ>XDwT8wtbDAdX0fi?dn5jF7X2avVuvp7Vm^ zD0iXg3SrUKX@&!XyYarK6wxF+K!1nF^n>@r+!F|gPNdC}q3cH$O$=oj#>gPvRBf@F z_ov*^>_nmOuDq||;rJ78RZFh|Xif&MY|8@KEyFA5VB6W**@B=hD%7|5b|7JMS%0A1 zLwHPzS-Uz%PNBgb1lWzdu+U~3fhMvrqZ=y~MuMTz{Yh0~1lp+munnU{(dJx)TTA*- z7trp>(0uu-W)gD|zI_?PvVx{@Mb&{I3Ocd!XqP^sX1mg6!PY{@ zPq~8H?22QV@yym1Ottah&X90iE>(L1=g3(tC@G!y0Gy1%QGFOF?dmVM?y?KR=K3@I zGrW(Wdn`a45R*X|=ME*nFrN#+h7-bUN+RP$NE-RIv#LvbQgHeza+Ip*_MNVK_q_yX z^YjgbxSZA2;Y@hW#H5 zAJ1Jr+LnS)&KD>ux-7B$ddXyB4ERa9zsX;A%%kVU*nBvF|2?g;Jw?#mI*+-XXG3qK zch00BpUXOnbX)ZMmjtA{eFBQCuV|s7;BlVA16S`Ho@-;G9JfyFU_ElO6HRv*Yqaa} z<5c|z!(bK4om{7xPQeV`qR<&e_lP2kNX0YJ&H>NQ?0RIFkpdDUm|X~FM{R5?UbC7F zCCMAM@Dov*?Z4ucm-Xa}zB=P~elIFB#P1I>x>yp;n7ium+CYS$hw%&tcPBe8Xp0q1 zcJ^eAIn$X(p>LEys0%8QZZ)`QDwJa{m*xedV>>(EJF zrV5V!y+(Il(VQ4$+!>xeF9NQMAQ1|Ebs!Qho!XQ=`P1!i@y+pwfk5jgvMOJf3^e;m zxcbpl?@u`EmluzB@D)N62@dDsqA8sEluzx7GWa7H6RS;jA6E*B>jy z^d7(6$ogy+)CQxO2sR-cSQ*g|h+>*y1)|+@-s#eQBmsX`j8Kb`ixORBzUk->D2y^( zf8j8*=5_}LW~PC?g999{hBIQZRUB*BkEN|Aoy4xl0LPxQC^-~^n?H#SSU z1V<7nBZRfg4)-_AXnuZ`9=L!?pmPWAoFSfz52!>#D*u#VOmhAhc_(lrPj9|8n~0V{+|3m_SpX`5?3s!0h@fJWSH-!kr%qsoeAOGJR-@O`wO-? zp~O`glOAjoIzHB=BXvnPFDE|w@t+IL2;$<{3BJ&oq&|ND*59Z;bP>yUxc%S*3M~@~9-dY& zLQHmC3N}>r?Z-mu-$sXj;n#ffq=J;d-N}gp@i~2H`NAU%g1V zNKDc4HV$)!z5MM2elt??eL5sgPW(!D^}epJ4&qhC+I?xQs;NszVogKoBpP2hr)8$H z&>OYP+#xmoLzTC(Kx)(6f?j{)eTI~@!x1i%G;$N+cYMP7OaX9T13;Z{66Sp`9(saJ z^eU8=TtmsuiKB_h7Gc^kF5yfcfLYun=qz*5&QZRC*fTL#~A@{=#9E?61t>&IJHP_ zYo=of&B&J1?I$wh)<&xL9lqPZ8wE2WUjicJ!3V7wGh?l4au}&4Rh}E}0#Aad0bl=n zg?~T?nbC9r@w_gB;z1L;wKx&F6cvmg%=hGTG=uniRw35oJ9ptyU8SjZnl%%Tz!X>~ zSk4KYiOm&=G1M4>F_anCflKMpWrxBe)C`{7a<1Chx(;QWs5z`dNs}g2>l-e{;am>?rLCUvjT2>+rX^V81U8D%lL5 zN~eh#H;q?#AFX}fP|h2^Z*QYlsf4)X*LF$4KD0zib?&j?hiek1#Qo9hEWoA-#X5NV&kID0bFa)MH9;>LkZj;=p~LU!yAiv-*{e z)$;sD4iMM3Qtp$h{N3k!_V zdpcY#U8beZM}Os665XhFD)bw9*VwzK17><2$}VC4u-+4~98_tq!?^R9AF?Bj4RS*e z*Rhwd^H)Zq&Ua_wrLVo!V17B4L;w0l+}0J(xiZ-pbM2r#FE2?S!ti-{@^mg|a&Fvr ze5>QCb6Y*RP8E%pMfHHjt$Q@pvOc}KL&@c5`PA3?4WucZ#Vbz248NKcUNA-5-|?jC z+>cc-m2KFiD{3EIo5(woqeF~CJ#U0*w(;!iE~$HG3!p*kJ)|!RRko{oCS?kPhg8R^kX279YIGBmz0R4gTA^~0)pJi)W z8*ju9a>8?YtyPEZdssDF;?_Mmv}%Ime6)tabs~`f*cz>k4fk-yX_-CO9&2e{%0t0u z|CBzW=uSpEQy0VShJneme=MuN7fa|I)Hj!@0sF z*mqFPPUsCGok|{aIIp1%!DPW=!D1;uWm$G&Fz#JgJkW)R;)0t@Z+x2c*wBIFlB?l$ z27|fZJ(_$xE>Apq3xcMR0D)KrFAvYp~O=CSb1&cC1XtO+*>qY_!Qpfbw$%mSKH&c9`x;Q?p)iyaB^^-es}D$XWKBbekrdh`1{FfWj%*?(HoyI}Rd3_bQYx z<^~1$>)LD!+SglbNf+i7lM z!d3u8*^c0uFAs%uT9`g>=9Rx;Gk=H5UMgi96z5;kmnRuh+}Nv_%U>!F=3lU%ekLjD zRcAhHr@Vnw!~6?hT4w zYw&NBIo=-MfPqx;?9cdf*q?j|sv8c|nIL0TIWDTG>g4G$HLRF-~6H)1KSIz<{vLF4ko+vu`Q^h zXro>p$VGY`V@|;9scJFiyy!3W%8>4yXqoz7V$bFM_R@DsZv>hLZbLJ}~n@G|rzX3s}dok@uW>F(*lhZNctXB|WQ|S$I@&NZ) z$}Jzt33xOkJ!$XBrBmyo8rNo+|9$9QOs>)IZOdi!8BAu11VZGGx%4A|ntvI}nFoS0 z6?O-L0*mG!RkppR_&T*0^-Jgseqld5wdDuZq&cKeGvc!jRaa@DK zN*<#F@|Omx1q`$&&^ zYh|}8Y~19K*a8`8|Lu};=b^iD+VAn>&ciUqP<6%T={u63J5lKw;fKz5NXo1m?anLP z5sf6Z{!bG!)~U}TPhRh-n!;LRsedHc3w}PEUTbBvr`75(6b1JE_(`YsAzMB{S0Y=y z<`2@!ZGxF%R`Ow%s~hIZk^;h?;{CkI4e^#Qe#Hh%a)wQiAYu&aH}-unEml3WY2>e^UNZ)2-y#=pdS$Vp~>`PJv`kNeYT+q8bso&L{s1`(Z$(<6)X`qcN0u@X$Km+GK z;V#8ANIv@E4Kz_BAFD{GV{zMtYeaUP+r8Bw!=!<7lxj`;F2%g2R?8w^Z)cKd=jZC{ z3Hy^Nxxqi49=z4X2L13Pzi+Ly3K~C)X>rpNmD>B?j7x`W8tMtve62DfBiGjdY!Pa) za-emC!8)K-C|fp<;`yWYl`%TNpnpz zLbWEAWWH8)_hcEaW<#cv>||`5*BBxH ziwiR6{9!UteHR`cLKR#s zM=ai0NI4x63uV$7X@yYBG3oUu*w-|#yge6wtGUi_`wKfyopH3K7b+etgySmdq{^J`AW{l9;}hq+euu~fHGiU;19nL6J*gv zG^Y`^vW!@~vj5&%@0)zvS|^F))cnzklJ3t?4dz*{%Hr`z`emC}&Z+xr);Xbx8(&?V zKV__%<|cZ9R;?gGDd2nDr!Oe!SCr|)s=<^VMxm5K)UQB~5(Az(oI%TUqlpeM7lyzQLtqJPNa&~R;Nou+>72oOx(~XFl z=3T$sFjlOZw#LJXD-+w`8q+@qdv^ZNrn4V1{j#Oej3h}c3n8K-(jm}k4<8CfHK)@l6t~z4<>dQT*e6bp zMs3BAN*VU3Q{hu_@eUOlDGmOj8nFhUL*)V;Mmf?=FPz31H*geYjR5R?r(5n`xb}Yh z9m$WfN~2+)oC8eNNzKL%$wA57)9rK%zxJpa5gBepIUExqfpqDSp52ex(zb+^HufV` zWe&sAv-~_)_sGY-_Hju?qj<9Ia_w^MzGdxty-WH~1~6`oUx)WPtXtx9r@m(b$gMg! zld#z{Mdn85ndPui$uYmADfXz(kl9|G7d<%znF{-}uAd(hMx zq=>aiL8c8We287`B#>StDy6)lVmSx7iPmG#Z6ab@XXRv%!#e;vCdcWcXth zLhptcmI0MJ%BhlXxcjFz(||We?^S`(MUk38fo7d9exr zh8}|uWmEEKN2Q;&&nMoW%KfO4F|Lt@f_K{Qj$5NFKW^?k79KIgx@B)!5+O%+BYplG zUsTVQCr1aq!5*Tydq<5~*~UGdcQ9M|Wa*S9jDQ8Zi({+`trN~kDh4NxfHr$81Wqy& z=X%-=#Ma=&J)9N)KZj84iN5`)!K#h+=RtsOifKo;y1zWvVeya&Z5MD0tUHiZIs_1u zUguxxKV)^#>(n#1sF!BxnAx*JKYQ298qT5jlL;{(Jc*D5NTv}s6CMDSh?WFupLuQYwLb#&&S9StI)xEZ>#2{ zpN2reP%gDg0|h|}MC0xMxRj0n_e7Hx-;0p0i|NeK`W4?I%yZh+4ABP@k)*ni;*;-p zUcW(;ur$cfE%NgLx(`r^M<=r8H~`)boG{ftjrbz9f06|H1s2HvlVZYwVwECh_{HMj zkbykIXLM@G<=A_Ena01EKFX2)=?-sps?K#{+zff1%{jj-S* z!5}~#hD#lw)gJO!u{=;IVMj|Uj3|sKJkTk$j6Y`3&kn|UKvwq;6)7>@O^rs$n4}}M zx`7bvGo=pB4Z@36ReW3N5WvD?F9Sa=Rd|JdfV9&h^?>ZJas5TK44it))7CogjVxdf zC%uUFz3c@IPYvmS5+j5IVK@d{Z5v|H>;&hMwvx6ZQnn*uzob@dcpwZ3*sv|2+rY7RMcTQa>}DW^hh z7tzy;)A`TrdA@YECvCb-EACCQN@7iZCC!tCM#dPk0>7{Z1%8&&-k|uKZ&T;;)Hb2DiaFq}hqiOeYG24OE6_!RSog>wR5zHB=-vD7e6{5IN z`}xwTr51;Su3^X)h|snaQj}~f#kVLXYS!Tsrr=MzszvhC*jqb0l~ z;By#_1df)*jauMGE`VpSA=qn%!e~zg!=$A0%XiB;XPVjt4OzyMoH5r3HG=Q#)GgF< zVWml$MFv&=niwBpOMx~@w2Ed$yG)mcXBQLggfa}3?arHg{grcp5WaUUvDT%rXJJUX zsO0Y$*p(`al~YB1Kb<*^ZD4GBF7A&kp%!H~e8E*B8mlde=~Qfi|FiJ@vYkOvK4uGb z6vp*2Z0)(xvZ=|_&#n#PsY+aK;LHjc`Bbb0FJEC{5OLqBR`tas9%UA3DTo_9MLy73 z(fa;VxHXb4li#4M4@5^S1D=I+oO?n)Tc+oXOSlq91cxA?N2gg3Ob~Tv(=VTArq`T2 zL%^LVd)i>2rb0x&yTIaj0Bb#bDsQ>iu-6nY^ZKfCd^9CrC6==-jwOCqw^Ztab17XgK8|tcw|YDe~|M+ z7>+dJ*VM-%Pe(XsXky?G&o7xp;<7)J+2%P)E{ zB1BTqI_I>j(`XUf+UMty)j=M#9pZ6;=O`l0XA2QH(05lyC$Tj5UY_^CdWE69RXCN(;ce?5zr}t05osjM?g0h}WfQF%fqZ zVm*15IL?c)9=hCgvo#8K?xbj&Sq?wDw@JP4Fj1wdnm8j1$K7lA^3!l_?FBx?qGx4XGnvFKWK{2uwH7Q`r(l!zY-NaWD!V3N2M6)4q*;Y^H4 zj3u?f!rt&HiT__NuM#?|F&|A@~gzp-Wtpe2mPQm@zABR0YR8_4wQ|>#BXttBIt(s03fpI>lh~lW6 zz1KiXDO489lg=+av4Dxq8#N1mKfR&@QPz|*7{nExEn&HL^uVU2_anlcwsGBVKItdc z^9?!Ih8cyr3i9tIC!_WiB**#-`hxQ@A)Lvz52I-zg%+KlS%l6KL%u{4vk=Cb6}$Ni ztjI-V4ytw`-hZ12pqfoo9jti7^*%xy4fy_XKA6`zfCK*P*_VB!aRj>rS$Wzc#{omP zBfd_nleU3H$y3p?AWgil+QGn@w-`orI=fe4o^v#M7}J`Q477dLA8|`zY{b4#ymR1f zsFHj)&=I5iPiq6PcEw5C21875GZ?sJ63RW>=v-!s`%nCaj%@1+0d009@Y{>rh(r3b zk9uxS4X#%yQ4Qy6CX1&g|H}Q9NL?HFyNCa$08e2l-~0zHTDHRkx8}rSgR2vP7vhwn zdGk7b)YpcUTp}$m#C{u<#f1i2i=4J@!@vN8fC+Q``<;hF6tGNvE}RbYVaeuF4rJu6 z;YanBaNVCJ?^Yt2TQsw_gkiLis&z=0=ScpMwhQ^)!^qQP)+6rJFHe%t{(%D`gGZ)L zH%{BObj0(G&kr)u3M^#~p|Llyr)1j3q$1PhXlRLE-oJ7!f2j@!#K5LDz@&-m%7?E} z<6xkW6N+-p)LU<55yNuYB&rI(D6mM}vy=o#&SM!yK=}5Oly;{kUV8JHMdBgER^pV@j%3wT2^?X($PY4Dx2CT?6m zGYcw-P!x)bh6f=6HOW9#{z8@_iMp1g*U0+cBadx=Zw~o>d0@*|kg1mcJA zL&pkQ;nj}ptPhwq#nN~=UheLnAyAR$QzW~` z7&CLfTsz0FE9?Zp-rw+7UXM8EkS zq#RFqYf8u0mO3}|4zzBn6)hYYuhTldefkWKToKa@QIg`xakfgXqbW61tT49`GdUYM zuSh5AfQesKlh@0B+~g`BH=kU$WGQFo{T$oP?zYOIpYFqpPvxCiy(6PoH-p$=DXA0H zfNo-_5SFGKL7&W`NVNmCbTqe;VnB!>m029uFiqb-Y1Qr9g_F^k90qeg&$L2kCfu+^ zGyRYP;h_&MphdU7yeS@9_DkvIbVq+~&95t)n9XT?4E%g3#YOCIWHEf-gS1b(C)ulZsCbL1(Q#s8@J59SgE3z3`PS0aE>h;Cq z9puz)QfA3LT&bCu9rj2tVx`@^|9L(LiQ@5ged8)$v$P5Ds!kR|pw68=0N*@6sv62h4z)0nDOA zAw2Sk+ZjJLN%Ex6wRXp73vnx8Y=iX*c_x=E0n9QE$jvYgV)`?!%negIE5!&e)~BJ;2Nzrv#X4n&>_hyVbM z1OR~ef7%&U0|MOKee5mXL`MIu_ir;wjzA(%f?