From 034d0a6a610c8838d6c43b51a4401ce818b77624 Mon Sep 17 00:00:00 2001 From: Luis Rodrigues Date: Wed, 23 Jul 2014 18:19:49 +0200 Subject: [PATCH 001/172] 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/172] 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/172] 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/172] att ekf: add descriptions for params --- .../attitude_estimator_ekf_params.c | 54 +++++++++++++++---- 1 file changed, 45 insertions(+), 9 deletions(-) diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c index 5c33bc2ac7..5637ec102f 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c @@ -44,18 +44,54 @@ /* Extended Kalman Filter covariances */ -/* gyro process noise */ + +/** + * Body angular rate process noise + * + * @group attitude_ekf + */ PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q0, 1e-4f); + +/** + * Body angular acceleration process noise + * + * @group attitude_ekf + */ PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q1, 0.08f); + +/** + * Acceleration process noise + * + * @group attitude_ekf + */ PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q2, 0.009f); -/* gyro offsets process noise */ + +/** + * Magnet field vector process noise + * + * @group attitude_ekf + */ PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q3, 0.005f); -/* gyro measurement noise */ +/** + * Gyro measurement noise + * + * @group attitude_ekf + */ PARAM_DEFINE_FLOAT(EKF_ATT_V4_R0, 0.0008f); -/* accel measurement noise */ + +/** + * Accel measurement noise + * + * @group attitude_ekf + */ PARAM_DEFINE_FLOAT(EKF_ATT_V4_R1, 10000.0f); -/* mag measurement noise */ + +/** + * Mag measurement noise + * + * @group attitude_ekf + */ PARAM_DEFINE_FLOAT(EKF_ATT_V4_R2, 100.0f); /* magnetic declination, in degrees */ @@ -63,7 +99,7 @@ PARAM_DEFINE_FLOAT(ATT_MAG_DECL, 0.0f); PARAM_DEFINE_INT32(ATT_ACC_COMP, 2); -/* +/** * Moment of inertia matrix diagonal entry (1, 1) * * @group attitude_ekf @@ -71,7 +107,7 @@ PARAM_DEFINE_INT32(ATT_ACC_COMP, 2); */ PARAM_DEFINE_FLOAT(ATT_J11, 0.0018); -/* +/** * Moment of inertia matrix diagonal entry (2, 2) * * @group attitude_ekf @@ -79,7 +115,7 @@ PARAM_DEFINE_FLOAT(ATT_J11, 0.0018); */ PARAM_DEFINE_FLOAT(ATT_J22, 0.0018); -/* +/** * Moment of inertia matrix diagonal entry (3, 3) * * @group attitude_ekf @@ -87,7 +123,7 @@ PARAM_DEFINE_FLOAT(ATT_J22, 0.0018); */ PARAM_DEFINE_FLOAT(ATT_J33, 0.0037); -/* +/** * Moment of inertia enabled in estimator * * If set to != 0 the moment of inertia will be used in the estimator From 58aabe648acf296267bd1e29e8a02b33ea9e7cf7 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 21 Aug 2014 11:25:09 +0200 Subject: [PATCH 005/172] 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 5c77fc0012eaaad5b92e1b31cedf8bf5b035b988 Mon Sep 17 00:00:00 2001 From: philipoe Date: Mon, 20 Oct 2014 15:55:45 +0200 Subject: [PATCH 006/172] 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 007/172] 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 008/172] 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 009/172] 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 010/172] 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 011/172] 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 012/172] 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 013/172] 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 014/172] 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 015/172] 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 016/172] 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 017/172] 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 018/172] 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 019/172] 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 020/172] 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 021/172] 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 83eb81251c8405c96aaa72fc1634d15a5b54af29 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 1 Nov 2014 11:22:18 +0100 Subject: [PATCH 022/172] NSH term: Only time out if no arming information is available, if arming information is available abort if unconnected on arming --- src/systemcmds/nshterm/nshterm.c | 27 +++++++++++++++++++++++++-- 1 file changed, 25 insertions(+), 2 deletions(-) diff --git a/src/systemcmds/nshterm/nshterm.c b/src/systemcmds/nshterm/nshterm.c index f06c49552e..edeb5c6248 100644 --- a/src/systemcmds/nshterm/nshterm.c +++ b/src/systemcmds/nshterm/nshterm.c @@ -51,6 +51,8 @@ #include #include +#include + __EXPORT int nshterm_main(int argc, char *argv[]); int @@ -62,9 +64,30 @@ nshterm_main(int argc, char *argv[]) } unsigned retries = 0; 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) { + + /* abort if an arming topic is published and system is armed */ + bool updated = false; + if (orb_check(armed_fd, &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) { + /* this is not an error, but we are done */ + exit(0); + } + } - /* try the first 30 seconds */ - while (retries < 300) { /* the retries are to cope with the behaviour of /dev/ttyACM0 */ /* which may not be ready immediately. */ fd = open(argv[1], O_RDWR); From c442f820dd89aad9fac394ce56c07b10cfcacf73 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 1 Nov 2014 16:54:11 +0100 Subject: [PATCH 023/172] Encode RC type in RSSI field for GCS --- src/modules/mavlink/mavlink_messages.cpp | 43 +++++++++++++++++++++++- 1 file changed, 42 insertions(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index cccb698bf9..87858690fb 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1717,7 +1717,48 @@ protected: msg.chan16_raw = (rc.channel_count > 15) ? rc.values[15] : UINT16_MAX; msg.chan17_raw = (rc.channel_count > 16) ? rc.values[16] : UINT16_MAX; msg.chan18_raw = (rc.channel_count > 17) ? rc.values[17] : UINT16_MAX; - msg.rssi = rc.rssi; + + /* RSSI has a max value of 100, and when Spektrum or S.BUS are + * available, the RSSI field is invalid, as they do not provide + * an RSSI measurement. Use an out of band magic value to signal + * these digital ports. XXX revise MAVLink spec to address this. + * One option would be to use the top bit to toggle between RSSI + * and input source mode. + * + * Full RSSI field: 0b 1 111 1111 + * + * ^ If bit is set, RSSI encodes type + RSSI + * + * ^ These three bits encode a total of 8 + * digital RC input types. + * 0: PPM, 1: SBUS, 2: Spektrum, 2: ST24 + * ^ These four bits encode a total of + * 16 RSSI levels. 15 = full, 0 = no signal + * + */ + + /* Initialize RSSI with the special mode level flag */ + msg.rssi = (1 << 7); + + /* Set RSSI */ + msg.rssi |= (rc.rssi <= 100) ? ((rc.rssi / 7) + 1) : 15; + + switch (rc.input_source) { + 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; + } _mavlink->send_message(MAVLINK_MSG_ID_RC_CHANNELS, &msg); } From 412ddde5dca055ba090ed2e18db2dc77b6af93a4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 1 Nov 2014 16:58:12 +0100 Subject: [PATCH 024/172] Force RSSI to zero if RC is lost --- src/modules/mavlink/mavlink_messages.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 87858690fb..a2b3cc62eb 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1760,6 +1760,11 @@ protected: break; } + if (rc.rc_lost) { + /* RSSI is by definition zero */ + msg.rssi = 0; + } + _mavlink->send_message(MAVLINK_MSG_ID_RC_CHANNELS, &msg); } } From 58f36714f808c9d06527da1952d348201c4f7e72 Mon Sep 17 00:00:00 2001 From: Holger Steinhaus Date: Thu, 14 Aug 2014 15:43:27 +0200 Subject: [PATCH 025/172] UAVCAN: allow to arm single ESCs --- src/modules/uavcan/actuators/esc.cpp | 26 +++++++++++++++++++++----- src/modules/uavcan/actuators/esc.hpp | 9 ++++++++- src/modules/uavcan/uavcan_main.cpp | 2 +- 3 files changed, 30 insertions(+), 7 deletions(-) diff --git a/src/modules/uavcan/actuators/esc.cpp b/src/modules/uavcan/actuators/esc.cpp index 1990651ef2..fbd4f0bcd4 100644 --- a/src/modules/uavcan/actuators/esc.cpp +++ b/src/modules/uavcan/actuators/esc.cpp @@ -40,6 +40,9 @@ #include "esc.hpp" #include + +#define MOTOR_BIT(x) (1<<(x)) + UavcanEscController::UavcanEscController(uavcan::INode &node) : _node(node), _uavcan_pub_raw_cmd(node), @@ -95,9 +98,8 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs) static const int cmd_max = uavcan::equipment::esc::RawCommand::FieldTypes::cmd::RawValueType::max(); - if (_armed) { - for (unsigned i = 0; i < num_outputs; i++) { - + 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 @@ -112,6 +114,9 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs) _esc_status.esc[i].esc_setpoint_raw = abs(static_cast(scaled)); } + else { + msg.cmd.push_back(static_cast(0)); + } } /* @@ -121,9 +126,20 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs) (void)_uavcan_pub_raw_cmd.broadcast(msg); } -void UavcanEscController::arm_esc(bool arm) +void UavcanEscController::arm_all_escs(bool arm) { - _armed = arm; + if (arm) + _armed_mask = -1; + else + _armed_mask = 0; +} + +void UavcanEscController::arm_single_esc(int num, bool arm) +{ + if (arm) + _armed_mask = MOTOR_BIT(num); + else + _armed_mask = 0; } void UavcanEscController::esc_status_sub_cb(const uavcan::ReceivedDataStructure &msg) diff --git a/src/modules/uavcan/actuators/esc.hpp b/src/modules/uavcan/actuators/esc.hpp index f4a3877e62..ff3ecfb215 100644 --- a/src/modules/uavcan/actuators/esc.hpp +++ b/src/modules/uavcan/actuators/esc.hpp @@ -61,7 +61,8 @@ public: void update_outputs(float *outputs, unsigned num_outputs); - void arm_esc(bool arm); + void arm_all_escs(bool arm); + void arm_single_esc(int num, bool arm); private: /** @@ -98,6 +99,12 @@ private: uavcan::Subscriber _uavcan_sub_status; uavcan::TimerEventForwarder _orb_timer; + /* + * ESC states + */ + uint32_t _armed_mask = 0; + uavcan::equipment::esc::Status _states[MAX_ESCS]; + /* * Perf counters */ diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index a8485ee44e..59f0f43004 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -429,7 +429,7 @@ int UavcanNode::arm_actuators(bool arm) { _is_armed = arm; - _esc_controller.arm_esc(arm); + _esc_controller.arm_all_escs(arm); return OK; } From a18460183cdfa590af29c2299acb30206b418cb1 Mon Sep 17 00:00:00 2001 From: Holger Steinhaus Date: Mon, 3 Nov 2014 19:37:44 +0100 Subject: [PATCH 026/172] motor_test: cleanup --- src/modules/uORB/topics/test_motor.h | 2 +- src/systemcmds/motor_test/motor_test.c | 23 ++++++++++++----------- 2 files changed, 13 insertions(+), 12 deletions(-) diff --git a/src/modules/uORB/topics/test_motor.h b/src/modules/uORB/topics/test_motor.h index 491096934a..c880fe099b 100644 --- a/src/modules/uORB/topics/test_motor.h +++ b/src/modules/uORB/topics/test_motor.h @@ -57,7 +57,7 @@ struct test_motor_s { uint64_t timestamp; /**< output timestamp in us since system boot */ unsigned motor_number; /**< number of motor to spin */ - float value; /**< output data, in natural output units */ + float value; /**< output power, range [0..1] }; /** diff --git a/src/systemcmds/motor_test/motor_test.c b/src/systemcmds/motor_test/motor_test.c index 079f99674a..a73f466264 100644 --- a/src/systemcmds/motor_test/motor_test.c +++ b/src/systemcmds/motor_test/motor_test.c @@ -54,7 +54,8 @@ #include "systemlib/err.h" -__EXPORT int motor_test_main(int argc, char *argv[]); +__EXPORT int motor_test_main(int argc, char *argv[]); + static void motor_test(unsigned channel, float value); static void usage(const char *reason); @@ -67,13 +68,13 @@ void motor_test(unsigned channel, float value) _test_motor.timestamp = hrt_absolute_time(); _test_motor.value = value; - if (_test_motor_pub > 0) { - /* publish armed state */ - orb_publish(ORB_ID(test_motor), _test_motor_pub, &_test_motor); - } else { - /* advertise and publish */ - _test_motor_pub = orb_advertise(ORB_ID(test_motor), &_test_motor); - } + if (_test_motor_pub > 0) { + /* publish test state */ + orb_publish(ORB_ID(test_motor), _test_motor_pub, &_test_motor); + } else { + /* advertise and publish */ + _test_motor_pub = orb_advertise(ORB_ID(test_motor), &_test_motor); + } } static void usage(const char *reason) @@ -102,18 +103,18 @@ int motor_test_main(int argc, char *argv[]) switch (ch) { case 'm': - /* Read in channels supplied as one int and convert to mask: 1234 -> 0xF */ + /* Read in motor number */ channel = strtoul(optarg, NULL, 0); break; case 'p': - /* Read in custom low value */ + /* Read in power value */ lval = strtoul(optarg, NULL, 0); if (lval > 100) usage("value invalid"); - value = (float)lval/100.f; + value = ((float)lval)/100.f; break; default: usage(NULL); From 0800fa4715994921b6a0d15cd2c44b9e51417117 Mon Sep 17 00:00:00 2001 From: Holger Steinhaus Date: Mon, 3 Nov 2014 18:47:07 +0100 Subject: [PATCH 027/172] UAVCAN: implemented motor testing --- src/modules/uavcan/actuators/esc.hpp | 1 - src/modules/uavcan/uavcan_main.cpp | 28 ++++++++++++++++++++++++---- src/modules/uavcan/uavcan_main.hpp | 5 +++++ 3 files changed, 29 insertions(+), 5 deletions(-) diff --git a/src/modules/uavcan/actuators/esc.hpp b/src/modules/uavcan/actuators/esc.hpp index ff3ecfb215..12c0355422 100644 --- a/src/modules/uavcan/actuators/esc.hpp +++ b/src/modules/uavcan/actuators/esc.hpp @@ -103,7 +103,6 @@ private: * ESC states */ uint32_t _armed_mask = 0; - uavcan::equipment::esc::Status _states[MAX_ESCS]; /* * Perf counters diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 59f0f43004..653d4f98cf 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -279,6 +279,7 @@ int UavcanNode::run() _output_count = 2; _armed_sub = orb_subscribe(ORB_ID(actuator_armed)); + _test_motor_sub = orb_subscribe(ORB_ID(test_motor)); actuator_outputs_s outputs; memset(&outputs, 0, sizeof(outputs)); @@ -344,7 +345,14 @@ int UavcanNode::run() } // can we mix? - if (controls_updated && (_mixers != nullptr)) { + if (_test_in_progress) { + float outputs[NUM_ACTUATOR_OUTPUTS] = {}; + outputs[_test_motor.motor_number] = _test_motor.value*2.0f-1.0f; + + // Output to the bus + _esc_controller.update_outputs(outputs, NUM_ACTUATOR_OUTPUTS); + } + else if (controls_updated && (_mixers != nullptr)) { // XXX one output group has 8 outputs max, // but this driver could well serve multiple groups. @@ -384,15 +392,27 @@ int UavcanNode::run() } } - // Check arming state + // Check motor test state bool updated = false; + orb_check(_test_motor_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(test_motor), _test_motor_sub, &_test_motor); + + // Update the test status and check that we're not locked down + _test_in_progress = (_test_motor.value > 0); + _esc_controller.arm_single_esc(_test_motor.motor_number, _test_in_progress); + } + + // Check arming state orb_check(_armed_sub, &updated); if (updated) { orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed); - // Update the armed status and check that we're not locked down - bool set_armed = _armed.armed && !_armed.lockdown; + // Update the armed status and check that we're not locked down and motor + // test is not running + bool set_armed = _armed.armed && !_armed.lockdown && !_test_in_progress; arm_actuators(set_armed); } diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index be7db97411..274321f0db 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -41,6 +41,7 @@ #include #include #include +#include #include "actuators/esc.hpp" #include "sensors/sensor_bridge.hpp" @@ -103,6 +104,10 @@ private: 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; + bool _test_in_progress = false; + unsigned _output_count = 0; ///< number of actuators currently available static UavcanNode *_instance; ///< singleton pointer From d907b030ee618b581e31964a34a61648e88793a3 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Wed, 5 Nov 2014 13:15:52 -0800 Subject: [PATCH 028/172] Initialize RSSI so it doesn't remain uninitialized MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Spektrum doesn’t support rssi so it is not set by st24_decode. --- src/modules/px4iofirmware/controls.c | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index ad60ee03e0..3fd73fc601 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -41,6 +41,7 @@ #include #include +#include #include #include #include @@ -91,6 +92,7 @@ bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated) for (unsigned i = 0; i < n_bytes; i++) { /* set updated flag if one complete packet was parsed */ + st24_rssi = RC_INPUT_RSSI_MAX; *st24_updated |= (OK == st24_decode(bytes[i], &st24_rssi, &rx_count, &st24_channel_count, r_raw_rc_values, PX4IO_RC_INPUT_CHANNELS)); } From f414eef042be7ebf641638009b3676523bf4bed6 Mon Sep 17 00:00:00 2001 From: philipoe Date: Tue, 4 Nov 2014 11:10:14 +0100 Subject: [PATCH 029/172] TECS: Modify absolute-value limiting of throttle demand --- src/lib/external_lgpl/tecs/tecs.cpp | 14 ++++++-------- 1 file changed, 6 insertions(+), 8 deletions(-) diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp index 0ed210cf4c..cfcc48b62a 100644 --- a/src/lib/external_lgpl/tecs/tecs.cpp +++ b/src/lib/external_lgpl/tecs/tecs.cpp @@ -321,31 +321,29 @@ void TECS::_update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotM ff_throttle = nomThr - STEdot_dem / _STEdot_min * nomThr; } - // Calculate PD + FF throttle + // Calculate PD + FF throttle and constrain to avoid blow-up of the integrator later _throttle_dem = (_STE_error + STEdot_error * _thrDamp) * K_STE2Thr + ff_throttle; + _throttle_dem = constrain(_throttle_dem, _THRminf, _THRmaxf); // Rate limit PD + FF throttle // Calculate the throttle increment from the specified slew time if (fabsf(_throttle_slewrate) > 0.01f) { float thrRateIncr = _DT * (_THRmaxf - _THRminf) * _throttle_slewrate; - _throttle_dem = constrain(_throttle_dem, - _last_throttle_dem - thrRateIncr, - _last_throttle_dem + thrRateIncr); + _last_throttle_dem - thrRateIncr, + _last_throttle_dem + thrRateIncr); } // Ensure _last_throttle_dem is always initialized properly - // Also: The throttle_slewrate limit is only applied to throttle_dem, but does not limit the integrator!! _last_throttle_dem = _throttle_dem; - // Calculate integrator state upper and lower limits - // Set to a value thqat will allow 0.1 (10%) throttle saturation to allow for noise on the demand + // Set to a value that will allow 0.1 (10%) throttle saturation to allow for noise on the demand float integ_max = (_THRmaxf - _throttle_dem + 0.1f); float integ_min = (_THRminf - _throttle_dem - 0.1f); // Calculate integrator state, constraining state - // Set integrator to a max throttle value dduring climbout + // Set integrator to a max throttle value during climbout _integ6_state = _integ6_state + (_STE_error * _integGain) * _DT * K_STE2Thr; if (_climbOutDem) { From 3f3353f2c4ad9f7b8263379c8b31bd9a437b0174 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 5 Nov 2014 21:19:04 +1100 Subject: [PATCH 030/172] mixer: fixed stream handle leakage --- src/modules/systemlib/mixer/mixer_load.c | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/systemlib/mixer/mixer_load.c b/src/modules/systemlib/mixer/mixer_load.c index bf3428a50b..0d629d6100 100644 --- a/src/modules/systemlib/mixer/mixer_load.c +++ b/src/modules/systemlib/mixer/mixer_load.c @@ -91,6 +91,7 @@ int load_mixer_file(const char *fname, char *buf, unsigned maxlen) /* if the line is too long to fit in the buffer, bail */ if ((strlen(line) + strlen(buf) + 1) >= maxlen) { warnx("line too long"); + fclose(fp); return -1; } @@ -98,6 +99,7 @@ int load_mixer_file(const char *fname, char *buf, unsigned maxlen) strcat(buf, line); } + fclose(fp); return 0; } From d253c8ba7d9cfc408328afd7c3708892b23626c0 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 10 Nov 2014 21:38:13 +1000 Subject: [PATCH 031/172] commander: correct the description array --- src/modules/commander/commander.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 46caddd469..1c3b68edd5 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -760,7 +760,10 @@ int commander_thread_main(int argc, char *argv[]) nav_states_str[NAVIGATION_STATE_AUTO_MISSION] = "AUTO_MISSION"; nav_states_str[NAVIGATION_STATE_AUTO_LOITER] = "AUTO_LOITER"; nav_states_str[NAVIGATION_STATE_AUTO_RTL] = "AUTO_RTL"; + nav_states_str[NAVIGATION_STATE_AUTO_RCRECOVER] = "AUTO_RCRECOVER"; nav_states_str[NAVIGATION_STATE_AUTO_RTGS] = "AUTO_RTGS"; + nav_states_str[NAVIGATION_STATE_AUTO_LANDENGFAIL] = "AUTO_LANDENGFAIL"; + nav_states_str[NAVIGATION_STATE_AUTO_LANDGPSFAIL] = "AUTO_LANDGPSFAIL"; nav_states_str[NAVIGATION_STATE_ACRO] = "ACRO"; nav_states_str[NAVIGATION_STATE_LAND] = "LAND"; nav_states_str[NAVIGATION_STATE_DESCEND] = "DESCEND"; From f7db91ac32d6db1fbef9aa0c868636561bd2282c Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 10 Nov 2014 21:38:46 +1000 Subject: [PATCH 032/172] commander: enhance the failsafe verbose output --- src/modules/commander/commander.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 1c3b68edd5..f9ea4e9147 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1851,7 +1851,11 @@ int commander_thread_main(int argc, char *argv[]) if (status.failsafe != failsafe_old) { status_changed = true; - mavlink_log_info(mavlink_fd, "[cmd] failsafe state: %i", status.failsafe); + if (status.failsafe) { + mavlink_log_info(mavlink_fd, "[cmd] failsafe on"); + } else { + mavlink_log_info(mavlink_fd, "[cmd] failsafe off"); + } failsafe_old = status.failsafe; } From f8bed3cd892157b2db147b0d6a60f26371acdeb8 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 10 Nov 2014 21:39:35 +1000 Subject: [PATCH 033/172] state_machine_helper: whitespace --- 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 9568752aed..a49965efd9 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -444,7 +444,7 @@ transition_result_t hil_state_transition(hil_state_t new_state, int status_pub, * Check failsafe and main status and set navigation status for navigator accordingly */ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished, - const bool stay_in_failsafe) + const bool stay_in_failsafe) { navigation_state_t nav_state_old = status->nav_state; From 2c577815818b579743dab13ef0055f02605699d1 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 10 Nov 2014 21:40:13 +1000 Subject: [PATCH 034/172] state_machine_helper: trying to clean up some failsafe logic --- .../commander/state_machine_helper.cpp | 43 +++++++++++++------ 1 file changed, 30 insertions(+), 13 deletions(-) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index a49965efd9..3553dcdc3c 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -497,11 +497,14 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en break; case MAIN_STATE_AUTO_MISSION: + /* go into failsafe * - if commanded to do so * - if we have an engine failure - * - if either the datalink is enabled and lost as well as RC is lost - * - if there is no datalink and the mission is finished */ + * - if the datalink is enabled and lost as well as RC is lost + * or if the datalink is disabled and lost as well as RC is lost and the mission is finished */ + + /* first look at the commands */ if (status->engine_failure_cmd) { status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL; } else if (status->data_link_lost_cmd) { @@ -509,14 +512,16 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en } else if (status->gps_failure_cmd) { status->nav_state = NAVIGATION_STATE_AUTO_LANDGPSFAIL; } else if (status->rc_signal_lost_cmd) { - status->nav_state = NAVIGATION_STATE_AUTO_RTGS; //XXX - /* Finished handling commands which have priority , now handle failures */ + status->nav_state = NAVIGATION_STATE_AUTO_RCRECOVER; + + /* finished handling commands which have priority, now handle failures */ } else if (status->gps_failure) { status->nav_state = NAVIGATION_STATE_AUTO_LANDGPSFAIL; } else if (status->engine_failure) { status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL; - } else if (((status->data_link_lost && data_link_loss_enabled) && status->rc_signal_lost) || - (!data_link_loss_enabled && status->rc_signal_lost && mission_finished)) { + + /* check for RC signal and datalink lost (with datalink enabled) after mission is finished */ + } else if ((status->data_link_lost && data_link_loss_enabled) && status->rc_signal_lost) { status->failsafe = true; if (status->condition_global_position_valid && status->condition_home_position_valid) { @@ -529,7 +534,21 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en status->nav_state = NAVIGATION_STATE_TERMINATION; } - /* also go into failsafe if just datalink is lost */ + /* check if RC signal is lost (with dataink disabled) after mission is finished*/ + } else if (!data_link_loss_enabled && status->rc_signal_lost && mission_finished) { + status->failsafe = true; + + if (status->condition_global_position_valid && status->condition_home_position_valid) { + status->nav_state = NAVIGATION_STATE_AUTO_RCRECOVER; + } else if (status->condition_local_position_valid) { + status->nav_state = NAVIGATION_STATE_LAND; + } else if (status->condition_local_altitude_valid) { + status->nav_state = NAVIGATION_STATE_DESCEND; + } else { + status->nav_state = NAVIGATION_STATE_TERMINATION; + } + + /* also go into failsafe if just datalink is lost (with datalink enabled) */ } else if (status->data_link_lost && data_link_loss_enabled) { status->failsafe = true; @@ -543,13 +562,11 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en status->nav_state = NAVIGATION_STATE_TERMINATION; } - /* don't bother if RC is lost and mission is not yet finished */ - } else if (status->rc_signal_lost && !stay_in_failsafe) { + /* stay where you are */ + } else if (stay_in_failsafe){ - /* this mode is ok, we don't need RC for missions */ - status->nav_state = NAVIGATION_STATE_AUTO_MISSION; - } else if (!stay_in_failsafe){ - /* everything is perfect */ + /* everything is perfect */ + } else { status->nav_state = NAVIGATION_STATE_AUTO_MISSION; } break; From f1d56cbddc575115215f680ba42cdedde40b2626 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 10 Nov 2014 21:41:41 +1000 Subject: [PATCH 035/172] navigator: don't reset the finished flag, this fixes the strange problem where it toggles between MISSION and RTL --- src/modules/navigator/navigator_main.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index a867dd0daf..eff3a59383 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -632,7 +632,6 @@ Navigator::publish_mission_result() } /* reset reached bool */ _mission_result.reached = false; - _mission_result.finished = false; } void From 89724c24a16bd340c33c27531435089e7bd338fb Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 10 Nov 2014 21:42:13 +1000 Subject: [PATCH 036/172] vehicle_status: whitespace --- src/modules/uORB/topics/vehicle_status.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index a1b2667e39..91491c148f 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -108,7 +108,7 @@ typedef enum { NAVIGATION_STATE_AUTO_LANDGPSFAIL, /**< Auto land on gps failure (e.g. open loop loiter down) */ NAVIGATION_STATE_ACRO, /**< Acro mode */ NAVIGATION_STATE_LAND, /**< Land mode */ - NAVIGATION_STATE_DESCEND, /**< Descend mode (no position control) */ + NAVIGATION_STATE_DESCEND, /**< Descend mode (no position control) */ NAVIGATION_STATE_TERMINATION, /**< Termination mode */ NAVIGATION_STATE_OFFBOARD, NAVIGATION_STATE_MAX, From d15203de91cc07530809060f949374bb8918daea Mon Sep 17 00:00:00 2001 From: mazahner Date: Mon, 10 Nov 2014 17:35:48 +0100 Subject: [PATCH 037/172] Debug/NuttX: small changes to make it work with the current stable branch. Debug/NuttX made existing code work with python3 in gdb. Added some new features --- Debug/NuttX | 38 ++++- Debug/Nuttx.py | 433 +++++++++++++++++++++++++++++++++++++++++++------ 2 files changed, 416 insertions(+), 55 deletions(-) diff --git a/Debug/NuttX b/Debug/NuttX index d34e9f5b4b..4b9f4b5a1b 100644 --- a/Debug/NuttX +++ b/Debug/NuttX @@ -78,7 +78,7 @@ end ################################################################################ define showfiles - set $task = (struct _TCB *)$arg0 + set $task = (struct tcb_s *)$arg0 set $nfiles = sizeof((*(struct filelist*)0).fl_files) / sizeof(struct file) printf "%d files\n", $nfiles set $index = 0 @@ -102,7 +102,7 @@ end ################################################################################ define _showtask_oneline - set $task = (struct _TCB *)$arg0 + set $task = (struct tcb_s *)$arg0 printf " %p %.2d %.3d %s\n", $task, $task->pid, $task->sched_priority, $task->name end @@ -139,7 +139,7 @@ end # Print task registers for a NuttX v7em target with FPU enabled. # define _showtaskregs_v7em - set $task = (struct _TCB *)$arg0 + set $task = (struct tcb_s *)$arg0 set $regs = (uint32_t *)&($task->xcp.regs[0]) printf " r0: 0x%08x 0x%08x 0x%08x 0x%08x 0x%08x 0x%08x 0x%08x 0x%08x\n", $regs[27], $regs[28], $regs[29], $regs[30], $regs[2], $regs[3], $regs[4], $regs[5] @@ -162,7 +162,7 @@ end define _showsemaphore printf "count %d ", $arg0->semcount if $arg0->holder.htcb != 0 - set $_task = (struct _TCB *)$arg0->holder.htcb + set $_task = (struct tcb_s *)$arg0->holder.htcb printf "held by %s", $_task->name end printf "\n" @@ -172,7 +172,7 @@ end # Print information about a task's stack usage # define showtaskstack - set $task = (struct _TCB *)$arg0 + set $task = (struct tcb_s *)$arg0 if $task == &g_idletcb printf "can't measure idle stack\n" @@ -189,7 +189,7 @@ end # Print details of a task # define showtask - set $task = (struct _TCB *)$arg0 + set $task = (struct tcb_s *)$arg0 printf "%p %.2d ", $task, $task->pid _showtaskstate $task @@ -204,7 +204,7 @@ define showtask if $task->task_state != TSTATE_TASK_RUNNING _showtaskregs_v7em $task else - _showcurrentregs_v7em + _showtaskregs_v7em $task end # XXX print registers here @@ -247,8 +247,10 @@ define showtasks _showtasklist &g_pendingtasks printf "RUNNABLE\n" _showtasklist &g_readytorun - printf "WAITING\n" + printf "WAITING for Semaphore\n" _showtasklist &g_waitingforsemaphore + printf "WAITING for Signal\n" + _showtasklist &g_waitingforsignal printf "INACTIVE\n" _showtasklist &g_inactivetasks end @@ -257,3 +259,23 @@ document showtasks . showtasks . Print a list of all tasks in the system, separated into their respective queues. end + +define my_mem + + set $start = $arg0 + set $end = $arg1 + set $cursor = $start + + if $start < $end + while $cursor != $end + set *$cursor = 0x0000 + set $cursor = $cursor + 4 + printf "0x%x of 0x%x\n",$cursor,$end + end + else + while $cursor != $end + set *$cursor = 0x0000 + set $cursor = $cursor - 4 + end + end +end \ No newline at end of file diff --git a/Debug/Nuttx.py b/Debug/Nuttx.py index 7cc21b99f4..cf86dd668f 100644 --- a/Debug/Nuttx.py +++ b/Debug/Nuttx.py @@ -59,30 +59,42 @@ class NX_register_set(object): def __init__(self, xcpt_regs): if xcpt_regs is None: - self.regs['R0'] = long(gdb.parse_and_eval('$r0')) - self.regs['R1'] = long(gdb.parse_and_eval('$r1')) - self.regs['R2'] = long(gdb.parse_and_eval('$r2')) - self.regs['R3'] = long(gdb.parse_and_eval('$r3')) - self.regs['R4'] = long(gdb.parse_and_eval('$r4')) - self.regs['R5'] = long(gdb.parse_and_eval('$r5')) - self.regs['R6'] = long(gdb.parse_and_eval('$r6')) - self.regs['R7'] = long(gdb.parse_and_eval('$r7')) - self.regs['R8'] = long(gdb.parse_and_eval('$r8')) - self.regs['R9'] = long(gdb.parse_and_eval('$r9')) - self.regs['R10'] = long(gdb.parse_and_eval('$r10')) - self.regs['R11'] = long(gdb.parse_and_eval('$r11')) - self.regs['R12'] = long(gdb.parse_and_eval('$r12')) - self.regs['R13'] = long(gdb.parse_and_eval('$r13')) - self.regs['SP'] = long(gdb.parse_and_eval('$sp')) - self.regs['R14'] = long(gdb.parse_and_eval('$r14')) - self.regs['LR'] = long(gdb.parse_and_eval('$lr')) - self.regs['R15'] = long(gdb.parse_and_eval('$r15')) - self.regs['PC'] = long(gdb.parse_and_eval('$pc')) - self.regs['XPSR'] = long(gdb.parse_and_eval('$xpsr')) + self.regs['R0'] = self.mon_reg_call('r0') + self.regs['R1'] = self.mon_reg_call('r1') + self.regs['R2'] = self.mon_reg_call('r2') + self.regs['R3'] = self.mon_reg_call('r3') + self.regs['R4'] = self.mon_reg_call('r4') + self.regs['R5'] = self.mon_reg_call('r5') + self.regs['R6'] = self.mon_reg_call('r6') + self.regs['R7'] = self.mon_reg_call('r7') + self.regs['R8'] = self.mon_reg_call('r8') + self.regs['R9'] = self.mon_reg_call('r9') + self.regs['R10'] = self.mon_reg_call('r10') + self.regs['R11'] = self.mon_reg_call('r11') + self.regs['R12'] = self.mon_reg_call('r12') + self.regs['R13'] = self.mon_reg_call('r13') + self.regs['SP'] = self.mon_reg_call('sp') + self.regs['R14'] = self.mon_reg_call('r14') + self.regs['LR'] = self.mon_reg_call('lr') + self.regs['R15'] = self.mon_reg_call('r15') + self.regs['PC'] = self.mon_reg_call('pc') + self.regs['XPSR'] = self.mon_reg_call('xPSR') else: for key in self.v7em_regmap.keys(): self.regs[key] = int(xcpt_regs[self.v7em_regmap[key]]) + def mon_reg_call(self,register): + """ + register is the register as a string e.g. 'pc' + return integer containing the value of the register + """ + str_to_eval = "mon reg "+register + resp = gdb.execute(str_to_eval,to_string = True) + content = resp.split()[-1]; + try: + return int(content,16) + except: + return 0 @classmethod def with_xcpt_regs(cls, xcpt_regs): @@ -172,7 +184,7 @@ class NX_task(object): self.__dict__['stack_used'] = 0 else: stack_limit = self._tcb['adj_stack_size'] - for offset in range(0, stack_limit): + for offset in range(0, int(stack_limit)): if stack_base[offset] != 0xff: break self.__dict__['stack_used'] = stack_limit - offset @@ -187,7 +199,7 @@ class NX_task(object): def state(self): """return the name of the task's current state""" statenames = gdb.types.make_enum_dict(gdb.lookup_type('enum tstate_e')) - for name,value in statenames.iteritems(): + for name,value in statenames.items(): if value == self._tcb['task_state']: return name return 'UNKNOWN' @@ -196,16 +208,19 @@ class NX_task(object): def waiting_for(self): """return a description of what the task is waiting for, if it is waiting""" if self._state_is('TSTATE_WAIT_SEM'): - waitsem = self._tcb['waitsem'].dereference() - waitsem_holder = waitsem['holder'] - holder = NX_task.for_tcb(waitsem_holder['htcb']) - if holder is not None: - return '{}({})'.format(waitsem.address, holder.name) - else: - return '{}()'.format(waitsem.address) + try: + waitsem = self._tcb['waitsem'].dereference() + waitsem_holder = waitsem['holder'] + holder = NX_task.for_tcb(waitsem_holder['htcb']) + if holder is not None: + return '{}({})'.format(waitsem.address, holder.name) + else: + return '{}()'.format(waitsem.address) + except: + return 'EXCEPTION' if self._state_is('TSTATE_WAIT_SIG'): return 'signal' - return None + return "" @property def is_waiting(self): @@ -229,7 +244,7 @@ class NX_task(object): filearray = filelist['fl_files'] result = dict() for i in range(filearray.type.range()[0],filearray.type.range()[1]): - inode = long(filearray[i]['f_inode']) + inode = int(filearray[i]['f_inode']) if inode != 0: result[i] = inode return result @@ -253,7 +268,18 @@ class NX_task(object): def __str__(self): return "{}:{}".format(self.pid, self.name) - + + def showoff(self): + print("-------") + print(self.pid,end = ", ") + print(self.name,end = ", ") + print(self.state,end = ", ") + print(self.waiting_for,end = ", ") + print(self.stack_used,end = ", ") + print(self._tcb['adj_stack_size'],end = ", ") + print(self.file_descriptors) + print(self.registers) + def __format__(self, format_spec): return format_spec.format( pid = self.pid, @@ -265,7 +291,7 @@ class NX_task(object): file_descriptors = self.file_descriptors, registers = self.registers ) - + class NX_show_task (gdb.Command): """(NuttX) prints information about a task""" @@ -285,7 +311,7 @@ class NX_show_task (gdb.Command): my_fmt += ' R8 {registers[R8]:#010x} {registers[R9]:#010x} {registers[R10]:#010x} {registers[R11]:#010x}\n' my_fmt += ' R12 {registers[PC]:#010x}\n' my_fmt += ' SP {registers[SP]:#010x} LR {registers[LR]:#010x} PC {registers[PC]:#010x} XPSR {registers[XPSR]:#010x}\n' - print format(t, my_fmt) + print(format(t, my_fmt)) class NX_show_tasks (gdb.Command): """(NuttX) prints a list of tasks""" @@ -295,8 +321,10 @@ class NX_show_tasks (gdb.Command): def invoke(self, args, from_tty): tasks = NX_task.tasks() + print ('Number of tasks: ' + str(len(tasks))) for t in tasks: - print format(t, '{pid:<2} {name:<16} {state:<20} {stack_used:>4}/{stack_limit:<4}') + #t.showoff() + print(format(t, 'Task: {pid} {name} {state} {stack_used}/{stack_limit}')) NX_show_task() NX_show_tasks() @@ -306,15 +334,15 @@ class NX_show_heap (gdb.Command): def __init__(self): super(NX_show_heap, self).__init__('show heap', gdb.COMMAND_USER) - struct_mm_allocnode_s = gdb.lookup_type('struct mm_allocnode_s') - preceding_size = struct_mm_allocnode_s['preceding'].type.sizeof - if preceding_size == 2: + struct_mm_allocnode_s = gdb.lookup_type('struct mm_allocnode_s') + preceding_size = struct_mm_allocnode_s['preceding'].type.sizeof + if preceding_size == 2: self._allocflag = 0x8000 - elif preceding_size == 4: + elif preceding_size == 4: self._allocflag = 0x80000000 - else: - raise gdb.GdbError('invalid mm_allocnode_s.preceding size %u' % preceding_size) - self._allocnodesize = struct_mm_allocnode_s.sizeof + else: + raise gdb.GdbError('invalid mm_allocnode_s.preceding size %u' % preceding_size) + self._allocnodesize = struct_mm_allocnode_s.sizeof def _node_allocated(self, allocnode): if allocnode['preceding'] & self._allocflag: @@ -328,7 +356,7 @@ class NX_show_heap (gdb.Command): if region_start >= region_end: raise gdb.GdbError('heap region {} corrupt'.format(hex(region_start))) nodecount = region_end - region_start - print 'heap {} - {}'.format(region_start, region_end) + print ('heap {} - {}'.format(region_start, region_end)) cursor = 1 while cursor < nodecount: allocnode = region_start[cursor] @@ -336,8 +364,8 @@ class NX_show_heap (gdb.Command): state = '' else: state = '(free)' - print ' {} {} {}'.format(allocnode.address + self._allocnodesize, - self._node_size(allocnode), state) + print( ' {} {} {}'.format(allocnode.address + self._allocnodesize, + self._node_size(allocnode), state)) cursor += self._node_size(allocnode) / self._allocnodesize def invoke(self, args, from_tty): @@ -345,7 +373,7 @@ class NX_show_heap (gdb.Command): nregions = heap['mm_nregions'] region_starts = heap['mm_heapstart'] region_ends = heap['mm_heapend'] - print '{} heap(s)'.format(nregions) + print( '{} heap(s)'.format(nregions)) # walk the heaps for i in range(0, nregions): self._print_allocations(region_starts[i], region_ends[i]) @@ -370,6 +398,317 @@ class NX_show_interrupted_thread (gdb.Command): my_fmt += ' R8 {registers[R8]:#010x} {registers[R9]:#010x} {registers[R10]:#010x} {registers[R11]:#010x}\n' my_fmt += ' R12 {registers[PC]:#010x}\n' my_fmt += ' SP {registers[SP]:#010x} LR {registers[LR]:#010x} PC {registers[PC]:#010x} XPSR {registers[XPSR]:#010x}\n' - print format(registers, my_fmt) + print (format(registers, my_fmt)) NX_show_interrupted_thread() + +class NX_check_tcb(gdb.Command): + """ check the tcb of a task from a address """ + def __init__(self): + super(NX_check_tcb,self).__init__('show tcb', gdb.COMMAND_USER) + + def invoke(self,args,sth): + tasks = NX_task.tasks() + print("tcb int: ",int(args)) + print(tasks[int(args)]._tcb) + a =tasks[int(args)]._tcb['xcp']['regs'] + print("relevant registers:") + for reg in regmap: + hex_addr= hex(int(a[regmap[reg]])) + eval_string = 'info line *'+str(hex_addr) + print(reg,": ",hex_addr,) +NX_check_tcb() + +class NX_tcb(object): + def __init__(self): + pass + + def is_in(self,arg,list): + for i in list: + if arg == i: + return True; + return False + + def find_tcb_list(self,dq_entry_t): + tcb_list = [] + tcb_ptr = dq_entry_t.cast(gdb.lookup_type('struct tcb_s').pointer()) + 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]): + 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] + + def getTCB(self): + list_of_listsnames = ['g_pendingtasks','g_readytorun','g_waitingforsemaphore','g_waitingforsignal','g_inactivetasks'] + tcb_list = []; + for l in list_of_listsnames: + li = gdb.lookup_global_symbol(l) + print(li) + cursor = li.value()['head'] + tcb_list = tcb_list + self.find_tcb_list(cursor) + +class NX_check_stack_order(gdb.Command): + """ Check the Stack order corresponding to the tasks """ + + def __init__(self): + super(NX_check_stack_order,self).__init__('show check_stack', gdb.COMMAND_USER) + + def is_in(self,arg,list): + for i in list: + if arg == i: + return True; + return False + + def find_tcb_list(self,dq_entry_t): + tcb_list = [] + tcb_ptr = dq_entry_t.cast(gdb.lookup_type('struct tcb_s').pointer()) + 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]): + 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] + + def getTCB(self): + list_of_listsnames = ['g_pendingtasks','g_readytorun','g_waitingforsemaphore','g_waitingforsignal','g_inactivetasks'] + tcb_list = []; + for l in list_of_listsnames: + li = gdb.lookup_global_symbol(l) + cursor = li.value()['head'] + tcb_list = tcb_list + self.find_tcb_list(cursor) + return tcb_list + + def getSPfromTask(self,tcb): + regmap = NX_register_set.v7em_regmap + a =tcb['xcp']['regs'] + return int(a[regmap['SP']]) + + def find_closest(self,list,val): + tmp_list = [abs(i-val) for i in list] + tmp_min = min(tmp_list) + idx = tmp_list.index(tmp_min) + return idx,list[idx] + + def find_next_stack(self,address,_dict_in): + add_list = [] + name_list = [] + for key in _dict_in.keys(): + for i in range(3): + if _dict_in[key][i] < address: + add_list.append(_dict_in[key][i]) + if i == 2: # the last one is the processes stack pointer + name_list.append(self.check_name(key)+"_SP") + else: + name_list.append(self.check_name(key)) + + idx,new_address = self.find_closest(add_list,address) + return new_address,name_list[idx] + + def check_name(self,name): + if isinstance(name,(list)): + name = name[0]; + idx = name.find("\\") + newname = name[:idx] + + return newname + + def invoke(self,args,sth): + tcb = self.getTCB(); + stackadresses={}; + 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(self.getSPfromTask(t)) + stackadresses[str(t['name'])] = p; + address = int("0x30000000",0) + print("stack address : process") + for i in range(len(stackadresses)*3): + address,name = self.find_next_stack(address,stackadresses) + print(hex(address),": ",name) + +NX_check_stack_order() + +class NX_run_debug_util(gdb.Command): + """ show the registers of a task corresponding to a tcb address""" + def __init__(self): + super(NX_run_debug_util,self).__init__('show regs', gdb.COMMAND_USER) + + def printRegisters(self,task): + regmap = NX_register_set.v7em_regmap + a =task._tcb['xcp']['regs'] + print("relevant registers in ",task.name,":") + for reg in regmap: + hex_addr= hex(int(a[regmap[reg]])) + eval_string = 'info line *'+str(hex_addr) + print(reg,": ",hex_addr,) + + def getPCfromTask(self,task): + regmap = NX_register_set.v7em_regmap + a =task._tcb['xcp']['regs'] + return hex(int(a[regmap['PC']])) + + def invoke(self,args,sth): + tasks = NX_task.tasks() + if args == '': + for t in tasks: + self.printRegisters(t) + eval_str = "list *"+str(self.getPCfromTask(t)) + print("this is the location in code where the current threads $pc is:") + gdb.execute(eval_str) + else: + tcb_nr = int(args); + print("tcb_nr = ",tcb_nr) + t = tasks[tcb_nr] + self.printRegisters(t) + eval_str = "list *"+str(self.getPCfromTask(t)) + print("this is the location in code where the current threads $pc is:") + gdb.execute(eval_str) + +NX_run_debug_util() + + +class NX_search_tcb(gdb.Command): + """ shot PID's of all running tasks """ + + def __init__(self): + super(NX_search_tcb,self).__init__('show alltcb', gdb.COMMAND_USER) + + def is_in(self,arg,list): + for i in list: + if arg == i: + return True; + return False + + def find_tcb_list(self,dq_entry_t): + tcb_list = [] + tcb_ptr = dq_entry_t.cast(gdb.lookup_type('struct tcb_s').pointer()) + 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]): + 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] + + def invoke(self,args,sth): + list_of_listsnames = ['g_pendingtasks','g_readytorun','g_waitingforsemaphore','g_waitingforsignal','g_inactivetasks'] + tasks = []; + for l in list_of_listsnames: + li = gdb.lookup_global_symbol(l) + cursor = li.value()['head'] + tasks = tasks + self.find_tcb_list(cursor) + + # filter for tasks that are listed twice + tasks_filt = {} + for t in tasks: + pid = 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))) + for pid in tasks_filt.keys(): + print("PID: ",pid," ",tasks_filt[pid]) + +NX_search_tcb() + + +class NX_my_bt(gdb.Command): + """ 'fake' backtrace: backtrace the stack of a process and check every suspicious address for the list + arg: tcb_address$ + (can easily be found by typing 'showtask'). + """ + + def __init__(self): + super(NX_my_bt,self).__init__('show mybt', gdb.COMMAND_USER) + + def readmem(self,addr): + ''' + read memory at addr and return nr + ''' + str_to_eval = "x/x "+hex(addr) + resp = gdb.execute(str_to_eval,to_string = True) + idx = resp.find('\t') + return int(resp[idx:],16) + + def is_in_bounds(self,val): + lower_bound = int("08004000",16) + upper_bound = int("080ae0c0",16); + #print(lower_bound," ",val," ",upper_bound) + if val>lower_bound and val0: + 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 + + + + + def invoke(self,args,sth): + addr_dec = int(args[2:],16) + _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 + + 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): + 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 + +NX_my_bt() From 88093ebf1af5483d8b5c218adb1ea0c4798ee21e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 10 Nov 2014 18:52:18 +0100 Subject: [PATCH 038/172] Hotfix: Fix IO channel mapping --- src/drivers/px4io/px4io.cpp | 47 +++++++++++++++++++------------------ 1 file changed, 24 insertions(+), 23 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 6d68d9f604..06faf49a4f 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1245,41 +1245,42 @@ PX4IO::io_set_rc_config() * for compatibility reasons with existing * autopilots / GCS'. */ + + /* ROLL */ param_get(param_find("RC_MAP_ROLL"), &ichan); + if ((ichan > 0) && (ichan <= (int)_max_rc_input)) { + input_map[ichan - 1] = 0; + } - /* subtract one from 1-based index - this might be - * a negative number now - */ - ichan -= 1; - - if ((ichan >= 0) && (ichan < (int)_max_rc_input)) - input_map[ichan] = 0; - + /* PITCH */ param_get(param_find("RC_MAP_PITCH"), &ichan); + if ((ichan > 0) && (ichan <= (int)_max_rc_input)) { + input_map[ichan - 1] = 1; + } - if ((ichan >= 0) && (ichan < (int)_max_rc_input)) - input_map[ichan] = 1; - + /* YAW */ param_get(param_find("RC_MAP_YAW"), &ichan); + if ((ichan > 0) && (ichan <= (int)_max_rc_input)) { + input_map[ichan - 1] = 2; + } - if ((ichan >= 0) && (ichan < (int)_max_rc_input)) - input_map[ichan] = 2; - + /* THROTTLE */ param_get(param_find("RC_MAP_THROTTLE"), &ichan); + if ((ichan > 0) && (ichan <= (int)_max_rc_input)) { + input_map[ichan - 1] = 3; + } - if ((ichan >= 0) && (ichan < (int)_max_rc_input)) - input_map[ichan] = 3; - + /* FLAPS */ param_get(param_find("RC_MAP_FLAPS"), &ichan); + if ((ichan > 0) && (ichan <= (int)_max_rc_input)) { + input_map[ichan - 1] = 4; + } - if ((ichan >= 0) && (ichan < (int)_max_rc_input)) - input_map[ichan] = 4; - + /* MAIN MODE SWITCH */ param_get(param_find("RC_MAP_MODE_SW"), &ichan); - - if ((ichan >= 0) && (ichan < (int)_max_rc_input)) { + if ((ichan > 0) && (ichan <= (int)_max_rc_input)) { /* use out of normal bounds index to indicate special channel */ - input_map[ichan] = PX4IO_P_RC_CONFIG_ASSIGNMENT_MODESWITCH; + input_map[ichan - 1] = PX4IO_P_RC_CONFIG_ASSIGNMENT_MODESWITCH; } /* From c8a8305326f84cdfd92326b36241caee64b0c529 Mon Sep 17 00:00:00 2001 From: fludwig Date: Wed, 15 Oct 2014 23:48:40 +0200 Subject: [PATCH 039/172] added switch to send PWM to Brushless Controller in unarmed state --- ROMFS/px4fmu_common/init.d/3030_io_camflyer | 3 +++ 1 file changed, 3 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/3030_io_camflyer b/ROMFS/px4fmu_common/init.d/3030_io_camflyer index 83c470234c..0f0cb3a89f 100644 --- a/ROMFS/px4fmu_common/init.d/3030_io_camflyer +++ b/ROMFS/px4fmu_common/init.d/3030_io_camflyer @@ -3,3 +3,6 @@ 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_DISARMED 1000 From e474600978f0fe92de79622a746d06f2c9df77a9 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 11 Nov 2014 09:18:31 +1000 Subject: [PATCH 040/172] commander: make the failsafe message critical --- 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 f9ea4e9147..002a338119 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1852,9 +1852,9 @@ int commander_thread_main(int argc, char *argv[]) if (status.failsafe != failsafe_old) { status_changed = true; if (status.failsafe) { - mavlink_log_info(mavlink_fd, "[cmd] failsafe on"); + mavlink_log_critical(mavlink_fd, "failsafe mode on"); } else { - mavlink_log_info(mavlink_fd, "[cmd] failsafe off"); + mavlink_log_critical(mavlink_fd, "failsafe mode off"); } failsafe_old = status.failsafe; } From fa5f3658828280f54a7fec3c33ec3e25e4198561 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 11 Nov 2014 10:25:53 +1000 Subject: [PATCH 041/172] state_machine_helper: fixed comments --- src/modules/commander/state_machine_helper.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 3553dcdc3c..e1a6262627 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -520,7 +520,7 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en } else if (status->engine_failure) { status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL; - /* check for RC signal and datalink lost (with datalink enabled) after mission is finished */ + /* check for RC signal and datalink lost (with datalink enabled) */ } else if ((status->data_link_lost && data_link_loss_enabled) && status->rc_signal_lost) { status->failsafe = true; @@ -534,7 +534,7 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en status->nav_state = NAVIGATION_STATE_TERMINATION; } - /* check if RC signal is lost (with dataink disabled) after mission is finished*/ + /* check if RC signal is lost (with datalink disabled) after mission is finished */ } else if (!data_link_loss_enabled && status->rc_signal_lost && mission_finished) { status->failsafe = true; From 3c9a73f3e41a379e5e7bbf6e98d67facc3b2b49f Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 11 Nov 2014 10:28:24 +1000 Subject: [PATCH 042/172] fixed empty if body --- src/modules/commander/state_machine_helper.cpp | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index e1a6262627..a9a5c40511 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -562,11 +562,8 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en status->nav_state = NAVIGATION_STATE_TERMINATION; } - /* stay where you are */ - } else if (stay_in_failsafe){ - - /* everything is perfect */ - } else { + /* stay where you are if you should stay in failsafe, otherwise everything is perfect */ + } else if (!stay_in_failsafe){ status->nav_state = NAVIGATION_STATE_AUTO_MISSION; } break; From d98831c3b50addab0084f9636060cece46ff92bc Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 11 Nov 2014 10:42:26 +1000 Subject: [PATCH 043/172] navigator: move waypoint reached reset to a more obvious location --- src/modules/navigator/mission.cpp | 1 + src/modules/navigator/navigator_main.cpp | 2 -- 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 7fac69a611..b8e3274fdb 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -707,6 +707,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(); + reset_mission_item_reached(); } void diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index eff3a59383..e933919fb4 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -630,8 +630,6 @@ Navigator::publish_mission_result() /* advertise and publish */ _mission_result_pub = orb_advertise(ORB_ID(mission_result), &_mission_result); } - /* reset reached bool */ - _mission_result.reached = false; } void From 51ffb887c39ecfd0cb94879475f032ee65505321 Mon Sep 17 00:00:00 2001 From: Holger Steinhaus Date: Tue, 11 Nov 2014 11:13:08 +0100 Subject: [PATCH 044/172] UAVCAN: initialize device id for mag and baro to allow DEVIOCGDEVICEID ioctl to return useful data --- src/drivers/device/device.h | 3 ++- src/modules/uavcan/sensors/mag.cpp | 2 ++ src/modules/uavcan/sensors/sensor_bridge.cpp | 3 +++ src/modules/uavcan/sensors/sensor_bridge.hpp | 2 ++ 4 files changed, 9 insertions(+), 1 deletion(-) diff --git a/src/drivers/device/device.h b/src/drivers/device/device.h index 9d684e3948..67aaa0affc 100644 --- a/src/drivers/device/device.h +++ b/src/drivers/device/device.h @@ -130,7 +130,8 @@ public: enum DeviceBusType { DeviceBusType_UNKNOWN = 0, DeviceBusType_I2C = 1, - DeviceBusType_SPI = 2 + DeviceBusType_SPI = 2, + DeviceBusType_UAVCAN = 3, }; /* diff --git a/src/modules/uavcan/sensors/mag.cpp b/src/modules/uavcan/sensors/mag.cpp index 8e6a9a22f0..0d9ea08c5f 100644 --- a/src/modules/uavcan/sensors/mag.cpp +++ b/src/modules/uavcan/sensors/mag.cpp @@ -49,6 +49,8 @@ UavcanMagnetometerBridge::UavcanMagnetometerBridge(uavcan::INode& node) : UavcanCDevSensorBridgeBase("uavcan_mag", "/dev/uavcan/mag", MAG_DEVICE_PATH, MAG_TOPICS), _sub_mag(node) { + _device_id.devid_s.devtype = DRV_MAG_DEVTYPE_HMC5883; + _scale.x_scale = 1.0F; _scale.y_scale = 1.0F; _scale.z_scale = 1.0F; diff --git a/src/modules/uavcan/sensors/sensor_bridge.cpp b/src/modules/uavcan/sensors/sensor_bridge.cpp index 9608ce6804..0999938fc3 100644 --- a/src/modules/uavcan/sensors/sensor_bridge.cpp +++ b/src/modules/uavcan/sensors/sensor_bridge.cpp @@ -103,6 +103,9 @@ void UavcanCDevSensorBridgeBase::publish(const int node_id, const void *report) return; } + // update device id as we now know our device node_id + _device_id.devid_s.address = static_cast(node_id); + // Ask the CDev helper which class instance we can take const int class_instance = register_class_devname(_class_devname); if (class_instance < 0 || class_instance >= int(_max_channels)) { diff --git a/src/modules/uavcan/sensors/sensor_bridge.hpp b/src/modules/uavcan/sensors/sensor_bridge.hpp index 1316f7ecc5..04c59b77d7 100644 --- a/src/modules/uavcan/sensors/sensor_bridge.hpp +++ b/src/modules/uavcan/sensors/sensor_bridge.hpp @@ -112,6 +112,8 @@ protected: _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 = 1; // @TBD: insert UAVCAN bus no. here } /** From cf2baac516b6f86daf30342be47264b2dcc8abc4 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 11 Nov 2014 21:45:13 +1000 Subject: [PATCH 045/172] navigator: more reset reached calls --- src/modules/navigator/datalinkloss.cpp | 4 ++++ src/modules/navigator/rcloss.cpp | 2 ++ 2 files changed, 6 insertions(+) diff --git a/src/modules/navigator/datalinkloss.cpp b/src/modules/navigator/datalinkloss.cpp index 66f1c8c731..e789fd10de 100644 --- a/src/modules/navigator/datalinkloss.cpp +++ b/src/modules/navigator/datalinkloss.cpp @@ -156,6 +156,7 @@ DataLinkLoss::set_dll_item() /* Request flight termination from the commander */ _navigator->get_mission_result()->flight_termination = true; _navigator->publish_mission_result(); + reset_mission_item_reached(); warnx("not switched to manual: request flight termination"); pos_sp_triplet->previous.valid = false; pos_sp_triplet->current.valid = false; @@ -188,6 +189,7 @@ DataLinkLoss::advance_dll() 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(); + reset_mission_item_reached(); _dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP; } else { if (!_param_skipcommshold.get()) { @@ -208,6 +210,7 @@ DataLinkLoss::advance_dll() _dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP; _navigator->get_mission_result()->stay_in_failsafe = true; _navigator->publish_mission_result(); + reset_mission_item_reached(); break; case DLL_STATE_FLYTOAIRFIELDHOMEWP: _dll_state = DLL_STATE_TERMINATE; @@ -215,6 +218,7 @@ DataLinkLoss::advance_dll() mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no manual control, terminating"); _navigator->get_mission_result()->stay_in_failsafe = true; _navigator->publish_mission_result(); + reset_mission_item_reached(); break; case DLL_STATE_TERMINATE: warnx("dll end"); diff --git a/src/modules/navigator/rcloss.cpp b/src/modules/navigator/rcloss.cpp index 5564a1c42e..42392e7399 100644 --- a/src/modules/navigator/rcloss.cpp +++ b/src/modules/navigator/rcloss.cpp @@ -163,6 +163,7 @@ RCLoss::advance_rcl() _rcl_state = RCL_STATE_TERMINATE; _navigator->get_mission_result()->stay_in_failsafe = true; _navigator->publish_mission_result(); + reset_mission_item_reached(); } break; case RCL_STATE_LOITER: @@ -171,6 +172,7 @@ RCLoss::advance_rcl() mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RC not regained, terminating"); _navigator->get_mission_result()->stay_in_failsafe = true; _navigator->publish_mission_result(); + reset_mission_item_reached(); break; case RCL_STATE_TERMINATE: warnx("rcl end"); From 1394b02c2e2158b2796a51ca622c17893f78361f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 11 Nov 2014 13:45:25 +0100 Subject: [PATCH 046/172] Make tools executable --- Documentation/versionfilter.sh | 0 Tools/px_generate_xml.sh | 0 Tools/px_update_wiki.sh | 0 3 files changed, 0 insertions(+), 0 deletions(-) mode change 100644 => 100755 Documentation/versionfilter.sh mode change 100644 => 100755 Tools/px_generate_xml.sh mode change 100644 => 100755 Tools/px_update_wiki.sh diff --git a/Documentation/versionfilter.sh b/Documentation/versionfilter.sh old mode 100644 new mode 100755 diff --git a/Tools/px_generate_xml.sh b/Tools/px_generate_xml.sh old mode 100644 new mode 100755 diff --git a/Tools/px_update_wiki.sh b/Tools/px_update_wiki.sh old mode 100644 new mode 100755 From 50b410664f4087bb4fd50e8bba62267c70719b71 Mon Sep 17 00:00:00 2001 From: Holger Steinhaus Date: Tue, 11 Nov 2014 17:01:20 +0100 Subject: [PATCH 047/172] UAVCAN: set bus number part of device_id to zero --- src/modules/uavcan/sensors/sensor_bridge.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/uavcan/sensors/sensor_bridge.hpp b/src/modules/uavcan/sensors/sensor_bridge.hpp index 04c59b77d7..e319605370 100644 --- a/src/modules/uavcan/sensors/sensor_bridge.hpp +++ b/src/modules/uavcan/sensors/sensor_bridge.hpp @@ -113,7 +113,7 @@ protected: { memcpy(_orb_topics, orb_topics, sizeof(orb_id_t) * MaxChannels); _device_id.devid_s.bus_type = DeviceBusType_UAVCAN; - _device_id.devid_s.bus = 1; // @TBD: insert UAVCAN bus no. here + _device_id.devid_s.bus = 0; } /** From d63c054bb00a0df6666f498950ab797dc07817b4 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 12 Nov 2014 09:48:32 +1000 Subject: [PATCH 048/172] state_machine_helper: changed failsafe behaviour: always require at least one link for default, do RTGS as soon as datalink is lost if datalink loss mode is enabled --- .../commander/state_machine_helper.cpp | 29 ++++++------------- 1 file changed, 9 insertions(+), 20 deletions(-) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index a9a5c40511..e37019d02d 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -501,8 +501,7 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en /* go into failsafe * - if commanded to do so * - if we have an engine failure - * - if the datalink is enabled and lost as well as RC is lost - * or if the datalink is disabled and lost as well as RC is lost and the mission is finished */ + * - depending on datalink, RC and if the mission is finished */ /* first look at the commands */ if (status->engine_failure_cmd) { @@ -520,8 +519,9 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en } else if (status->engine_failure) { status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL; - /* check for RC signal and datalink lost (with datalink enabled) */ - } else if ((status->data_link_lost && data_link_loss_enabled) && status->rc_signal_lost) { + /* datalink loss enabled: + * check for datalink lost: this should always trigger RTGS */ + } else if (data_link_loss_enabled && status->data_link_lost) { status->failsafe = true; if (status->condition_global_position_valid && status->condition_home_position_valid) { @@ -534,8 +534,11 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en status->nav_state = NAVIGATION_STATE_TERMINATION; } - /* check if RC signal is lost (with datalink disabled) after mission is finished */ - } else if (!data_link_loss_enabled && status->rc_signal_lost && mission_finished) { + /* datalink loss disabled: + * check if both, RC and datalink are lost during the mission + * or RC is lost after the mission is finished: this should always trigger RCRECOVER */ + } else if (!data_link_loss_enabled && ((status->rc_signal_lost && status->data_link_lost) || + (status->rc_signal_lost && mission_finished))) { status->failsafe = true; if (status->condition_global_position_valid && status->condition_home_position_valid) { @@ -548,20 +551,6 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en status->nav_state = NAVIGATION_STATE_TERMINATION; } - /* also go into failsafe if just datalink is lost (with datalink enabled) */ - } else if (status->data_link_lost && data_link_loss_enabled) { - status->failsafe = true; - - if (status->condition_global_position_valid && status->condition_home_position_valid) { - status->nav_state = NAVIGATION_STATE_AUTO_RTGS; - } else if (status->condition_local_position_valid) { - status->nav_state = NAVIGATION_STATE_LAND; - } else if (status->condition_local_altitude_valid) { - status->nav_state = NAVIGATION_STATE_DESCEND; - } else { - status->nav_state = NAVIGATION_STATE_TERMINATION; - } - /* stay where you are if you should stay in failsafe, otherwise everything is perfect */ } else if (!stay_in_failsafe){ status->nav_state = NAVIGATION_STATE_AUTO_MISSION; From 20703c8d2352dd137ec13be69e42fe6fdfd9f8f2 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 12 Nov 2014 11:05:06 +1000 Subject: [PATCH 049/172] manual_control_setpoint: whitespace --- src/modules/uORB/topics/manual_control_setpoint.h | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/modules/uORB/topics/manual_control_setpoint.h b/src/modules/uORB/topics/manual_control_setpoint.h index dde237adc3..c0c33a6298 100644 --- a/src/modules/uORB/topics/manual_control_setpoint.h +++ b/src/modules/uORB/topics/manual_control_setpoint.h @@ -47,7 +47,7 @@ * Switch position */ typedef enum { - SWITCH_POS_NONE = 0, /**< switch is not mapped */ + SWITCH_POS_NONE = 0, /**< switch is not mapped */ SWITCH_POS_ON, /**< switch activated (value = 1) */ SWITCH_POS_MIDDLE, /**< middle position (value = 0) */ SWITCH_POS_OFF /**< switch not activated (value = -1) */ @@ -93,12 +93,12 @@ struct manual_control_setpoint_s { float aux4; /**< default function: camera roll */ float aux5; /**< default function: payload drop */ - switch_pos_t mode_switch; /**< main mode 3 position switch (mandatory): _MANUAL_, ASSIST, AUTO */ - switch_pos_t return_switch; /**< return to launch 2 position switch (mandatory): _NORMAL_, RTL */ - switch_pos_t posctl_switch; /**< position control 2 position switch (optional): _ALTCTL_, POSCTL */ - switch_pos_t loiter_switch; /**< loiter 2 position switch (optional): _MISSION_, LOITER */ - switch_pos_t acro_switch; /**< acro 2 position switch (optional): _MANUAL_, ACRO */ - switch_pos_t offboard_switch; /**< offboard 2 position switch (optional): _NORMAL_, OFFBOARD */ + switch_pos_t mode_switch; /**< main mode 3 position switch (mandatory): _MANUAL_, ASSIST, AUTO */ + switch_pos_t return_switch; /**< return to launch 2 position switch (mandatory): _NORMAL_, RTL */ + switch_pos_t posctl_switch; /**< position control 2 position switch (optional): _ALTCTL_, POSCTL */ + switch_pos_t loiter_switch; /**< loiter 2 position switch (optional): _MISSION_, LOITER */ + switch_pos_t acro_switch; /**< acro 2 position switch (optional): _MANUAL_, ACRO */ + switch_pos_t offboard_switch; /**< offboard 2 position switch (optional): _NORMAL_, OFFBOARD */ }; /**< manual control inputs */ /** From b1c6692f2028b616581efaf7a320a7cf7072a02f Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 12 Nov 2014 11:06:25 +1000 Subject: [PATCH 050/172] commander: removed unused definition --- src/modules/commander/commander.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 46caddd469..149c0f60e4 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -220,8 +220,6 @@ void control_status_leds(vehicle_status_s *status, const actuator_armed_s *actua void check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *valid_out, bool *changed); -void check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *status); - transition_result_t set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoint_s *sp_man); void set_control_mode(); From f1058bdf0816cda718cee4d5a69c4ac3d53ea781 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 12 Nov 2014 11:07:27 +1000 Subject: [PATCH 051/172] manual_control_setpoint: wrong comment --- src/modules/uORB/topics/manual_control_setpoint.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/uORB/topics/manual_control_setpoint.h b/src/modules/uORB/topics/manual_control_setpoint.h index c0c33a6298..50b7bd9e59 100644 --- a/src/modules/uORB/topics/manual_control_setpoint.h +++ b/src/modules/uORB/topics/manual_control_setpoint.h @@ -99,7 +99,7 @@ struct manual_control_setpoint_s { switch_pos_t loiter_switch; /**< loiter 2 position switch (optional): _MISSION_, LOITER */ switch_pos_t acro_switch; /**< acro 2 position switch (optional): _MANUAL_, ACRO */ switch_pos_t offboard_switch; /**< offboard 2 position switch (optional): _NORMAL_, OFFBOARD */ -}; /**< manual control inputs */ +}; /** * @} From 703fcec62d3430b75876d2c06183bbf4451f0dd6 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 12 Nov 2014 12:11:23 +1000 Subject: [PATCH 052/172] navigator: fix do jumps: after the repetitions are completed the current mission index is no longer invalid --- src/modules/navigator/mission.cpp | 21 ++++++++++++--------- 1 file changed, 12 insertions(+), 9 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 7fac69a611..840c9ec0e7 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -371,7 +371,7 @@ Mission::set_mission_items() } else { /* no mission available or mission finished, switch to loiter */ if (_mission_type != MISSION_TYPE_NONE) { - mavlink_log_critical(_navigator->get_mavlink_fd(), "mission finished, loitering"); + mavlink_log_critical(_navigator->get_mavlink_fd(), "mission finished"); /* use last setpoint for loiter */ _navigator->set_can_loiter_at_sp(true); @@ -595,13 +595,14 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s dm_item = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id); } - if (*mission_index_ptr < 0 || *mission_index_ptr >= (int)mission->count) { - /* mission item index out of bounds */ - return false; - } - /* repeat several to get the mission item because we might have to follow multiple DO_JUMPS */ for (int i = 0; i < 10; i++) { + + if (*mission_index_ptr < 0 || *mission_index_ptr >= (int)mission->count) { + /* mission item index out of bounds */ + return false; + } + const ssize_t len = sizeof(struct mission_item_s); /* read mission item to temp storage first to not overwrite current mission item if data damaged */ @@ -630,7 +631,7 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s /* not supposed to happen unless the datamanager can't access the * dataman */ mavlink_log_critical(_navigator->get_mavlink_fd(), - "ERROR DO JUMP waypoint could not be written"); + "ERROR DO JUMP waypoint could not be written"); return false; } } @@ -639,8 +640,10 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s *mission_index_ptr = mission_item_tmp.do_jump_mission_index; } else { - mavlink_log_critical(_navigator->get_mavlink_fd(), - "DO JUMP repetitions completed"); + if (is_current) { + mavlink_log_critical(_navigator->get_mavlink_fd(), + "DO JUMP repetitions completed"); + } /* no more DO_JUMPS, therefore just try to continue with next mission item */ (*mission_index_ptr)++; } From 08443205e90955c13532c79aa02a283a50cce25d Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 12 Nov 2014 13:07:06 +1000 Subject: [PATCH 053/172] navigator: make DO_JUMPS survive power on resets --- src/modules/navigator/mission.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 840c9ec0e7..cfe85c84e2 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -627,7 +627,8 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s if (is_current) { (mission_item_tmp.do_jump_current_count)++; /* save repeat count */ - if (dm_write(dm_item, *mission_index_ptr, DM_PERSIST_IN_FLIGHT_RESET, &mission_item_tmp, len) != len) { + if (dm_write(dm_item, *mission_index_ptr, DM_PERSIST_POWER_ON_RESET, + &mission_item_tmp, len) != len) { /* not supposed to happen unless the datamanager can't access the * dataman */ mavlink_log_critical(_navigator->get_mavlink_fd(), From 14b5732c6eff6998d013c0816e983320906c462d Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 12 Nov 2014 13:26:10 +1000 Subject: [PATCH 054/172] px4flow: small verbose output fix --- src/drivers/px4flow/px4flow.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp index 6c68636c6d..9e1ecb61e8 100644 --- a/src/drivers/px4flow/px4flow.cpp +++ b/src/drivers/px4flow/px4flow.cpp @@ -76,7 +76,7 @@ #define PX4FLOW_BUS 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 */ @@ -443,7 +443,7 @@ PX4FLOW::measure() 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", ret); return ret; } From 7bc9a6257731a741ff309f060f6cf87c33c4a266 Mon Sep 17 00:00:00 2001 From: Holger Steinhaus Date: Wed, 12 Nov 2014 09:52:35 +0100 Subject: [PATCH 055/172] code style, warnings --- src/modules/uavcan/actuators/esc.cpp | 13 +++++++------ src/modules/uavcan/uavcan_main.cpp | 9 ++++----- src/systemcmds/motor_test/motor_test.c | 19 ++++++++----------- 3 files changed, 19 insertions(+), 22 deletions(-) diff --git a/src/modules/uavcan/actuators/esc.cpp b/src/modules/uavcan/actuators/esc.cpp index fbd4f0bcd4..1d23099f3d 100644 --- a/src/modules/uavcan/actuators/esc.cpp +++ b/src/modules/uavcan/actuators/esc.cpp @@ -113,8 +113,7 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs) msg.cmd.push_back(static_cast(scaled)); _esc_status.esc[i].esc_setpoint_raw = abs(static_cast(scaled)); - } - else { + } else { msg.cmd.push_back(static_cast(0)); } } @@ -128,18 +127,20 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs) void UavcanEscController::arm_all_escs(bool arm) { - if (arm) + if (arm) { _armed_mask = -1; - else + } else { _armed_mask = 0; + } } void UavcanEscController::arm_single_esc(int num, bool arm) { - if (arm) + if (arm) { _armed_mask = MOTOR_BIT(num); - else + } else { _armed_mask = 0; + } } void UavcanEscController::esc_status_sub_cb(const uavcan::ReceivedDataStructure &msg) diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 653d4f98cf..2c543462ec 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -346,13 +346,12 @@ int UavcanNode::run() // can we mix? if (_test_in_progress) { - float outputs[NUM_ACTUATOR_OUTPUTS] = {}; - outputs[_test_motor.motor_number] = _test_motor.value*2.0f-1.0f; + 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(outputs, NUM_ACTUATOR_OUTPUTS); - } - else if (controls_updated && (_mixers != nullptr)) { + _esc_controller.update_outputs(test_outputs, NUM_ACTUATOR_OUTPUTS); + } else if (controls_updated && (_mixers != nullptr)) { // XXX one output group has 8 outputs max, // but this driver could well serve multiple groups. diff --git a/src/systemcmds/motor_test/motor_test.c b/src/systemcmds/motor_test/motor_test.c index 079f99674a..087699b05d 100644 --- a/src/systemcmds/motor_test/motor_test.c +++ b/src/systemcmds/motor_test/motor_test.c @@ -67,19 +67,15 @@ void motor_test(unsigned channel, float value) _test_motor.timestamp = hrt_absolute_time(); _test_motor.value = value; - if (_test_motor_pub > 0) { - /* publish armed state */ - orb_publish(ORB_ID(test_motor), _test_motor_pub, &_test_motor); - } else { - /* advertise and publish */ - _test_motor_pub = orb_advertise(ORB_ID(test_motor), &_test_motor); - } + _test_motor_pub = orb_advertise(ORB_ID(test_motor), &_test_motor); + orb_publish(ORB_ID(test_motor), _test_motor_pub, &_test_motor); } static void usage(const char *reason) { - if (reason != NULL) + if (reason != NULL) { warnx("%s", reason); + } errx(1, "usage:\n" @@ -90,8 +86,9 @@ static void usage(const char *reason) int motor_test_main(int argc, char *argv[]) { - unsigned long channel, lval; - float value; + unsigned long channel = 0; + unsigned long lval; + float value = 0.0f; int ch; if (argc != 5) { @@ -122,7 +119,7 @@ int motor_test_main(int argc, char *argv[]) motor_test(channel, value); - printf("motor %d set to %.2f\n", channel, value); + printf("motor %d set to %.2f\n", channel, (double)value); exit(0); } From 64de403b5f2aa4af57918510451373424c162b68 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 12 Nov 2014 20:06:49 +1000 Subject: [PATCH 056/172] navigator: trying to improve a comment --- src/modules/navigator/mission.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index cfe85c84e2..d0611b9e58 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -595,7 +595,8 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s dm_item = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id); } - /* repeat several to get the mission item because we might have to follow multiple DO_JUMPS */ + /* Repeat this several times in case there are several DO JUMPS that we need to follow along, however, after + * 10 iterations we have to assume that the DO JUMPS are probably cycling and give up. */ for (int i = 0; i < 10; i++) { if (*mission_index_ptr < 0 || *mission_index_ptr >= (int)mission->count) { From ae99a179ac0fd2b7a240d51adf7d992ff91ed598 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 12 Nov 2014 12:21:15 +0100 Subject: [PATCH 057/172] Include NuttX sscanf() fix --- NuttX | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/NuttX b/NuttX index 5ee4b2b2c2..47a94a0979 160000 --- a/NuttX +++ b/NuttX @@ -1 +1 @@ -Subproject commit 5ee4b2b2c26bbc35d1669840f0676e8aa383b984 +Subproject commit 47a94a0979fd5aee2c8416e0197d6ce5926e94ca From d7ebea6ec2f3783bfdd4becd7cda5fd24c340a85 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 12 Nov 2014 09:01:00 +0100 Subject: [PATCH 058/172] Fix comparison in upload script for test builds --- Tools/upload.sh | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/Tools/upload.sh b/Tools/upload.sh index 17d87fe610..c0dd65eba1 100755 --- a/Tools/upload.sh +++ b/Tools/upload.sh @@ -10,17 +10,17 @@ SYSTYPE=`uname -s` # # XXX The uploader should be smarter than this. # -if [ $SYSTYPE=Darwin ]; +if [ $SYSTYPE = "Darwin" ]; then SERIAL_PORTS="/dev/tty.usbmodemPX*,/dev/tty.usbmodem*" fi -if [ $SYSTYPE=Linux ]; +if [ $SYSTYPE = "Linux" ]; then SERIAL_PORTS="/dev/serial/by-id/usb-3D_Robotics*" fi -if [ $SYSTYPE="" ]; +if [ $SYSTYPE = "" ]; then SERIAL_PORTS="COM32,COM31,COM30,COM29,COM28,COM27,COM26,COM25,COM24,COM23,COM22,COM21,COM20,COM19,COM18,COM17,COM16,COM15,COM14,COM13,COM12,COM11,COM10,COM9,COM8,COM7,COM6,COM5,COM4,COM3,COM2,COM1,COM0" fi From afcbab7fba84cf3d3d016a63fbca405ade15889a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 12 Nov 2014 13:37:58 +0100 Subject: [PATCH 059/172] PX4FLOW driver: Prevent sensor from spamming the system on errors. Use the perf system call to look at error counters. --- src/drivers/px4flow/px4flow.cpp | 12 +++++------- 1 file changed, 5 insertions(+), 7 deletions(-) diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp index 9e1ecb61e8..804027b056 100644 --- a/src/drivers/px4flow/px4flow.cpp +++ b/src/drivers/px4flow/px4flow.cpp @@ -200,7 +200,7 @@ PX4FLOW::PX4FLOW(int bus, int address) : _buffer_overflows(perf_alloc(PC_COUNT, "px4flow_buffer_overflows")) { // enable debug() calls - _debug_enabled = true; + _debug_enabled = false; // work_cancel in the dtor will explode if we don't do this... memset(&_work, 0, sizeof(_work)); @@ -240,7 +240,7 @@ PX4FLOW::init() _px4flow_topic = orb_advertise(ORB_ID(optical_flow), &zero_report); if (_px4flow_topic < 0) { - debug("failed to create px4flow object. Did you start uOrb?"); + warnx("failed to create px4flow object. Did you start uOrb?"); } ret = OK; @@ -442,8 +442,6 @@ PX4FLOW::measure() if (OK != ret) { perf_count(_comms_errors); - log("i2c::transfer returned %d", ret); - printf("i2c::transfer flow returned %d", ret); return ret; } @@ -465,7 +463,7 @@ PX4FLOW::collect() ret = transfer(nullptr, 0, &val[0], 22); 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; @@ -565,7 +563,7 @@ PX4FLOW::cycle() /* perform collection */ if (OK != collect()) { - log("collection error"); + debug("collection error"); /* restart the measurement state machine */ start(); return; @@ -592,7 +590,7 @@ PX4FLOW::cycle() /* measurement phase */ if (OK != measure()) { - log("measure error"); + debug("measure error"); } /* next phase is collection */ From 826fd3f4cdf492be1e5b05f7da4493ff10118876 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 13 Nov 2014 09:43:51 +1000 Subject: [PATCH 060/172] commander: reordering navigation states to match enum --- src/modules/commander/commander.cpp | 153 +++++++++++++++------------- 1 file changed, 83 insertions(+), 70 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 7917bc9f25..3307c4bfcc 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -2201,6 +2201,58 @@ set_control_mode() 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; + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = true; + control_mode.flag_control_altitude_enabled = true; + control_mode.flag_control_climb_rate_enabled = true; + control_mode.flag_control_position_enabled = false; + control_mode.flag_control_velocity_enabled = false; + control_mode.flag_control_termination_enabled = false; + break; + + case NAVIGATION_STATE_POSCTL: + control_mode.flag_control_manual_enabled = true; + control_mode.flag_control_auto_enabled = false; + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = true; + control_mode.flag_control_altitude_enabled = true; + control_mode.flag_control_climb_rate_enabled = true; + control_mode.flag_control_position_enabled = true; + control_mode.flag_control_velocity_enabled = true; + control_mode.flag_control_termination_enabled = false; + break; + + case NAVIGATION_STATE_AUTO_MISSION: + case NAVIGATION_STATE_AUTO_LOITER: + case NAVIGATION_STATE_AUTO_RTL: + case NAVIGATION_STATE_AUTO_RTGS: + case NAVIGATION_STATE_AUTO_LANDENGFAIL: + control_mode.flag_control_manual_enabled = false; + control_mode.flag_control_auto_enabled = true; + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = true; + control_mode.flag_control_altitude_enabled = true; + control_mode.flag_control_climb_rate_enabled = true; + control_mode.flag_control_position_enabled = true; + control_mode.flag_control_velocity_enabled = true; + control_mode.flag_control_termination_enabled = false; + break; + + case NAVIGATION_STATE_AUTO_LANDGPSFAIL: + control_mode.flag_control_manual_enabled = false; + control_mode.flag_control_auto_enabled = false; + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = true; + control_mode.flag_control_altitude_enabled = false; + control_mode.flag_control_climb_rate_enabled = true; + control_mode.flag_control_position_enabled = false; + control_mode.flag_control_velocity_enabled = false; + control_mode.flag_control_termination_enabled = false; + break; + case NAVIGATION_STATE_ACRO: control_mode.flag_control_manual_enabled = true; control_mode.flag_control_auto_enabled = false; @@ -2213,18 +2265,46 @@ set_control_mode() 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; + + case NAVIGATION_STATE_LAND: + control_mode.flag_control_manual_enabled = false; + control_mode.flag_control_auto_enabled = true; control_mode.flag_control_rates_enabled = true; control_mode.flag_control_attitude_enabled = true; + /* in failsafe LAND mode position may be not available */ + control_mode.flag_control_position_enabled = status.condition_local_position_valid; + control_mode.flag_control_velocity_enabled = status.condition_local_position_valid; control_mode.flag_control_altitude_enabled = true; control_mode.flag_control_climb_rate_enabled = true; + control_mode.flag_control_termination_enabled = false; + break; + + case NAVIGATION_STATE_DESCEND: + /* TODO: check if this makes sense */ + control_mode.flag_control_manual_enabled = false; + control_mode.flag_control_auto_enabled = true; + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = true; control_mode.flag_control_position_enabled = false; control_mode.flag_control_velocity_enabled = false; + control_mode.flag_control_altitude_enabled = false; + control_mode.flag_control_climb_rate_enabled = true; control_mode.flag_control_termination_enabled = false; break; + case NAVIGATION_STATE_TERMINATION: + /* disable all controllers on termination */ + control_mode.flag_control_manual_enabled = false; + control_mode.flag_control_auto_enabled = false; + control_mode.flag_control_rates_enabled = false; + control_mode.flag_control_attitude_enabled = false; + control_mode.flag_control_position_enabled = false; + control_mode.flag_control_velocity_enabled = false; + control_mode.flag_control_altitude_enabled = false; + control_mode.flag_control_climb_rate_enabled = false; + control_mode.flag_control_termination_enabled = true; + break; + case NAVIGATION_STATE_OFFBOARD: control_mode.flag_control_manual_enabled = false; control_mode.flag_control_auto_enabled = false; @@ -2280,73 +2360,6 @@ set_control_mode() control_mode.flag_control_position_enabled = false; control_mode.flag_control_velocity_enabled = false; } - - break; - - case NAVIGATION_STATE_POSCTL: - control_mode.flag_control_manual_enabled = true; - control_mode.flag_control_auto_enabled = false; - control_mode.flag_control_rates_enabled = true; - control_mode.flag_control_attitude_enabled = true; - control_mode.flag_control_altitude_enabled = true; - control_mode.flag_control_climb_rate_enabled = true; - control_mode.flag_control_position_enabled = true; - control_mode.flag_control_velocity_enabled = true; - control_mode.flag_control_termination_enabled = false; - break; - - case NAVIGATION_STATE_AUTO_MISSION: - case NAVIGATION_STATE_AUTO_LOITER: - case NAVIGATION_STATE_AUTO_RTL: - case NAVIGATION_STATE_AUTO_RTGS: - case NAVIGATION_STATE_AUTO_LANDENGFAIL: - control_mode.flag_control_manual_enabled = false; - control_mode.flag_control_auto_enabled = true; - control_mode.flag_control_rates_enabled = true; - control_mode.flag_control_attitude_enabled = true; - control_mode.flag_control_altitude_enabled = true; - control_mode.flag_control_climb_rate_enabled = true; - control_mode.flag_control_position_enabled = true; - control_mode.flag_control_velocity_enabled = true; - control_mode.flag_control_termination_enabled = false; - break; - - case NAVIGATION_STATE_AUTO_LANDGPSFAIL: - control_mode.flag_control_manual_enabled = false; - control_mode.flag_control_auto_enabled = false; - control_mode.flag_control_rates_enabled = true; - control_mode.flag_control_attitude_enabled = true; - control_mode.flag_control_altitude_enabled = false; - control_mode.flag_control_climb_rate_enabled = true; - control_mode.flag_control_position_enabled = false; - control_mode.flag_control_velocity_enabled = false; - control_mode.flag_control_termination_enabled = false; - break; - - case NAVIGATION_STATE_LAND: - control_mode.flag_control_manual_enabled = false; - control_mode.flag_control_auto_enabled = true; - control_mode.flag_control_rates_enabled = true; - control_mode.flag_control_attitude_enabled = true; - /* in failsafe LAND mode position may be not available */ - control_mode.flag_control_position_enabled = status.condition_local_position_valid; - control_mode.flag_control_velocity_enabled = status.condition_local_position_valid; - control_mode.flag_control_altitude_enabled = true; - control_mode.flag_control_climb_rate_enabled = true; - control_mode.flag_control_termination_enabled = false; - break; - - case NAVIGATION_STATE_TERMINATION: - /* disable all controllers on termination */ - control_mode.flag_control_manual_enabled = false; - control_mode.flag_control_auto_enabled = false; - control_mode.flag_control_rates_enabled = false; - control_mode.flag_control_attitude_enabled = false; - control_mode.flag_control_position_enabled = false; - control_mode.flag_control_velocity_enabled = false; - control_mode.flag_control_altitude_enabled = false; - control_mode.flag_control_climb_rate_enabled = false; - control_mode.flag_control_termination_enabled = true; break; default: From 00cc2d55111fe6dcf03fc847aa6e65204e4a0a2c Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 13 Nov 2014 09:44:17 +1000 Subject: [PATCH 061/172] commander: added navstate RCRECOVER which was missing --- src/modules/commander/commander.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 3307c4bfcc..5c4ad26f47 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -2225,9 +2225,11 @@ set_control_mode() control_mode.flag_control_termination_enabled = false; break; + case NAVIGATION_STATE_AUTO_MISSION: case NAVIGATION_STATE_AUTO_LOITER: case NAVIGATION_STATE_AUTO_RTL: + case NAVIGATION_STATE_AUTO_RCRECOVER: case NAVIGATION_STATE_AUTO_RTGS: case NAVIGATION_STATE_AUTO_LANDENGFAIL: control_mode.flag_control_manual_enabled = false; From db3f2fc94621ee13c7ec22d5fef1dd5ae74a0742 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 13 Nov 2014 09:46:12 +1000 Subject: [PATCH 062/172] commander: whitespace --- src/modules/commander/commander.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 5c4ad26f47..ec173c12be 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -2225,7 +2225,6 @@ set_control_mode() control_mode.flag_control_termination_enabled = false; break; - case NAVIGATION_STATE_AUTO_MISSION: case NAVIGATION_STATE_AUTO_LOITER: case NAVIGATION_STATE_AUTO_RTL: From 6a543b1d0154d7da9ddb4123df1479cf01aa88bc Mon Sep 17 00:00:00 2001 From: Holger Steinhaus Date: Thu, 13 Nov 2014 10:45:32 +0100 Subject: [PATCH 063/172] build fix --- src/modules/uORB/topics/test_motor.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/uORB/topics/test_motor.h b/src/modules/uORB/topics/test_motor.h index c880fe099b..2dd90e69da 100644 --- a/src/modules/uORB/topics/test_motor.h +++ b/src/modules/uORB/topics/test_motor.h @@ -57,7 +57,7 @@ struct test_motor_s { uint64_t timestamp; /**< output timestamp in us since system boot */ unsigned motor_number; /**< number of motor to spin */ - float value; /**< output power, range [0..1] + float value; /**< output power, range [0..1] */ }; /** From 4d489ef7f4c7626a86b700b6d7144e35000ce387 Mon Sep 17 00:00:00 2001 From: Holger Steinhaus Date: Tue, 11 Nov 2014 11:19:15 +0100 Subject: [PATCH 064/172] 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 065/172] 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 066/172] 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 067/172] 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: Sat, 15 Nov 2014 11:51:21 +0100 Subject: [PATCH 068/172] Updated NuttX, only comment changes --- NuttX | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/NuttX b/NuttX index 47a94a0979..9d06b64579 160000 --- a/NuttX +++ b/NuttX @@ -1 +1 @@ -Subproject commit 47a94a0979fd5aee2c8416e0197d6ce5926e94ca +Subproject commit 9d06b645790e1445f14e3b19c71d40b3088f4e4f From 780451dd65db72aa28b75ddad2effbbba8db8626 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 15 Nov 2014 13:48:30 +0100 Subject: [PATCH 069/172] Remove unneeded file --- ROMFS/px4fmu_common/init.d/rc.jig | 10 ---------- 1 file changed, 10 deletions(-) delete mode 100644 ROMFS/px4fmu_common/init.d/rc.jig diff --git a/ROMFS/px4fmu_common/init.d/rc.jig b/ROMFS/px4fmu_common/init.d/rc.jig deleted file mode 100644 index e2b5d8f30d..0000000000 --- a/ROMFS/px4fmu_common/init.d/rc.jig +++ /dev/null @@ -1,10 +0,0 @@ -#!nsh -# -# Test jig startup script -# - -echo "[testing] doing production test.." - -tests jig - -echo "[testing] testing done" From 4bcb2697415009279bd481eb9d22f015846147f1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 15 Nov 2014 13:48:49 +0100 Subject: [PATCH 070/172] Add capability to read chip ID and revision from ver command --- src/systemcmds/ver/ver.c | 85 +++++++++++++++++++++++++++++++++++++++- 1 file changed, 84 insertions(+), 1 deletion(-) diff --git a/src/systemcmds/ver/ver.c b/src/systemcmds/ver/ver.c index 9ae080ee21..b11b593c38 100644 --- a/src/systemcmds/ver/ver.c +++ b/src/systemcmds/ver/ver.c @@ -53,13 +53,38 @@ static const char sz_ver_bdate_str[] = "bdate"; static const char sz_ver_gcc_str[] = "gcc"; static const char sz_ver_all_str[] = "all"; +#ifdef CONFIG_ARCH_CHIP_STM32 +#include + +static const char mcu_ver_str[] = "mcu"; + +#define DBGMCU_IDCODE 0xE0042000 + +#define STM32F40x_41x 0x413 +#define STM32F42x_43x 0x419 + +#define REVID_MASK 0xFFFF0000 +#define DEVID_MASK 0xFFF + +/* magic numbers from reference manual */ +enum STM32F4_REV { + STM32F4_REV_A = 0x1000, + STM32F4_REV_Z = 0x1001, + STM32F4_REV_Y = 0x1003, + STM32F4_REV_1 = 0x1007, + STM32F4_REV_3 = 0x2001 +}; +#else +#error stm32 +#endif + static void usage(const char *reason) { if (reason != NULL) { printf("%s\n", reason); } - printf("usage: ver {hw|hwcmp|git|bdate|gcc|all}\n\n"); + printf("usage: ver {hw|hwcmp|git|bdate|gcc|all|mcu}\n\n"); } __EXPORT int ver_main(int argc, char *argv[]); @@ -107,6 +132,64 @@ int ver_main(int argc, char *argv[]) printf("GCC toolchain: %s\n", __VERSION__); ret = 0; +#ifdef CONFIG_ARCH_CHIP_STM32 + } else if (!strncmp(argv[1], mcu_ver_str, sizeof(mcu_ver_str))) { + uint32_t abc = getreg32(DBGMCU_IDCODE); + + uint32_t chip_version = abc & DEVID_MASK; + enum STM32F4_REV revid = (abc & REVID_MASK) >> 16; + + printf("CHIP TYPE: "); + + switch (revid) { + case STM32F40x_41x: + printf("STM32F40x"); + break; + case STM32F42x_43x: + printf("STM32F42x"); + break; + default: + printf("STM32F???"); + break; + } + + char rev; + + switch (chip_version) { + + case STM32F4_REV_A: + rev = 'A'; + break; + case STM32F4_REV_Z: + rev = 'Z'; + break; + case STM32F4_REV_Y: + rev = 'Y'; + break; + case STM32F4_REV_1: + rev = '1'; + break; + case STM32F4_REV_3: + rev = '3'; + break; + default: + rev = '?'; + break; + } + + printf("\nHW REV: %c\n", rev); + + if (rev < STM32F4_REV_3) { + printf("\n\nWARNING WARNING WARNING!\n" + "Revision %c has a silicon errata\n" + "on USB connectivity combined with\n" + "flash bank #2. This device can only\n" + "utilize a maximum of 1MB flash safely!\n" + "http://px4.io/help/errata\n", rev); + } + ret = 0; +#endif + } else { errx(1, "unknown command.\n"); } From 8e8dd62fbd9906c980d9e88943a2fda5b90977e0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 15 Nov 2014 13:49:06 +0100 Subject: [PATCH 071/172] Let the uploader print the binary size --- Tools/px_uploader.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/px_uploader.py b/Tools/px_uploader.py index d4e4612262..3a4540ac06 100755 --- a/Tools/px_uploader.py +++ b/Tools/px_uploader.py @@ -458,7 +458,7 @@ if os.path.exists("/usr/sbin/ModemManager"): # Load the firmware file fw = firmware(args.firmware) -print("Loaded firmware for %x,%x, waiting for the bootloader..." % (fw.property('board_id'), fw.property('board_revision'))) +print("Loaded firmware for %x,%x, size: %d bytes, waiting for the bootloader..." % (fw.property('board_id'), fw.property('board_revision'), fw.property('image_size'))) print("If the board does not respond within 1-2 seconds, unplug and re-plug the USB connector.") # Spin waiting for a device to show up From a7eaf07f4f9d6df54b7602c07ce7fbdf2db700f9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 15 Nov 2014 13:49:48 +0100 Subject: [PATCH 072/172] Motor test: optimize for size --- src/systemcmds/motor_test/module.mk | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/systemcmds/motor_test/module.mk b/src/systemcmds/motor_test/module.mk index eb36d2ded8..261428ef94 100644 --- a/src/systemcmds/motor_test/module.mk +++ b/src/systemcmds/motor_test/module.mk @@ -39,3 +39,5 @@ MODULE_COMMAND = motor_test SRCS = motor_test.c MODULE_STACKSIZE = 4096 + +MAXOPTIMIZATION = -Os From d4a5f345aabc83506b05277b9dcf71e24700c70d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 15 Nov 2014 13:50:38 +0100 Subject: [PATCH 073/172] Remove unneeded apps from build --- makefiles/config_px4fmu-v2_default.mk | 17 +---------------- makefiles/config_px4fmu-v2_test.mk | 7 +++++++ 2 files changed, 8 insertions(+), 16 deletions(-) diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index d17dff5bb0..3c65b19e02 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -40,14 +40,8 @@ MODULES += drivers/meas_airspeed MODULES += drivers/frsky_telemetry MODULES += modules/sensors MODULES += drivers/mkblctrl -MODULES += drivers/pca8574 MODULES += drivers/px4flow - -# Needs to be burned to the ground and re-written; for now, -# just don't build it. -#MODULES += drivers/mkblctrl - # # System commands # @@ -61,7 +55,6 @@ MODULES += systemcmds/pwm MODULES += systemcmds/esc_calib MODULES += systemcmds/reboot MODULES += systemcmds/top -MODULES += systemcmds/tests MODULES += systemcmds/config MODULES += systemcmds/nshterm MODULES += systemcmds/mtd @@ -81,10 +74,8 @@ MODULES += modules/uavcan # Estimation modules (EKF/ SO3 / other filters) # MODULES += modules/attitude_estimator_ekf -MODULES += modules/attitude_estimator_so3 MODULES += modules/ekf_att_pos_estimator MODULES += modules/position_estimator_inav -MODULES += examples/flow_position_estimator # # Vehicle Control @@ -100,12 +91,6 @@ MODULES += modules/mc_pos_control # MODULES += modules/sdlog2 -# -# Unit tests -# -#MODULES += modules/unit_test -#MODULES += modules/commander/commander_tests - # # Library modules # @@ -139,7 +124,7 @@ MODULES += modules/bottle_drop #MODULES += examples/math_demo # Tutorial code from # https://pixhawk.ethz.ch/px4/dev/hello_sky -MODULES += examples/px4_simple_app +#MODULES += examples/px4_simple_app # Tutorial code from # https://pixhawk.ethz.ch/px4/dev/daemon diff --git a/makefiles/config_px4fmu-v2_test.mk b/makefiles/config_px4fmu-v2_test.mk index 2f4d9d6a47..6f54b960c1 100644 --- a/makefiles/config_px4fmu-v2_test.mk +++ b/makefiles/config_px4fmu-v2_test.mk @@ -49,6 +49,13 @@ MODULES += lib/mathlib MODULES += lib/mathlib/math/filter MODULES += lib/conversion +# +# Modules to test-build, but not useful for test environment +# +MODULES += modules/attitude_estimator_so3 +MODULES += drivers/pca8574 +MODULES += examples/flow_position_estimator + # # Libraries # From 75bc8136b1d1d773a5664a226b5b766867fe5ff3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 15 Nov 2014 13:50:56 +0100 Subject: [PATCH 074/172] Build NuttX optimized for size --- nuttx-configs/px4fmu-v1/nsh/Make.defs | 2 +- nuttx-configs/px4fmu-v2/nsh/Make.defs | 2 +- nuttx-configs/px4fmu-v2/scripts/ld.script | 3 ++- nuttx-configs/px4io-v1/nsh/Make.defs | 2 +- nuttx-configs/px4io-v2/nsh/Make.defs | 6 +----- 5 files changed, 6 insertions(+), 9 deletions(-) diff --git a/nuttx-configs/px4fmu-v1/nsh/Make.defs b/nuttx-configs/px4fmu-v1/nsh/Make.defs index 7b2ea703a2..e7318e5196 100644 --- a/nuttx-configs/px4fmu-v1/nsh/Make.defs +++ b/nuttx-configs/px4fmu-v1/nsh/Make.defs @@ -53,7 +53,7 @@ NM = $(CROSSDEV)nm OBJCOPY = $(CROSSDEV)objcopy OBJDUMP = $(CROSSDEV)objdump -MAXOPTIMIZATION = -O3 +MAXOPTIMIZATION = -Os ARCHCPUFLAGS = -mcpu=cortex-m4 \ -mthumb \ -march=armv7e-m \ diff --git a/nuttx-configs/px4fmu-v2/nsh/Make.defs b/nuttx-configs/px4fmu-v2/nsh/Make.defs index e70320aaa4..f3ce53b4ae 100644 --- a/nuttx-configs/px4fmu-v2/nsh/Make.defs +++ b/nuttx-configs/px4fmu-v2/nsh/Make.defs @@ -53,7 +53,7 @@ NM = $(CROSSDEV)nm OBJCOPY = $(CROSSDEV)objcopy OBJDUMP = $(CROSSDEV)objdump -MAXOPTIMIZATION = -O3 +MAXOPTIMIZATION = -Os ARCHCPUFLAGS = -mcpu=cortex-m4 \ -mthumb \ -march=armv7e-m \ diff --git a/nuttx-configs/px4fmu-v2/scripts/ld.script b/nuttx-configs/px4fmu-v2/scripts/ld.script index 1be39fb87f..bec896d1ce 100644 --- a/nuttx-configs/px4fmu-v2/scripts/ld.script +++ b/nuttx-configs/px4fmu-v2/scripts/ld.script @@ -50,7 +50,8 @@ MEMORY { - flash (rx) : ORIGIN = 0x08004000, LENGTH = 2032K + /* disabled due to silicon errata flash (rx) : ORIGIN = 0x08004000, LENGTH = 2032K */ + flash (rx) : ORIGIN = 0x08004000, LENGTH = 1008K sram (rwx) : ORIGIN = 0x20000000, LENGTH = 192K ccsram (rwx) : ORIGIN = 0x10000000, LENGTH = 64K } diff --git a/nuttx-configs/px4io-v1/nsh/Make.defs b/nuttx-configs/px4io-v1/nsh/Make.defs index 712631f471..7a0792ff64 100644 --- a/nuttx-configs/px4io-v1/nsh/Make.defs +++ b/nuttx-configs/px4io-v1/nsh/Make.defs @@ -53,7 +53,7 @@ NM = $(CROSSDEV)nm OBJCOPY = $(CROSSDEV)objcopy OBJDUMP = $(CROSSDEV)objdump -MAXOPTIMIZATION = -O3 +MAXOPTIMIZATION = -Os ARCHCPUFLAGS = -mcpu=cortex-m3 \ -mthumb \ -march=armv7-m diff --git a/nuttx-configs/px4io-v2/nsh/Make.defs b/nuttx-configs/px4io-v2/nsh/Make.defs index cd2d8eba3b..1717464d2f 100644 --- a/nuttx-configs/px4io-v2/nsh/Make.defs +++ b/nuttx-configs/px4io-v2/nsh/Make.defs @@ -53,15 +53,11 @@ NM = $(CROSSDEV)nm OBJCOPY = $(CROSSDEV)objcopy OBJDUMP = $(CROSSDEV)objdump -MAXOPTIMIZATION = -O3 +MAXOPTIMIZATION = -Os ARCHCPUFLAGS = -mcpu=cortex-m3 \ -mthumb \ -march=armv7-m -# enable precise stack overflow tracking -#INSTRUMENTATIONDEFINES = -finstrument-functions \ -# -ffixed-r10 - # use our linker script LDSCRIPT = ld.script From 3d2a5bae51763e5a542506383c9d97f95fc7d1ef Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 15 Nov 2014 13:51:16 +0100 Subject: [PATCH 075/172] Board drivers: Optimize for size --- src/drivers/boards/aerocore/module.mk | 2 ++ src/drivers/boards/px4fmu-v1/module.mk | 2 ++ src/drivers/boards/px4fmu-v2/module.mk | 2 ++ src/drivers/boards/px4io-v1/module.mk | 2 ++ src/drivers/boards/px4io-v2/module.mk | 2 ++ 5 files changed, 10 insertions(+) diff --git a/src/drivers/boards/aerocore/module.mk b/src/drivers/boards/aerocore/module.mk index b53fe0a293..0a2d910091 100644 --- a/src/drivers/boards/aerocore/module.mk +++ b/src/drivers/boards/aerocore/module.mk @@ -6,3 +6,5 @@ SRCS = aerocore_init.c \ aerocore_pwm_servo.c \ aerocore_spi.c \ aerocore_led.c + +MAXOPTIMIZATION = -Os diff --git a/src/drivers/boards/px4fmu-v1/module.mk b/src/drivers/boards/px4fmu-v1/module.mk index 66b7769173..5e1a27d5a1 100644 --- a/src/drivers/boards/px4fmu-v1/module.mk +++ b/src/drivers/boards/px4fmu-v1/module.mk @@ -8,3 +8,5 @@ SRCS = px4fmu_can.c \ px4fmu_spi.c \ px4fmu_usb.c \ px4fmu_led.c + +MAXOPTIMIZATION = -Os diff --git a/src/drivers/boards/px4fmu-v2/module.mk b/src/drivers/boards/px4fmu-v2/module.mk index 99d37eecac..103232b0ca 100644 --- a/src/drivers/boards/px4fmu-v2/module.mk +++ b/src/drivers/boards/px4fmu-v2/module.mk @@ -8,3 +8,5 @@ SRCS = px4fmu_can.c \ px4fmu_spi.c \ px4fmu_usb.c \ px4fmu2_led.c + +MAXOPTIMIZATION = -Os diff --git a/src/drivers/boards/px4io-v1/module.mk b/src/drivers/boards/px4io-v1/module.mk index 2601a1c15a..a7a14dd072 100644 --- a/src/drivers/boards/px4io-v1/module.mk +++ b/src/drivers/boards/px4io-v1/module.mk @@ -4,3 +4,5 @@ SRCS = px4io_init.c \ px4io_pwm_servo.c + +MAXOPTIMIZATION = -Os diff --git a/src/drivers/boards/px4io-v2/module.mk b/src/drivers/boards/px4io-v2/module.mk index 85f94e8be1..3f0e9a0b3a 100644 --- a/src/drivers/boards/px4io-v2/module.mk +++ b/src/drivers/boards/px4io-v2/module.mk @@ -4,3 +4,5 @@ SRCS = px4iov2_init.c \ px4iov2_pwm_servo.c + +MAXOPTIMIZATION = -Os From c0f34dff2605381afacbec2bc1eba6b648daddd2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 15 Nov 2014 13:51:33 +0100 Subject: [PATCH 076/172] STM32 drivers: Optimize for size --- src/drivers/stm32/adc/module.mk | 2 ++ src/drivers/stm32/module.mk | 2 ++ src/drivers/stm32/tone_alarm/module.mk | 2 ++ 3 files changed, 6 insertions(+) diff --git a/src/drivers/stm32/adc/module.mk b/src/drivers/stm32/adc/module.mk index 48620feea8..38ea490a0e 100644 --- a/src/drivers/stm32/adc/module.mk +++ b/src/drivers/stm32/adc/module.mk @@ -40,3 +40,5 @@ MODULE_COMMAND = adc SRCS = adc.cpp INCLUDE_DIRS += $(NUTTX_SRC)/arch/arm/src/stm32 $(NUTTX_SRC)/arch/arm/src/common + +MAXOPTIMIZATION = -Os diff --git a/src/drivers/stm32/module.mk b/src/drivers/stm32/module.mk index bb751c7f68..54428e270d 100644 --- a/src/drivers/stm32/module.mk +++ b/src/drivers/stm32/module.mk @@ -41,3 +41,5 @@ SRCS = drv_hrt.c \ drv_pwm_servo.c INCLUDE_DIRS += $(NUTTX_SRC)/arch/arm/src/stm32 $(NUTTX_SRC)/arch/arm/src/common + +MAXOPTIMIZATION = -Os diff --git a/src/drivers/stm32/tone_alarm/module.mk b/src/drivers/stm32/tone_alarm/module.mk index 827cf30b27..25a194ef61 100644 --- a/src/drivers/stm32/tone_alarm/module.mk +++ b/src/drivers/stm32/tone_alarm/module.mk @@ -40,3 +40,5 @@ MODULE_COMMAND = tone_alarm SRCS = tone_alarm.cpp INCLUDE_DIRS += $(NUTTX_SRC)/arch/arm/src/stm32 $(NUTTX_SRC)/arch/arm/src/common + +MAXOPTIMIZATION = -Os From f7f54062439f0b1c1208d0778f992f7052e1518b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 15 Nov 2014 13:51:49 +0100 Subject: [PATCH 077/172] FMU driver: optimize for size --- src/drivers/px4fmu/module.mk | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/drivers/px4fmu/module.mk b/src/drivers/px4fmu/module.mk index a60f1a434a..a06323a52f 100644 --- a/src/drivers/px4fmu/module.mk +++ b/src/drivers/px4fmu/module.mk @@ -8,3 +8,5 @@ SRCS = fmu.cpp MODULE_STACKSIZE = 1200 EXTRACXXFLAGS = -Weffc++ + +MAXOPTIMIZATION = -Os From a12c98ba63c12ba08f1fb0f66df7b7079e444e1d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 15 Nov 2014 13:52:08 +0100 Subject: [PATCH 078/172] Sensors: Optimize for size --- src/modules/sensors/module.mk | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/sensors/module.mk b/src/modules/sensors/module.mk index 5b1bc5e86f..dfbba92d91 100644 --- a/src/modules/sensors/module.mk +++ b/src/modules/sensors/module.mk @@ -42,3 +42,5 @@ SRCS = sensors.cpp \ sensor_params.c MODULE_STACKSIZE = 1200 + +MAXOPTIMIZATION = -Os From a869105ba23394cfabb0a0000dd3b7fa3dc9fdb1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 15 Nov 2014 13:52:34 +0100 Subject: [PATCH 079/172] Systemlib: Optimize for size --- src/modules/systemlib/module.mk | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/systemlib/module.mk b/src/modules/systemlib/module.mk index 147903aa03..5c4f2ec419 100644 --- a/src/modules/systemlib/module.mk +++ b/src/modules/systemlib/module.mk @@ -55,3 +55,4 @@ SRCS = err.c \ pwm_limit/pwm_limit.c \ circuit_breaker.c +MAXOPTIMIZATION = -Os From 54f296ce9d8245845a7a0bf32f5045a44aab40c7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 15 Nov 2014 13:54:01 +0100 Subject: [PATCH 080/172] Sensor drivers: Optimize for size --- src/drivers/hmc5883/module.mk | 2 ++ src/drivers/l3gd20/module.mk | 2 ++ src/drivers/lsm303d/module.mk | 2 ++ src/drivers/mpu6000/module.mk | 2 ++ 4 files changed, 8 insertions(+) diff --git a/src/drivers/hmc5883/module.mk b/src/drivers/hmc5883/module.mk index 5daa01dc57..be2ee72766 100644 --- a/src/drivers/hmc5883/module.mk +++ b/src/drivers/hmc5883/module.mk @@ -42,3 +42,5 @@ SRCS = hmc5883.cpp MODULE_STACKSIZE = 1200 EXTRACXXFLAGS = -Weffc++ + +MAXOPTIMIZATION = -Os diff --git a/src/drivers/l3gd20/module.mk b/src/drivers/l3gd20/module.mk index 5630e7aeca..3d64d62be0 100644 --- a/src/drivers/l3gd20/module.mk +++ b/src/drivers/l3gd20/module.mk @@ -8,3 +8,5 @@ SRCS = l3gd20.cpp MODULE_STACKSIZE = 1200 EXTRACXXFLAGS = -Weffc++ + +MAXOPTIMIZATION = -Os diff --git a/src/drivers/lsm303d/module.mk b/src/drivers/lsm303d/module.mk index b4f3974f4d..0421eb113e 100644 --- a/src/drivers/lsm303d/module.mk +++ b/src/drivers/lsm303d/module.mk @@ -8,3 +8,5 @@ SRCS = lsm303d.cpp MODULE_STACKSIZE = 1200 EXTRACXXFLAGS = -Weffc++ + +MAXOPTIMIZATION = -Os diff --git a/src/drivers/mpu6000/module.mk b/src/drivers/mpu6000/module.mk index 5b4893b127..da9fcc0fc4 100644 --- a/src/drivers/mpu6000/module.mk +++ b/src/drivers/mpu6000/module.mk @@ -42,3 +42,5 @@ SRCS = mpu6000.cpp MODULE_STACKSIZE = 1200 EXTRACXXFLAGS = -Weffc++ + +MAXOPTIMIZATION = -Os From 32313a13dd1f98426f83b99310fc22b5adced37c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 15 Nov 2014 13:54:16 +0100 Subject: [PATCH 081/172] PX4IO driver: Optimize for size --- src/drivers/px4io/module.mk | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/drivers/px4io/module.mk b/src/drivers/px4io/module.mk index 5b838fb75e..924283356b 100644 --- a/src/drivers/px4io/module.mk +++ b/src/drivers/px4io/module.mk @@ -48,3 +48,5 @@ INCLUDE_DIRS += $(NUTTX_SRC)/arch/arm/src/stm32 $(NUTTX_SRC)/arch/arm/src/com MODULE_STACKSIZE = 1200 EXTRACXXFLAGS = -Weffc++ + +MAXOPTIMIZATION = -Os From 42575bbc3763fe2b9e2d3e5b074ec74e4fc57f5a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 15 Nov 2014 13:54:38 +0100 Subject: [PATCH 082/172] Bottle drop: Optimize for size --- src/modules/bottle_drop/module.mk | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/bottle_drop/module.mk b/src/modules/bottle_drop/module.mk index 6b18fff556..9602288790 100644 --- a/src/modules/bottle_drop/module.mk +++ b/src/modules/bottle_drop/module.mk @@ -39,3 +39,5 @@ MODULE_COMMAND = bottle_drop SRCS = bottle_drop.cpp \ bottle_drop_params.c + +MAXOPTIMIZATION = -Os From a7bc52754de52ccbf0f958c20ce31408a8b4372e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 15 Nov 2014 13:54:54 +0100 Subject: [PATCH 083/172] SDLOG2: Optimize for size --- src/modules/sdlog2/module.mk | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/sdlog2/module.mk b/src/modules/sdlog2/module.mk index a28d43e725..d4a00af39d 100644 --- a/src/modules/sdlog2/module.mk +++ b/src/modules/sdlog2/module.mk @@ -43,3 +43,5 @@ SRCS = sdlog2.c \ logbuffer.c MODULE_STACKSIZE = 1200 + +MAXOPTIMIZATION = -Os From 7b962e7d1bb6b60633c116b9d5395b24e6658664 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 15 Nov 2014 14:17:24 +0100 Subject: [PATCH 084/172] Version command optimization --- src/systemcmds/ver/ver.c | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/src/systemcmds/ver/ver.c b/src/systemcmds/ver/ver.c index b11b593c38..9d308bc3e7 100644 --- a/src/systemcmds/ver/ver.c +++ b/src/systemcmds/ver/ver.c @@ -182,9 +182,7 @@ int ver_main(int argc, char *argv[]) if (rev < STM32F4_REV_3) { printf("\n\nWARNING WARNING WARNING!\n" "Revision %c has a silicon errata\n" - "on USB connectivity combined with\n" - "flash bank #2. This device can only\n" - "utilize a maximum of 1MB flash safely!\n" + "This device can only utilize a maximum of 1MB flash safely!\n" "http://px4.io/help/errata\n", rev); } ret = 0; From 2a37d274b142f49e250265eb14821d0b4f7e51cf Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 15 Nov 2014 15:51:55 +0100 Subject: [PATCH 085/172] Systemlib: Add MCU version command. ver commandline tool: Add support for MCU version command --- src/modules/systemlib/mcu_version.c | 109 ++++++++++++++++++++++++++++ src/modules/systemlib/mcu_version.h | 52 +++++++++++++ src/modules/systemlib/module.mk | 4 +- src/systemcmds/ver/ver.c | 90 +++++------------------ 4 files changed, 180 insertions(+), 75 deletions(-) create mode 100644 src/modules/systemlib/mcu_version.c create mode 100644 src/modules/systemlib/mcu_version.h diff --git a/src/modules/systemlib/mcu_version.c b/src/modules/systemlib/mcu_version.c new file mode 100644 index 0000000000..4bcf957848 --- /dev/null +++ b/src/modules/systemlib/mcu_version.c @@ -0,0 +1,109 @@ +/**************************************************************************** + * + * 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 mcu_version.c + * + * Read out the microcontroller version from the board + * + * @author Lorenz Meier + * + */ + +#include "mcu_version.h" + +#include + +#ifdef CONFIG_ARCH_CHIP_STM32 +#include + +#define DBGMCU_IDCODE 0xE0042000 + +#define STM32F40x_41x 0x413 +#define STM32F42x_43x 0x419 + +#define REVID_MASK 0xFFFF0000 +#define DEVID_MASK 0xFFF + +#endif + + + +int mcu_version(char* rev, char** revstr) +{ +#ifdef CONFIG_ARCH_CHIP_STM32 + uint32_t abc = getreg32(DBGMCU_IDCODE); + + int32_t chip_version = abc & DEVID_MASK; + enum MCU_REV revid = (abc & REVID_MASK) >> 16; + + switch (chip_version) { + case STM32F40x_41x: + *revstr = "STM32F40x"; + break; + case STM32F42x_43x: + *revstr = "STM32F42x"; + break; + default: + *revstr = "STM32F???"; + break; + } + + switch (revid) { + + case MCU_REV_STM32F4_REV_A: + *rev = 'A'; + break; + case MCU_REV_STM32F4_REV_Z: + *rev = 'Z'; + break; + case MCU_REV_STM32F4_REV_Y: + *rev = 'Y'; + break; + case MCU_REV_STM32F4_REV_1: + *rev = '1'; + break; + case MCU_REV_STM32F4_REV_3: + *rev = '3'; + break; + default: + *rev = '?'; + revid = -1; + break; + } + + return revid; +#else + return -1; +#endif +} diff --git a/src/modules/systemlib/mcu_version.h b/src/modules/systemlib/mcu_version.h new file mode 100644 index 0000000000..1b3d0aba9b --- /dev/null +++ b/src/modules/systemlib/mcu_version.h @@ -0,0 +1,52 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#pragma once + +/* magic numbers from reference manual */ +enum MCU_REV { + MCU_REV_STM32F4_REV_A = 0x1000, + MCU_REV_STM32F4_REV_Z = 0x1001, + MCU_REV_STM32F4_REV_Y = 0x1003, + MCU_REV_STM32F4_REV_1 = 0x1007, + MCU_REV_STM32F4_REV_3 = 0x2001 +}; + +/** + * Reports the microcontroller version of the main CPU. + * + * @param rev The silicon revision character + * @param revstr The full chip name string + * @return The silicon revision / version number as integer + */ +__EXPORT int mcu_version(char* rev, char** revstr); diff --git a/src/modules/systemlib/module.mk b/src/modules/systemlib/module.mk index 147903aa03..233023e25c 100644 --- a/src/modules/systemlib/module.mk +++ b/src/modules/systemlib/module.mk @@ -53,5 +53,5 @@ SRCS = err.c \ otp.c \ board_serial.c \ pwm_limit/pwm_limit.c \ - circuit_breaker.c - + circuit_breaker.c \ + mcu_version.c diff --git a/src/systemcmds/ver/ver.c b/src/systemcmds/ver/ver.c index 9d308bc3e7..62a7a5b92a 100644 --- a/src/systemcmds/ver/ver.c +++ b/src/systemcmds/ver/ver.c @@ -44,6 +44,7 @@ #include #include #include +#include // string constants for version commands static const char sz_ver_hw_str[] = "hw"; @@ -52,32 +53,8 @@ static const char sz_ver_git_str[] = "git"; static const char sz_ver_bdate_str[] = "bdate"; static const char sz_ver_gcc_str[] = "gcc"; static const char sz_ver_all_str[] = "all"; - -#ifdef CONFIG_ARCH_CHIP_STM32 -#include - static const char mcu_ver_str[] = "mcu"; -#define DBGMCU_IDCODE 0xE0042000 - -#define STM32F40x_41x 0x413 -#define STM32F42x_43x 0x419 - -#define REVID_MASK 0xFFFF0000 -#define DEVID_MASK 0xFFF - -/* magic numbers from reference manual */ -enum STM32F4_REV { - STM32F4_REV_A = 0x1000, - STM32F4_REV_Z = 0x1001, - STM32F4_REV_Y = 0x1003, - STM32F4_REV_1 = 0x1007, - STM32F4_REV_3 = 0x2001 -}; -#else -#error stm32 -#endif - static void usage(const char *reason) { if (reason != NULL) { @@ -132,62 +109,29 @@ int ver_main(int argc, char *argv[]) printf("GCC toolchain: %s\n", __VERSION__); ret = 0; -#ifdef CONFIG_ARCH_CHIP_STM32 + } else if (!strncmp(argv[1], mcu_ver_str, sizeof(mcu_ver_str))) { - uint32_t abc = getreg32(DBGMCU_IDCODE); - - uint32_t chip_version = abc & DEVID_MASK; - enum STM32F4_REV revid = (abc & REVID_MASK) >> 16; - - printf("CHIP TYPE: "); - - switch (revid) { - case STM32F40x_41x: - printf("STM32F40x"); - break; - case STM32F42x_43x: - printf("STM32F42x"); - break; - default: - printf("STM32F???"); - break; - } char rev; + char* revstr; - switch (chip_version) { + int chip_version = mcu_version(&rev, &revstr); - case STM32F4_REV_A: - rev = 'A'; - break; - case STM32F4_REV_Z: - rev = 'Z'; - break; - case STM32F4_REV_Y: - rev = 'Y'; - break; - case STM32F4_REV_1: - rev = '1'; - break; - case STM32F4_REV_3: - rev = '3'; - break; - default: - rev = '?'; - break; + if (chip_version < 0) { + printf("UNKNOWN MCU"); + ret = 1; + + } else { + printf("MCU: %s, rev. %c\n", revstr, rev); + + if (chip_version < MCU_REV_STM32F4_REV_3) { + printf("\n\nWARNING WARNING WARNING!\n" + "Revision %c has a silicon errata\n" + "This device can only utilize a maximum of 1MB flash safely!\n" + "http://px4.io/help/errata\n", rev); + } } - printf("\nHW REV: %c\n", rev); - - if (rev < STM32F4_REV_3) { - printf("\n\nWARNING WARNING WARNING!\n" - "Revision %c has a silicon errata\n" - "This device can only utilize a maximum of 1MB flash safely!\n" - "http://px4.io/help/errata\n", rev); - } - ret = 0; -#endif - } else { errx(1, "unknown command.\n"); } From 6d59df1a5f67a58b6881d4d8a466082b847a2e61 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 15 Nov 2014 16:19:51 +0100 Subject: [PATCH 086/172] Make ROMFS less chatty --- .../init.d/1000_rc_fw_easystar.hil | 8 ++----- .../px4fmu_common/init.d/10015_tbs_discovery | 2 +- ROMFS/px4fmu_common/init.d/10016_3dr_iris | 2 +- .../init.d/10017_steadidrone_qu4d | 3 +-- 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 | 4 +--- .../init.d/1005_rc_fw_Malolo1.hil | 2 +- ROMFS/px4fmu_common/init.d/11001_hexa_cox | 4 ++-- ROMFS/px4fmu_common/init.d/12001_octo_cox | 2 +- ROMFS/px4fmu_common/init.d/3031_phantom | 4 +--- ROMFS/px4fmu_common/init.d/3032_skywalker_x5 | 6 +---- ROMFS/px4fmu_common/init.d/3033_wingwing | 2 +- ROMFS/px4fmu_common/init.d/3034_fx79 | 2 +- ROMFS/px4fmu_common/init.d/3035_viper | 2 +- .../px4fmu_common/init.d/3100_tbs_caipirinha | 8 ++----- ROMFS/px4fmu_common/init.d/4001_quad_x | 2 +- ROMFS/px4fmu_common/init.d/4008_ardrone | 3 --- ROMFS/px4fmu_common/init.d/4010_dji_f330 | 4 ++-- ROMFS/px4fmu_common/init.d/4011_dji_f450 | 4 ++-- 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/5001_quad_+ | 2 +- ROMFS/px4fmu_common/init.d/6001_hexa_x | 4 ++-- ROMFS/px4fmu_common/init.d/7001_hexa_+ | 4 ++-- ROMFS/px4fmu_common/init.d/8001_octo_x | 2 +- ROMFS/px4fmu_common/init.d/9001_octo_+ | 2 +- ROMFS/px4fmu_common/init.d/rc.autostart | 5 ++-- ROMFS/px4fmu_common/init.d/rc.sensors | 7 +----- ROMFS/px4fmu_common/init.d/rcS | 24 +++---------------- 30 files changed, 39 insertions(+), 83 deletions(-) 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 d114fe21a2..4d6d350b8a 100644 --- a/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil +++ b/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil @@ -1,14 +1,10 @@ #!nsh # -# HILStar / X-Plane -# -# Lorenz Meier +# HILStar +# # sh /etc/init.d/rc.fw_defaults -echo "X Plane HIL starting.." - set HIL yes - set MIXER FMU_AERT diff --git a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery index b1aa8c00b3..c8379e3a1a 100644 --- a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery +++ b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery @@ -2,7 +2,7 @@ # # Team Blacksheep Discovery Quadcopter # -# Anton Babushkin , Simon Wilks , Thomas Gubler +# Anton Babushkin , Simon Wilks # sh /etc/init.d/rc.mc_defaults diff --git a/ROMFS/px4fmu_common/init.d/10016_3dr_iris b/ROMFS/px4fmu_common/init.d/10016_3dr_iris index 3f47390c15..0b422de7e2 100644 --- a/ROMFS/px4fmu_common/init.d/10016_3dr_iris +++ b/ROMFS/px4fmu_common/init.d/10016_3dr_iris @@ -2,7 +2,7 @@ # # 3DR Iris Quadcopter # -# Anton Babushkin +# Anton Babushkin # sh /etc/init.d/rc.mc_defaults diff --git a/ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d b/ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d index 6179855f6a..a621de7ce8 100644 --- a/ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d +++ b/ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d @@ -2,8 +2,7 @@ # # Steadidrone QU4D # -# Thomas Gubler -# Lorenz Meier +# Thomas Gubler # sh /etc/init.d/rc.mc_defaults 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 7a7a9542c0..1c4f6803b6 100644 --- a/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil +++ b/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil @@ -2,7 +2,7 @@ # # HIL Quadcopter X # -# Anton Babushkin +# Anton Babushkin # sh /etc/init.d/rc.mc_defaults diff --git a/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil b/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil index c47500c7ac..0cbdd75bed 100644 --- a/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil +++ b/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil @@ -2,7 +2,7 @@ # # HIL Quadcopter + # -# Anton Babushkin +# Anton Babushkin # sh /etc/init.d/rc.mc_defaults 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 4e3e183262..fb440d2fc7 100644 --- a/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil +++ b/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil @@ -2,13 +2,11 @@ # # HIL Rascal 110 (Flightgear) # -# Thomas Gubler +# Thomas Gubler # sh /etc/init.d/rc.fw_defaults -echo "HIL Rascal 110 starting.." - set HIL yes set MIXER FMU_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 941f5664ad..00b97d675c 100644 --- a/ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil +++ b/ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil @@ -2,7 +2,7 @@ # # HIL Malolo 1 (Flightgear) # -# Thomas Gubler +# Thomas Gubler # sh /etc/init.d/rc.fw_defaults diff --git a/ROMFS/px4fmu_common/init.d/11001_hexa_cox b/ROMFS/px4fmu_common/init.d/11001_hexa_cox index daa04a4de8..e4d96fbd56 100644 --- a/ROMFS/px4fmu_common/init.d/11001_hexa_cox +++ b/ROMFS/px4fmu_common/init.d/11001_hexa_cox @@ -2,12 +2,12 @@ # # Generic 10" Hexa coaxial geometry # -# Lorenz Meier +# Lorenz Meier # sh /etc/init.d/rc.mc_defaults set MIXER FMU_hexa_cox -# We only can run one channel group with one rate, so set all 8 channels +# Need to set all 8 channels set PWM_OUTPUTS 12345678 diff --git a/ROMFS/px4fmu_common/init.d/12001_octo_cox b/ROMFS/px4fmu_common/init.d/12001_octo_cox index 8703f5f2fe..f820251ad3 100644 --- a/ROMFS/px4fmu_common/init.d/12001_octo_cox +++ b/ROMFS/px4fmu_common/init.d/12001_octo_cox @@ -2,7 +2,7 @@ # # Generic 10" Octo coaxial geometry # -# Lorenz Meier +# Lorenz Meier # sh /etc/init.d/rc.mc_defaults diff --git a/ROMFS/px4fmu_common/init.d/3031_phantom b/ROMFS/px4fmu_common/init.d/3031_phantom index 53c48d8aac..a7749cba6e 100644 --- a/ROMFS/px4fmu_common/init.d/3031_phantom +++ b/ROMFS/px4fmu_common/init.d/3031_phantom @@ -2,7 +2,7 @@ # # Phantom FPV Flying Wing # -# Simon Wilks , Thomas Gubler +# Simon Wilks # sh /etc/init.d/rc.fw_defaults @@ -21,8 +21,6 @@ then param set FW_PR_P 0.03 param set FW_P_LIM_MAX 50 param set FW_P_LIM_MIN -50 - param set FW_P_RMAX_NEG 0 - param set FW_P_RMAX_POS 0 param set FW_P_ROLLFF 1 param set FW_RR_FF 0.5 param set FW_RR_I 0.02 diff --git a/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 b/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 index 7d0dc5bff3..26c7c95e67 100644 --- a/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 +++ b/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 @@ -2,7 +2,7 @@ # # Skywalker X5 Flying Wing # -# Thomas Gubler , Julian Oes +# Thomas Gubler , Julian Oes # sh /etc/init.d/rc.fw_defaults @@ -19,10 +19,6 @@ then param set FW_PR_I 0 param set FW_PR_IMAX 0.2 param set FW_PR_P 0.03 - param set FW_P_LIM_MAX 45 - param set FW_P_LIM_MIN -45 - param set FW_P_RMAX_NEG 0 - param set FW_P_RMAX_POS 0 param set FW_P_ROLLFF 1 param set FW_RR_FF 0.3 param set FW_RR_I 0 diff --git a/ROMFS/px4fmu_common/init.d/3033_wingwing b/ROMFS/px4fmu_common/init.d/3033_wingwing index f4dedef15c..919eefb4a8 100644 --- a/ROMFS/px4fmu_common/init.d/3033_wingwing +++ b/ROMFS/px4fmu_common/init.d/3033_wingwing @@ -2,7 +2,7 @@ # # Wing Wing (aka Z-84) Flying Wing # -# Simon Wilks +# Simon Wilks # sh /etc/init.d/rc.fw_defaults diff --git a/ROMFS/px4fmu_common/init.d/3034_fx79 b/ROMFS/px4fmu_common/init.d/3034_fx79 index f4bd18269d..4a76ba6ebd 100644 --- a/ROMFS/px4fmu_common/init.d/3034_fx79 +++ b/ROMFS/px4fmu_common/init.d/3034_fx79 @@ -2,7 +2,7 @@ # # FX-79 Buffalo Flying Wing # -# Simon Wilks +# Simon Wilks # sh /etc/init.d/rc.fw_defaults diff --git a/ROMFS/px4fmu_common/init.d/3035_viper b/ROMFS/px4fmu_common/init.d/3035_viper index f3b0e84183..0f5f5502aa 100644 --- a/ROMFS/px4fmu_common/init.d/3035_viper +++ b/ROMFS/px4fmu_common/init.d/3035_viper @@ -2,7 +2,7 @@ # # Viper # -# Simon Wilks +# Simon Wilks # sh /etc/init.d/rc.fw_defaults diff --git a/ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha b/ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha index 9a21504035..9bfd9d9ed5 100644 --- a/ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha +++ b/ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha @@ -1,8 +1,8 @@ #!nsh # -# TBS Caipirinha Flying Wing +# TBS Caipirinha # -# Thomas Gubler +# Thomas Gubler # sh /etc/init.d/rc.fw_defaults @@ -22,10 +22,6 @@ then param set FW_PR_I 0 param set FW_PR_IMAX 0.2 param set FW_PR_P 0.03 - param set FW_P_LIM_MAX 45 - param set FW_P_LIM_MIN -45 - param set FW_P_RMAX_NEG 0 - param set FW_P_RMAX_POS 0 param set FW_P_ROLLFF 0 param set FW_RR_FF 0.3 param set FW_RR_I 0 diff --git a/ROMFS/px4fmu_common/init.d/4001_quad_x b/ROMFS/px4fmu_common/init.d/4001_quad_x index 06c54a41db..8fe8961c5c 100644 --- a/ROMFS/px4fmu_common/init.d/4001_quad_x +++ b/ROMFS/px4fmu_common/init.d/4001_quad_x @@ -2,7 +2,7 @@ # # Generic 10" Quad X geometry # -# Lorenz Meier +# Lorenz Meier # sh /etc/init.d/rc.mc_defaults diff --git a/ROMFS/px4fmu_common/init.d/4008_ardrone b/ROMFS/px4fmu_common/init.d/4008_ardrone index e6007db0e7..0488e3928e 100644 --- a/ROMFS/px4fmu_common/init.d/4008_ardrone +++ b/ROMFS/px4fmu_common/init.d/4008_ardrone @@ -3,9 +3,6 @@ # ARDrone # -echo "[init] 4008_ardrone: PX4FMU on PX4IOAR carrier board" - -# Just use the default multicopter settings. sh /etc/init.d/rc.mc_defaults # diff --git a/ROMFS/px4fmu_common/init.d/4010_dji_f330 b/ROMFS/px4fmu_common/init.d/4010_dji_f330 index 282ab620da..f0cc052075 100644 --- a/ROMFS/px4fmu_common/init.d/4010_dji_f330 +++ b/ROMFS/px4fmu_common/init.d/4010_dji_f330 @@ -1,8 +1,8 @@ #!nsh # -# DJI Flame Wheel F330 Quadcopter +# DJI Flame Wheel F330 # -# Anton Babushkin +# Anton Babushkin # sh /etc/init.d/4001_quad_x diff --git a/ROMFS/px4fmu_common/init.d/4011_dji_f450 b/ROMFS/px4fmu_common/init.d/4011_dji_f450 index 517b4aa86a..1ca716a6b3 100644 --- a/ROMFS/px4fmu_common/init.d/4011_dji_f450 +++ b/ROMFS/px4fmu_common/init.d/4011_dji_f450 @@ -1,8 +1,8 @@ #!nsh # -# DJI Flame Wheel F450 Quadcopter +# DJI Flame Wheel F450 # -# Lorenz Meier +# Lorenz Meier # sh /etc/init.d/4001_quad_x diff --git a/ROMFS/px4fmu_common/init.d/4012_quad_x_can b/ROMFS/px4fmu_common/init.d/4012_quad_x_can index 8c5a4fbf2d..5c4a6497a0 100644 --- a/ROMFS/px4fmu_common/init.d/4012_quad_x_can +++ b/ROMFS/px4fmu_common/init.d/4012_quad_x_can @@ -2,7 +2,7 @@ # # F450-sized quadrotor with CAN # -# Lorenz Meier +# Pavel Kirienko # sh /etc/init.d/4001_quad_x diff --git a/ROMFS/px4fmu_common/init.d/4020_hk_micro_pcb b/ROMFS/px4fmu_common/init.d/4020_hk_micro_pcb index 99ffd73a5f..9fe310ddee 100644 --- a/ROMFS/px4fmu_common/init.d/4020_hk_micro_pcb +++ b/ROMFS/px4fmu_common/init.d/4020_hk_micro_pcb @@ -3,7 +3,7 @@ # Hobbyking Micro Integrated PCB Quadcopter # with SimonK ESC firmware and Mystery A1510 motors # -# Thomas Gubler +# Thomas Gubler # echo "HK Micro PCB Quad" diff --git a/ROMFS/px4fmu_common/init.d/5001_quad_+ b/ROMFS/px4fmu_common/init.d/5001_quad_+ index 1fb25e5d88..5512aa738a 100644 --- a/ROMFS/px4fmu_common/init.d/5001_quad_+ +++ b/ROMFS/px4fmu_common/init.d/5001_quad_+ @@ -2,7 +2,7 @@ # # Generic 10" Quad + geometry # -# Anton Babushkin +# Anton Babushkin # sh /etc/init.d/rc.mc_defaults diff --git a/ROMFS/px4fmu_common/init.d/6001_hexa_x b/ROMFS/px4fmu_common/init.d/6001_hexa_x index 34fc6523fe..1043ad8adb 100644 --- a/ROMFS/px4fmu_common/init.d/6001_hexa_x +++ b/ROMFS/px4fmu_common/init.d/6001_hexa_x @@ -2,12 +2,12 @@ # # Generic 10" Hexa X geometry # -# Anton Babushkin +# Anton Babushkin # sh /etc/init.d/rc.mc_defaults set MIXER FMU_hexa_x -# We only can run one channel group with one rate, so set all 8 channels +# Need to set all 8 channels set PWM_OUTPUTS 12345678 diff --git a/ROMFS/px4fmu_common/init.d/7001_hexa_+ b/ROMFS/px4fmu_common/init.d/7001_hexa_+ index 235e376a6e..84ab88883c 100644 --- a/ROMFS/px4fmu_common/init.d/7001_hexa_+ +++ b/ROMFS/px4fmu_common/init.d/7001_hexa_+ @@ -2,12 +2,12 @@ # # Generic 10" Hexa + geometry # -# Anton Babushkin +# Anton Babushkin # sh /etc/init.d/rc.mc_defaults set MIXER FMU_hexa_+ -# We only can run one channel group with one rate, so set all 8 channels +# Need to set all 8 channels set PWM_OUTPUTS 12345678 diff --git a/ROMFS/px4fmu_common/init.d/8001_octo_x b/ROMFS/px4fmu_common/init.d/8001_octo_x index 769217dc78..74e304cd96 100644 --- a/ROMFS/px4fmu_common/init.d/8001_octo_x +++ b/ROMFS/px4fmu_common/init.d/8001_octo_x @@ -2,7 +2,7 @@ # # Generic 10" Octo X geometry # -# Anton Babushkin +# Anton Babushkin # sh /etc/init.d/rc.mc_defaults diff --git a/ROMFS/px4fmu_common/init.d/9001_octo_+ b/ROMFS/px4fmu_common/init.d/9001_octo_+ index 28b074a545..f7c06c6c80 100644 --- a/ROMFS/px4fmu_common/init.d/9001_octo_+ +++ b/ROMFS/px4fmu_common/init.d/9001_octo_+ @@ -2,7 +2,7 @@ # # Generic 10" Octo + geometry # -# Anton Babushkin +# Anton Babushkin # sh /etc/init.d/rc.mc_defaults diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart b/ROMFS/px4fmu_common/init.d/rc.autostart index 78778d8066..496a52c5fa 100644 --- a/ROMFS/px4fmu_common/init.d/rc.autostart +++ b/ROMFS/px4fmu_common/init.d/rc.autostart @@ -1,5 +1,4 @@ # -# Check if auto-setup from one of the standard scripts is wanted # SYS_AUTOSTART = 0 means no autostart (default) # # AUTOSTART PARTITION: @@ -18,7 +17,7 @@ # 12000 .. 12999 Octo Cox # -# Simulation setups +# Simulation # if param compare SYS_AUTOSTART 901 @@ -53,7 +52,7 @@ then fi # -# Standard plane +# Plane # if param compare SYS_AUTOSTART 2100 100 diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index 739df7ac0b..fbac50cf71 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -54,7 +54,6 @@ then fi fi -# Start airspeed sensors if meas_airspeed start then echo "[init] Using MEAS airspeed sensor" @@ -68,16 +67,12 @@ else fi fi -# Check for flow sensor if px4flow start then fi # -# Start the sensor collection task. -# IMPORTANT: this also loads param offsets -# ALWAYS start this task before the -# preflight_check. +# Start sensors -> preflight_check # if sensors start then diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index ea04ece34d..f9c8226350 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -137,9 +137,7 @@ then # if param compare SYS_AUTOCONFIG 1 then - # We can't be sure the defaults haven't changed, so - # if someone requests a re-configuration, we do it - # cleanly from scratch (except autostart / autoconfig) + # Wipe out params param reset_nostart set DO_AUTOCONFIG yes else @@ -202,12 +200,10 @@ then if px4io checkcrc $IO_FILE then - echo "[init] PX4IO CRC OK" echo "PX4IO CRC OK" >> $LOG_FILE set IO_PRESENT yes else - echo "[init] Trying to update" echo "PX4IO Trying to update" >> $LOG_FILE tone_alarm MLL32CP8MB @@ -217,18 +213,15 @@ then usleep 500000 if px4io checkcrc $IO_FILE then - echo "[init] PX4IO CRC OK, update successful" echo "PX4IO CRC OK after updating" >> $LOG_FILE tone_alarm MLL8CDE set IO_PRESENT yes else - echo "[init] ERROR: PX4IO update failed" echo "PX4IO update failed" >> $LOG_FILE tone_alarm $TUNE_OUT_ERROR fi else - echo "[init] ERROR: PX4IO update failed" echo "PX4IO update failed" >> $LOG_FILE tone_alarm $TUNE_OUT_ERROR fi @@ -281,16 +274,12 @@ then fi fi - # - # Start the datamanager (and do not abort boot if it fails) - # + # waypoint storage if dataman start then fi - # - # Start the Commander (needs to be this early for in-air-restarts) - # + # Needs to be this early for in-air-restarts commander start # @@ -424,9 +413,6 @@ then fi fi - # - # MAVLink - # if [ $MAVLINK_FLAGS == default ] then # Normal mode, use baudrate 57600 (default) and data rate 1000 bytes/s @@ -454,10 +440,6 @@ then # Sensors, Logging, GPS # sh /etc/init.d/rc.sensors - - # - # Start logging in all modes, including HIL - # sh /etc/init.d/rc.logging if [ $GPS == yes ] From e2c0ac3f700f003061e52fd61fd4770c982605c8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 15 Nov 2014 16:40:29 +0100 Subject: [PATCH 087/172] airspeed driver: Be less chatty --- src/drivers/airspeed/airspeed.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/airspeed/airspeed.cpp b/src/drivers/airspeed/airspeed.cpp index 293690d274..3a1e1b7b51 100644 --- a/src/drivers/airspeed/airspeed.cpp +++ b/src/drivers/airspeed/airspeed.cpp @@ -147,7 +147,7 @@ Airspeed::init() _airspeed_pub = orb_advertise(ORB_ID(differential_pressure), &arp); if (_airspeed_pub < 0) - warnx("failed to create airspeed sensor object. uORB started?"); + warnx("uORB started?"); } ret = OK; From 6f71173f8c7a13565ef90a8fec44187ba392f038 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 15 Nov 2014 16:40:42 +0100 Subject: [PATCH 088/172] ARDrone driver: be less chatty --- .../ardrone_interface/ardrone_interface.c | 36 ++++++++----------- .../ardrone_interface/ardrone_motor_control.c | 2 +- 2 files changed, 15 insertions(+), 23 deletions(-) diff --git a/src/drivers/ardrone_interface/ardrone_interface.c b/src/drivers/ardrone_interface/ardrone_interface.c index e5bb772b3c..9d2c1441de 100644 --- a/src/drivers/ardrone_interface/ardrone_interface.c +++ b/src/drivers/ardrone_interface/ardrone_interface.c @@ -89,8 +89,8 @@ static void usage(const char *reason) { if (reason) - fprintf(stderr, "%s\n", reason); - fprintf(stderr, "usage: ardrone_interface {start|stop|status} [-d ]\n\n"); + warnx("%s\n", reason); + warnx("usage: {start|stop|status} [-d ]\n\n"); exit(1); } @@ -110,7 +110,7 @@ int ardrone_interface_main(int argc, char *argv[]) if (!strcmp(argv[1], "start")) { if (thread_running) { - printf("ardrone_interface already running\n"); + warnx("already running\n"); /* this is not an error */ exit(0); } @@ -132,9 +132,9 @@ int ardrone_interface_main(int argc, char *argv[]) if (!strcmp(argv[1], "status")) { if (thread_running) { - printf("\tardrone_interface is running\n"); + warnx("running"); } else { - printf("\tardrone_interface not started\n"); + warnx("not started"); } exit(0); } @@ -158,7 +158,7 @@ static int ardrone_open_uart(char *uart_name, struct termios *uart_config_origin /* Back up the original uart configuration to restore it after exit */ if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) { - fprintf(stderr, "[ardrone_interface] ERROR getting baudrate / termios config for %s: %d\n", uart_name, termios_state); + warnx("ERR: TCGETATTR %s: %d", uart_name, termios_state); close(uart); return -1; } @@ -171,14 +171,14 @@ static int ardrone_open_uart(char *uart_name, struct termios *uart_config_origin /* Set baud rate */ if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) { - fprintf(stderr, "[ardrone_interface] ERROR setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)\n", uart_name, termios_state); + warnx("ERR: cfsetispeed %s: %d", uart_name, termios_state); close(uart); return -1; } if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) { - fprintf(stderr, "[ardrone_interface] ERROR setting baudrate / termios config for %s (tcsetattr)\n", uart_name); + warnx("ERR: tcsetattr: %s", uart_name); close(uart); return -1; } @@ -192,9 +192,6 @@ int ardrone_interface_thread_main(int argc, char *argv[]) char *device = "/dev/ttyS1"; - /* welcome user */ - printf("[ardrone_interface] Control started, taking over motors\n"); - /* File descriptors */ int gpios; @@ -237,7 +234,7 @@ int ardrone_interface_thread_main(int argc, char *argv[]) struct termios uart_config_original; if (motor_test_mode) { - printf("[ardrone_interface] Motor test mode enabled, setting 10 %% thrust.\n"); + warnx("setting 10 %% thrust.\n"); } /* Led animation */ @@ -255,9 +252,6 @@ int ardrone_interface_thread_main(int argc, char *argv[]) int actuator_controls_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); int armed_sub = orb_subscribe(ORB_ID(actuator_armed)); - printf("[ardrone_interface] Motors initialized - ready.\n"); - fflush(stdout); - /* enable UART, writes potentially an empty buffer, but multiplexing is disabled */ ardrone_write = ardrone_open_uart(device, &uart_config_original); @@ -265,7 +259,7 @@ int ardrone_interface_thread_main(int argc, char *argv[]) gpios = ar_multiplexing_init(); if (ardrone_write < 0) { - fprintf(stderr, "[ardrone_interface] Failed opening AR.Drone UART, exiting.\n"); + warnx("No UART, exiting."); thread_running = false; exit(ERROR); } @@ -273,7 +267,7 @@ int ardrone_interface_thread_main(int argc, char *argv[]) /* initialize motors */ if (OK != ar_init_motors(ardrone_write, gpios)) { close(ardrone_write); - fprintf(stderr, "[ardrone_interface] Failed initializing AR.Drone motors, exiting.\n"); + warnx("motor init fail"); thread_running = false; exit(ERROR); } @@ -294,7 +288,7 @@ int ardrone_interface_thread_main(int argc, char *argv[]) gpios = ar_multiplexing_init(); if (ardrone_write < 0) { - fprintf(stderr, "[ardrone_interface] Failed opening AR.Drone UART, exiting.\n"); + warnx("write fail"); thread_running = false; exit(ERROR); } @@ -302,7 +296,7 @@ int ardrone_interface_thread_main(int argc, char *argv[]) /* initialize motors */ if (OK != ar_init_motors(ardrone_write, gpios)) { close(ardrone_write); - fprintf(stderr, "[ardrone_interface] Failed initializing AR.Drone motors, exiting.\n"); + warnx("motor init fail"); thread_running = false; exit(ERROR); } @@ -378,11 +372,9 @@ int ardrone_interface_thread_main(int argc, char *argv[]) int termios_state; if ((termios_state = tcsetattr(ardrone_write, TCSANOW, &uart_config_original)) < 0) { - fprintf(stderr, "[ardrone_interface] ERROR setting baudrate / termios config for (tcsetattr)\n"); + warnx("ERR: tcsetattr"); } - printf("[ardrone_interface] Restored original UART config, exiting..\n"); - /* close uarts */ close(ardrone_write); ar_multiplexing_deinit(gpios); diff --git a/src/drivers/ardrone_interface/ardrone_motor_control.c b/src/drivers/ardrone_interface/ardrone_motor_control.c index fc017dd589..4fa24275f1 100644 --- a/src/drivers/ardrone_interface/ardrone_motor_control.c +++ b/src/drivers/ardrone_interface/ardrone_motor_control.c @@ -301,7 +301,7 @@ int ar_init_motors(int ardrone_uart, int gpios) ardrone_write_motor_commands(ardrone_uart, 0, 0, 0, 0); if (errcounter != 0) { - fprintf(stderr, "[ardrone_interface] init sequence incomplete, failed %d times", -errcounter); + warnx("Failed %d times", -errcounter); fflush(stdout); } return errcounter; From 8d5225b9679c2820a57ea30ed6d4c46f5d6c9baa Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 15 Nov 2014 16:41:18 +0100 Subject: [PATCH 089/172] FrSky: Be less chatty --- src/drivers/frsky_telemetry/frsky_telemetry.c | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/src/drivers/frsky_telemetry/frsky_telemetry.c b/src/drivers/frsky_telemetry/frsky_telemetry.c index 6e08390436..bccdf11906 100644 --- a/src/drivers/frsky_telemetry/frsky_telemetry.c +++ b/src/drivers/frsky_telemetry/frsky_telemetry.c @@ -84,7 +84,7 @@ static int frsky_open_uart(const char *uart_name, struct termios *uart_config_or /* Back up the original UART configuration to restore it after exit */ int termios_state; if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) { - warnx("ERROR get termios config %s: %d\n", uart_name, termios_state); + warnx("ERR: tcgetattr%s: %d\n", uart_name, termios_state); close(uart); return -1; } @@ -100,13 +100,13 @@ static int frsky_open_uart(const char *uart_name, struct termios *uart_config_or static const speed_t speed = B9600; if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) { - warnx("ERROR setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)\n", uart_name, termios_state); + warnx("ERR: %s: %d (cfsetispeed, cfsetospeed)\n", uart_name, termios_state); close(uart); return -1; } if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) { - warnx("ERROR setting baudrate / termios config for %s (tcsetattr)\n", uart_name); + warnx("ERR: %s (tcsetattr)\n", uart_name); close(uart); return -1; } @@ -151,9 +151,6 @@ static int frsky_telemetry_thread_main(int argc, char *argv[]) } } - /* Print welcome text */ - warnx("FrSky telemetry interface starting..."); - /* Open UART */ struct termios uart_config_original; const int uart = frsky_open_uart(device_name, &uart_config_original); From ef27225534df6cca0b5a98b85c6b7cf3364493c0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 15 Nov 2014 16:41:30 +0100 Subject: [PATCH 090/172] HIL: Be less chatty --- src/drivers/hil/hil.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/drivers/hil/hil.cpp b/src/drivers/hil/hil.cpp index f0dc0c6516..9b5c8133b4 100644 --- a/src/drivers/hil/hil.cpp +++ b/src/drivers/hil/hil.cpp @@ -442,8 +442,6 @@ HIL::task_main() /* make sure servos are off */ // up_pwm_servo_deinit(); - log("stopping"); - /* note - someone else is responsible for restoring the GPIO config */ /* tell the dtor that we are exiting */ From 5af710221a2948ab14f95225b4cb6c7fe5c66560 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 15 Nov 2014 16:54:34 +0100 Subject: [PATCH 091/172] HoTT: Less chatty --- src/drivers/hott/comms.cpp | 8 ++++---- src/drivers/hott/hott_sensors/hott_sensors.cpp | 6 +++--- src/drivers/hott/hott_telemetry/hott_telemetry.cpp | 6 +++--- 3 files changed, 10 insertions(+), 10 deletions(-) diff --git a/src/drivers/hott/comms.cpp b/src/drivers/hott/comms.cpp index cb8bbba37e..60a49b559c 100644 --- a/src/drivers/hott/comms.cpp +++ b/src/drivers/hott/comms.cpp @@ -55,7 +55,7 @@ open_uart(const char *device) const int uart = open(device, O_RDWR | O_NOCTTY); if (uart < 0) { - err(1, "Error opening port: %s", device); + err(1, "ERR: opening %s", device); } /* Back up the original uart configuration to restore it after exit */ @@ -63,7 +63,7 @@ open_uart(const char *device) struct termios uart_config_original; if ((termios_state = tcgetattr(uart, &uart_config_original)) < 0) { close(uart); - err(1, "Error getting baudrate / termios config for %s: %d", device, termios_state); + err(1, "ERR: %s: %d", device, termios_state); } /* Fill the struct for the new configuration */ @@ -76,13 +76,13 @@ open_uart(const char *device) /* Set baud rate */ if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) { close(uart); - err(1, "Error setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)", + err(1, "ERR: %s: %d (cfsetispeed, cfsetospeed)", device, termios_state); } if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) { close(uart); - err(1, "Error setting baudrate / termios config for %s (tcsetattr)", device); + err(1, "ERR: %s (tcsetattr)", device); } /* Activate single wire mode */ diff --git a/src/drivers/hott/hott_sensors/hott_sensors.cpp b/src/drivers/hott/hott_sensors/hott_sensors.cpp index a3d3a39333..8ab9d8d55f 100644 --- a/src/drivers/hott/hott_sensors/hott_sensors.cpp +++ b/src/drivers/hott/hott_sensors/hott_sensors.cpp @@ -204,7 +204,7 @@ hott_sensors_main(int argc, char *argv[]) if (!strcmp(argv[1], "start")) { if (thread_running) { - warnx("deamon already running"); + warnx("already running"); exit(0); } @@ -225,10 +225,10 @@ hott_sensors_main(int argc, char *argv[]) if (!strcmp(argv[1], "status")) { if (thread_running) { - warnx("daemon is running"); + warnx("is running"); } else { - warnx("daemon not started"); + warnx("not started"); } exit(0); diff --git a/src/drivers/hott/hott_telemetry/hott_telemetry.cpp b/src/drivers/hott/hott_telemetry/hott_telemetry.cpp index d293f9954e..edbb14172e 100644 --- a/src/drivers/hott/hott_telemetry/hott_telemetry.cpp +++ b/src/drivers/hott/hott_telemetry/hott_telemetry.cpp @@ -230,7 +230,7 @@ hott_telemetry_main(int argc, char *argv[]) if (!strcmp(argv[1], "start")) { if (thread_running) { - warnx("deamon already running"); + warnx("already running"); exit(0); } @@ -251,10 +251,10 @@ hott_telemetry_main(int argc, char *argv[]) if (!strcmp(argv[1], "status")) { if (thread_running) { - warnx("daemon is running"); + warnx("is running"); } else { - warnx("daemon not started"); + warnx("not started"); } exit(0); From 1fc7b588945b3bb1df70de1f779201c60a803b71 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 15 Nov 2014 16:54:47 +0100 Subject: [PATCH 092/172] Bottle drop: Less chatty --- src/modules/bottle_drop/bottle_drop.cpp | 19 +++++++++---------- 1 file changed, 9 insertions(+), 10 deletions(-) diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp index 31c9157e11..6d24e5d2d1 100644 --- a/src/modules/bottle_drop/bottle_drop.cpp +++ b/src/modules/bottle_drop/bottle_drop.cpp @@ -283,7 +283,6 @@ BottleDrop::drop() // force the door open if we have to if (_doors_opened == 0) { open_bay(); - warnx("bay not ready, forced open"); } while (hrt_elapsed_time(&_doors_opened) < 500 * 1000 && hrt_elapsed_time(&starttime) < 2000000) { @@ -723,16 +722,16 @@ BottleDrop::handle_command(struct vehicle_command_s *cmd) if (cmd->param1 > 0.5f && cmd->param2 > 0.5f) { open_bay(); drop(); - mavlink_log_info(_mavlink_fd, "#audio: drop bottle"); + mavlink_log_critical(_mavlink_fd, "drop bottle"); } else if (cmd->param1 > 0.5f) { open_bay(); - mavlink_log_info(_mavlink_fd, "#audio: opening bay"); + mavlink_log_critical(_mavlink_fd, "opening bay"); } else { lock_release(); close_bay(); - mavlink_log_info(_mavlink_fd, "#audio: closing bay"); + mavlink_log_critical(_mavlink_fd, "closing bay"); } answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); @@ -743,12 +742,12 @@ BottleDrop::handle_command(struct vehicle_command_s *cmd) switch ((int)(cmd->param1 + 0.5f)) { case 0: _drop_approval = false; - mavlink_log_info(_mavlink_fd, "#audio: got drop position, no approval"); + mavlink_log_critical(_mavlink_fd, "got drop position, no approval"); break; case 1: _drop_approval = true; - mavlink_log_info(_mavlink_fd, "#audio: got drop position and approval"); + mavlink_log_critical(_mavlink_fd, "got drop position and approval"); break; default: @@ -818,19 +817,19 @@ BottleDrop::answer_command(struct vehicle_command_s *cmd, enum VEHICLE_CMD_RESUL break; case VEHICLE_CMD_RESULT_DENIED: - mavlink_log_critical(_mavlink_fd, "#audio: command denied: %u", cmd->command); + mavlink_log_critical(_mavlink_fd, "command denied: %u", cmd->command); break; case VEHICLE_CMD_RESULT_FAILED: - mavlink_log_critical(_mavlink_fd, "#audio: command failed: %u", cmd->command); + mavlink_log_critical(_mavlink_fd, "command failed: %u", cmd->command); break; case VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED: - mavlink_log_critical(_mavlink_fd, "#audio: command temporarily rejected: %u", cmd->command); + mavlink_log_critical(_mavlink_fd, "command temporarily rejected: %u", cmd->command); break; case VEHICLE_CMD_RESULT_UNSUPPORTED: - mavlink_log_critical(_mavlink_fd, "#audio: command unsupported: %u", cmd->command); + mavlink_log_critical(_mavlink_fd, "command unsupported: %u", cmd->command); break; default: From beee7a89ed6e7a7c98ee712ab571c58eb6f3a692 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 15 Nov 2014 16:55:04 +0100 Subject: [PATCH 093/172] Airspeed: less chatty --- src/drivers/meas_airspeed/meas_airspeed.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index 1d9a463ad3..ba46de3798 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -519,7 +519,7 @@ test() ret = poll(&fds, 1, 2000); if (ret != 1) { - errx(1, "timed out waiting for sensor data"); + errx(1, "timed out"); } /* now go get it */ From d602c9a0c5eff39e0f533411b45f631cae63e486 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 15 Nov 2014 16:55:18 +0100 Subject: [PATCH 094/172] Controllib: Optimize for size --- src/modules/controllib/module.mk | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/controllib/module.mk b/src/modules/controllib/module.mk index f0139a4b70..2860d1efce 100644 --- a/src/modules/controllib/module.mk +++ b/src/modules/controllib/module.mk @@ -39,3 +39,5 @@ SRCS = test_params.c \ block/BlockParam.cpp \ uorb/blocks.cpp \ blocks.cpp + +MAXOPTIMIZATION = -Os From 9c7503ba7a3f5d8f98ab3017d221192489d0b3c9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 15 Nov 2014 18:38:33 +0100 Subject: [PATCH 095/172] uORB: Save space, does not do complex operations --- src/modules/uORB/module.mk | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/uORB/module.mk b/src/modules/uORB/module.mk index 0c29101fec..9385ce253c 100644 --- a/src/modules/uORB/module.mk +++ b/src/modules/uORB/module.mk @@ -44,3 +44,5 @@ SRCS = uORB.cpp \ objects_common.cpp \ Publication.cpp \ Subscription.cpp + +MAXOPTIMIZATION = -Os From 488739cc467cea08374e97f96f70317376ec58ec Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 15 Nov 2014 19:13:46 +0100 Subject: [PATCH 096/172] Fix up ver command handling to print MCU version on all command as well --- src/systemcmds/ver/ver.c | 66 +++++++++++++++++++++++----------------- 1 file changed, 38 insertions(+), 28 deletions(-) diff --git a/src/systemcmds/ver/ver.c b/src/systemcmds/ver/ver.c index 62a7a5b92a..eebeb9289c 100644 --- a/src/systemcmds/ver/ver.c +++ b/src/systemcmds/ver/ver.c @@ -46,7 +46,7 @@ #include #include -// string constants for version commands +/* string constants for version commands */ static const char sz_ver_hw_str[] = "hw"; static const char sz_ver_hwcmp_str[] = "hwcmp"; static const char sz_ver_git_str[] = "git"; @@ -68,49 +68,56 @@ __EXPORT int ver_main(int argc, char *argv[]); int ver_main(int argc, char *argv[]) { - int ret = 1; //defaults to an error + /* defaults to an error */ + int ret = 1; - // first check if there are at least 2 params + /* first check if there are at least 2 params */ if (argc >= 2) { if (argv[1] != NULL) { - if (!strncmp(argv[1], sz_ver_hw_str, sizeof(sz_ver_hw_str))) { - printf("%s\n", HW_ARCH); - ret = 0; - } else if (!strncmp(argv[1], sz_ver_hwcmp_str, sizeof(sz_ver_hwcmp_str))) { + if (!strncmp(argv[1], sz_ver_hwcmp_str, sizeof(sz_ver_hwcmp_str))) { if (argc >= 3 && argv[2] != NULL) { - // compare 3rd parameter with HW_ARCH string, in case of match, return 0 + /* compare 3rd parameter with HW_ARCH string, in case of match, return 0 */ ret = strncmp(HW_ARCH, argv[2], strlen(HW_ARCH)); if (ret == 0) { printf("ver hwcmp match: %s\n", HW_ARCH); + return ret; } } else { errx(1, "Not enough arguments, try 'ver hwcmp PX4FMU_V2'"); } + } - } else if (!strncmp(argv[1], sz_ver_git_str, sizeof(sz_ver_git_str))) { - printf("%s\n", FW_GIT); - ret = 0; + /* check if we want to show all */ + bool show_all = !strncmp(argv[1], sz_ver_all_str, sizeof(sz_ver_all_str)) - } else if (!strncmp(argv[1], sz_ver_bdate_str, sizeof(sz_ver_bdate_str))) { - printf("%s %s\n", __DATE__, __TIME__); - ret = 0; - - } else if (!strncmp(argv[1], sz_ver_gcc_str, sizeof(sz_ver_gcc_str))) { - printf("%s\n", __VERSION__); - ret = 0; - - } else if (!strncmp(argv[1], sz_ver_all_str, sizeof(sz_ver_all_str))) { + if (show_all || !strncmp(argv[1], sz_ver_hw_str, sizeof(sz_ver_hw_str))) { printf("HW arch: %s\n", HW_ARCH); - printf("Build datetime: %s %s\n", __DATE__, __TIME__); - printf("FW git-hash: %s\n", FW_GIT); - printf("GCC toolchain: %s\n", __VERSION__); ret = 0; + } - } else if (!strncmp(argv[1], mcu_ver_str, sizeof(mcu_ver_str))) { + if (show_all || !strncmp(argv[1], sz_ver_git_str, sizeof(sz_ver_git_str))) { + printf("FW git-hash: %s\n", FW_GIT); + ret = 0; + + } + + if (show_all || !strncmp(argv[1], sz_ver_bdate_str, sizeof(sz_ver_bdate_str))) { + printf("Build datetime: %s %s\n", __DATE__, __TIME__); + ret = 0; + + } + + if (show_all || !strncmp(argv[1], sz_ver_gcc_str, sizeof(sz_ver_gcc_str))) { + printf("Toolchain: %s\n", __VERSION__); + ret = 0; + + } + + if (show_all || !strncmp(argv[1], mcu_ver_str, sizeof(mcu_ver_str))) { char rev; char* revstr; @@ -118,21 +125,24 @@ int ver_main(int argc, char *argv[]) int chip_version = mcu_version(&rev, &revstr); if (chip_version < 0) { - printf("UNKNOWN MCU"); + printf("UNKNOWN MCU\n"); ret = 1; } else { printf("MCU: %s, rev. %c\n", revstr, rev); if (chip_version < MCU_REV_STM32F4_REV_3) { - printf("\n\nWARNING WARNING WARNING!\n" + printf("\nWARNING WARNING WARNING!\n" "Revision %c has a silicon errata\n" "This device can only utilize a maximum of 1MB flash safely!\n" - "http://px4.io/help/errata\n", rev); + "http://px4.io/help/errata\n\n", rev); } } - } else { + ret = 0; + } + + if (ret = 1) { errx(1, "unknown command.\n"); } From ed0f28ed4191c296aa3592088dc7875a8246cabb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 15 Nov 2014 19:43:11 +0100 Subject: [PATCH 097/172] Fix ver command --- src/systemcmds/ver/ver.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/systemcmds/ver/ver.c b/src/systemcmds/ver/ver.c index eebeb9289c..55cec149f2 100644 --- a/src/systemcmds/ver/ver.c +++ b/src/systemcmds/ver/ver.c @@ -91,7 +91,7 @@ int ver_main(int argc, char *argv[]) } /* check if we want to show all */ - bool show_all = !strncmp(argv[1], sz_ver_all_str, sizeof(sz_ver_all_str)) + bool show_all = !strncmp(argv[1], sz_ver_all_str, sizeof(sz_ver_all_str)); if (show_all || !strncmp(argv[1], sz_ver_hw_str, sizeof(sz_ver_hw_str))) { printf("HW arch: %s\n", HW_ARCH); From e2551d491754b752b8b835a302acaf3f4cf1f9d6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 15 Nov 2014 20:05:54 +0100 Subject: [PATCH 098/172] Fixed typo in ver command --- src/systemcmds/ver/ver.c | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/systemcmds/ver/ver.c b/src/systemcmds/ver/ver.c index 55cec149f2..c794f5819e 100644 --- a/src/systemcmds/ver/ver.c +++ b/src/systemcmds/ver/ver.c @@ -126,7 +126,6 @@ int ver_main(int argc, char *argv[]) if (chip_version < 0) { printf("UNKNOWN MCU\n"); - ret = 1; } else { printf("MCU: %s, rev. %c\n", revstr, rev); @@ -142,7 +141,7 @@ int ver_main(int argc, char *argv[]) ret = 0; } - if (ret = 1) { + if (ret == 1) { errx(1, "unknown command.\n"); } From 18dc5e342905f6171b54adba6de8a27d65d62ab2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 15 Nov 2014 20:07:32 +0100 Subject: [PATCH 099/172] Fix compile warning in MAVLink app --- src/modules/mavlink/mavlink_messages.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index b4911427f6..bed915bbe1 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -382,11 +382,11 @@ protected: clock_gettime(CLOCK_REALTIME, &ts); /* use GPS time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.bin */ time_t gps_time_sec = ts.tv_sec + (ts.tv_nsec / 1e9); - struct tm t; - gmtime_r(&gps_time_sec, &t); + struct tm tt; + gmtime_r(&gps_time_sec, &tt); // XXX we do not want to interfere here with the SD log app - strftime(log_file_name, sizeof(log_file_name), "msgs_%Y_%m_%d_%H_%M_%S.txt", &t); + 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"); } From c0f32d44a2acd0f840b4246f90816a95dc6a7527 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 16 Nov 2014 00:15:40 +0100 Subject: [PATCH 100/172] First stab at animation --- src/modules/px4iofirmware/px4io.c | 40 ++++++++++++++++++++++++++++++- 1 file changed, 39 insertions(+), 1 deletion(-) diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c index 30f32b38e7..51281f6609 100644 --- a/src/modules/px4iofirmware/px4io.c +++ b/src/modules/px4iofirmware/px4io.c @@ -125,7 +125,45 @@ heartbeat_blink(void) static bool heartbeat = false; LED_BLUE(heartbeat = !heartbeat); #ifdef GPIO_LED4 - LED_RING(heartbeat); + + const unsigned max_brightness = 1000; + + static unsigned counter = 0; + static unsigned brightness = max_brightness; + static unsigned brightness_counter = 0; + static unsigned on_counter = 0; + + if (brightness_counter < max_brightness) { + + bool on = ((on_counter * 100) / brightness_counter) <= ((brightness * 100) / max_brightness); + + LED_RING(on); + brightness_counter++; + + if (on) { + on_counter++; + } + + } else { + + if (counter >= 62) { + counter = 0; + } + + int n; + + if (counter < 32) { + n = counter; + + } else { + n = 62 - counter; + } + + brightness = n * n;// designed to be ~1000 / (31.0f * 31.0f); + brightness_counter = 0; + on_counter = 0; + } + #endif } From 63707ef058f01e324d8d74cf6bc48635e09ab507 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 16 Nov 2014 00:57:23 +0100 Subject: [PATCH 101/172] Let it breathe --- src/modules/px4iofirmware/px4io.c | 19 ++++++++++++++++--- 1 file changed, 16 insertions(+), 3 deletions(-) diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c index 51281f6609..8abfb0d2e0 100644 --- a/src/modules/px4iofirmware/px4io.c +++ b/src/modules/px4iofirmware/px4io.c @@ -85,6 +85,9 @@ static volatile uint8_t msg_next_out, msg_next_in; #define NUM_MSG 2 static char msg[NUM_MSG][40]; +static void heartbeat_blink(void); +static void ring_blink(void); + /* * add a debug message to be printed on the console */ @@ -124,9 +127,16 @@ heartbeat_blink(void) { static bool heartbeat = false; LED_BLUE(heartbeat = !heartbeat); +} + +static void +ring_blink(void) +{ #ifdef GPIO_LED4 - const unsigned max_brightness = 1000; + // XXX this led code does have + // intentionally a few magic numbers. + const unsigned max_brightness = 118; static unsigned counter = 0; static unsigned brightness = max_brightness; @@ -135,7 +145,7 @@ heartbeat_blink(void) if (brightness_counter < max_brightness) { - bool on = ((on_counter * 100) / brightness_counter) <= ((brightness * 100) / max_brightness); + bool on = ((on_counter * 100) / brightness_counter+1) <= ((brightness * 100) / max_brightness+1); LED_RING(on); brightness_counter++; @@ -159,9 +169,10 @@ heartbeat_blink(void) n = 62 - counter; } - brightness = n * n;// designed to be ~1000 / (31.0f * 31.0f); + brightness = (n * n) / 9; brightness_counter = 0; on_counter = 0; + counter++; } #endif @@ -338,6 +349,8 @@ user_start(int argc, char *argv[]) heartbeat_blink(); } + ring_blink(); + check_reboot(); /* check for debug activity (default: none) */ From 8583cf4dcc0e88842cdf717de26a4f0daa63729a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 16 Nov 2014 00:59:18 +0100 Subject: [PATCH 102/172] Let it breathe stronger --- src/modules/px4iofirmware/px4io.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c index 8abfb0d2e0..349aaac571 100644 --- a/src/modules/px4iofirmware/px4io.c +++ b/src/modules/px4iofirmware/px4io.c @@ -169,7 +169,7 @@ ring_blink(void) n = 62 - counter; } - brightness = (n * n) / 9; + brightness = (n * n) / 8; brightness_counter = 0; on_counter = 0; counter++; From 8caada2188b9c9a8fcab70fbad92ab14506c2427 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 16 Nov 2014 01:21:47 +0100 Subject: [PATCH 103/172] Breathe animation, led solid on arming --- src/modules/px4iofirmware/px4io.c | 14 ++++++++++++-- src/modules/px4iofirmware/px4io.h | 2 +- 2 files changed, 13 insertions(+), 3 deletions(-) diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c index 349aaac571..14ee9cb405 100644 --- a/src/modules/px4iofirmware/px4io.c +++ b/src/modules/px4iofirmware/px4io.c @@ -134,6 +134,12 @@ ring_blink(void) { #ifdef GPIO_LED4 + if (/* IO armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) + /* and FMU is armed */ && (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) { + LED_RING(1); + return; + } + // XXX this led code does have // intentionally a few magic numbers. const unsigned max_brightness = 118; @@ -147,7 +153,11 @@ ring_blink(void) bool on = ((on_counter * 100) / brightness_counter+1) <= ((brightness * 100) / max_brightness+1); - LED_RING(on); + // XXX once led is PWM driven, + // remove the ! in the line below + // to return to the proper breathe + // animation / pattern (currently inverted) + LED_RING(!on); brightness_counter++; if (on) { @@ -349,7 +359,7 @@ user_start(int argc, char *argv[]) heartbeat_blink(); } - ring_blink(); + ring_blink(); check_reboot(); diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index 8186e4c78b..93a33490fa 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -140,7 +140,7 @@ extern pwm_limit_t pwm_limit; #define LED_BLUE(_s) stm32_gpiowrite(GPIO_LED1, !(_s)) #define LED_AMBER(_s) stm32_gpiowrite(GPIO_LED2, !(_s)) #define LED_SAFETY(_s) stm32_gpiowrite(GPIO_LED3, !(_s)) -#define LED_RING(_s) stm32_gpiowrite(GPIO_LED4, !(_s)) +#define LED_RING(_s) stm32_gpiowrite(GPIO_LED4, (_s)) #ifdef CONFIG_ARCH_BOARD_PX4IO_V1 From 4e8387465e01cf16e0fbbe94b5e9a3c168540b77 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sun, 16 Nov 2014 13:38:41 +1000 Subject: [PATCH 104/172] navigator: only update sensor_combined topic with 50Hz --- src/modules/navigator/navigator_main.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index e933919fb4..ed9e999a97 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -289,8 +289,9 @@ Navigator::task_main() navigation_capabilities_update(); params_update(); - /* rate limit position updates to 50 Hz */ + /* rate limit position and sensor updates to 50 Hz */ orb_set_interval(_global_pos_sub, 20); + orb_set_interval(_sensor_combined_sub, 20); hrt_abstime mavlink_open_time = 0; const hrt_abstime mavlink_open_interval = 500000; From 4d69314a9925be8ded1b9cb723bd01c21f01a816 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 16 Nov 2014 10:55:18 +0100 Subject: [PATCH 105/172] Adjust navigator priority --- src/modules/navigator/navigator_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index ed9e999a97..3ca6ac2c0e 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -498,7 +498,7 @@ Navigator::start() /* start the task */ _navigator_task = task_spawn_cmd("navigator", SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, + SCHED_PRIORITY_DEFAULT + 20, 2000, (main_t)&Navigator::task_main_trampoline, nullptr); From 88bae21ce53fc6acadaae4c8271f3d0fce18a721 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 5 Nov 2014 21:20:32 +1100 Subject: [PATCH 106/172] px4io: added RC_CONFIG, SET_OVERRIDE_OK and CLEAR_OVERRIDE_OK ioctls this allows for full setup of RC override without needing param_get() to PX4 specific parameters --- src/drivers/drv_pwm_output.h | 26 ++++++++++++++++++++++++++ src/drivers/px4io/px4io.cpp | 36 ++++++++++++++++++++++++++++++++++++ 2 files changed, 62 insertions(+) diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index 6873f24b60..b41f088eb9 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -117,6 +117,23 @@ struct pwm_output_values { unsigned channel_count; }; + +/** + * RC config values for a channel + * + * This allows for PX4IO_PAGE_RC_CONFIG values to be set without a + * param_get() dependency + */ +struct pwm_output_rc_config { + uint8_t channel; + uint16_t rc_min; + uint16_t rc_trim; + uint16_t rc_max; + uint16_t rc_dz; + uint16_t rc_assignment; + bool rc_reverse; +}; + /* * ORB tag for PWM outputs. */ @@ -216,6 +233,15 @@ ORB_DECLARE(output_pwm); /** force safety switch on (to enable use of safety switch) */ #define PWM_SERVO_SET_FORCE_SAFETY_ON _IOC(_PWM_SERVO_BASE, 26) +/** set RC config for a channel. This takes a pointer to pwm_output_rc_config */ +#define PWM_SERVO_SET_RC_CONFIG _IOC(_PWM_SERVO_BASE, 27) + +/** set the 'OVERRIDE OK' bit, which allows for RC control on FMU loss */ +#define PWM_SERVO_SET_OVERRIDE_OK _IOC(_PWM_SERVO_BASE, 28) + +/** clear the 'OVERRIDE OK' bit, which allows for RC control on FMU loss */ +#define PWM_SERVO_CLEAR_OVERRIDE_OK _IOC(_PWM_SERVO_BASE, 29) + /* * * diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 06faf49a4f..dc33f7657e 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -2572,6 +2572,42 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg) break; + case PWM_SERVO_SET_RC_CONFIG: { + /* enable setting of RC configuration without relying + on param_get() + */ + struct pwm_output_rc_config* config = (struct pwm_output_rc_config*)arg; + if (config->channel >= _max_actuators) { + /* fail with error */ + return E2BIG; + } + + /* copy values to registers in IO */ + uint16_t regs[PX4IO_P_RC_CONFIG_STRIDE]; + uint16_t offset = config->channel * PX4IO_P_RC_CONFIG_STRIDE; + regs[PX4IO_P_RC_CONFIG_MIN] = config->rc_min; + regs[PX4IO_P_RC_CONFIG_CENTER] = config->rc_trim; + regs[PX4IO_P_RC_CONFIG_MAX] = config->rc_max; + regs[PX4IO_P_RC_CONFIG_DEADZONE] = config->rc_dz; + regs[PX4IO_P_RC_CONFIG_ASSIGNMENT] = config->rc_assignment; + regs[PX4IO_P_RC_CONFIG_OPTIONS] = PX4IO_P_RC_CONFIG_OPTIONS_ENABLED; + if (config->rc_reverse) { + regs[PX4IO_P_RC_CONFIG_OPTIONS] |= PX4IO_P_RC_CONFIG_OPTIONS_REVERSE; + } + ret = io_reg_set(PX4IO_PAGE_RC_CONFIG, offset, regs, PX4IO_P_RC_CONFIG_STRIDE); + break; + } + + case PWM_SERVO_SET_OVERRIDE_OK: + /* set the 'OVERRIDE OK' bit */ + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK); + break; + + case PWM_SERVO_CLEAR_OVERRIDE_OK: + /* clear the 'OVERRIDE OK' bit */ + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK, 0); + break; + default: /* see if the parent class can make any use of it */ ret = CDev::ioctl(filep, cmd, arg); From 6406e235d6f5cb37a914442036111f616b4b3839 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 6 Nov 2014 10:32:21 +1100 Subject: [PATCH 107/172] px4io: publish pwm values when STATUS_FLAGS_FMU_OK is not set this allows testing of FMU failure behaviour in px4io by monitoring the reported PWM output when the vehicle code stops sending updates. Otherwise testing needs to be done with "px4io status" which is very tedious. With this change a GCS can monitor the PWM outputs from the failsafe mixer using normal mavlink messages --- src/drivers/px4io/px4io.cpp | 4 ---- 1 file changed, 4 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index dc33f7657e..6a313b322b 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1657,10 +1657,6 @@ PX4IO::io_publish_raw_rc() int PX4IO::io_publish_pwm_outputs() { - /* if no FMU comms(!) just don't publish */ - if (!(_status & PX4IO_P_STATUS_FLAGS_FMU_OK)) - return OK; - /* data we are going to fetch */ actuator_outputs_s outputs; outputs.timestamp = hrt_absolute_time(); From ba811254536b6a080d4b3379106bf0e648a51eb0 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 5 Nov 2014 21:20:03 +1100 Subject: [PATCH 108/172] px4io: added OVERRIDE_IMMEDIATE arming flag this allows the flight code to choose whether FMU failure gives immediate manual pilot control, or waits for the mode switch to go past the override threshold --- src/drivers/drv_pwm_output.h | 3 +++ src/drivers/px4io/px4io.cpp | 18 ++++++++++++++++-- src/modules/px4iofirmware/controls.c | 9 +++++++++ src/modules/px4iofirmware/protocol.h | 1 + src/modules/px4iofirmware/registers.c | 3 ++- 5 files changed, 31 insertions(+), 3 deletions(-) diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index b41f088eb9..edb72f04ed 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -242,6 +242,9 @@ ORB_DECLARE(output_pwm); /** clear the 'OVERRIDE OK' bit, which allows for RC control on FMU loss */ #define PWM_SERVO_CLEAR_OVERRIDE_OK _IOC(_PWM_SERVO_BASE, 29) +/** setup OVERRIDE_IMMEDIATE behaviour on FMU fail */ +#define PWM_SERVO_SET_OVERRIDE_IMMEDIATE _IOC(_PWM_SERVO_BASE, 30) + /* * * diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 6a313b322b..519ba663a1 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -2057,7 +2057,7 @@ PX4IO::print_status(bool extended_status) ((features & PX4IO_P_SETUP_FEATURES_ADC_RSSI) ? " RSSI_ADC" : "") ); uint16_t arming = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING); - printf("arming 0x%04x%s%s%s%s%s%s%s%s\n", + printf("arming 0x%04x%s%s%s%s%s%s%s%s%s%s\n", arming, ((arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) ? " FMU_ARMED" : " FMU_DISARMED"), ((arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK) ? " IO_ARM_OK" : " IO_ARM_DENIED"), @@ -2067,7 +2067,8 @@ PX4IO::print_status(bool extended_status) ((arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE) ? " ALWAYS_PWM_ENABLE" : ""), ((arming & PX4IO_P_SETUP_ARMING_LOCKDOWN) ? " LOCKDOWN" : ""), ((arming & PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE) ? " FORCE_FAILSAFE" : ""), - ((arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE) ? " TERM_FAILSAFE" : "") + ((arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE) ? " TERM_FAILSAFE" : ""), + ((arming & PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE) ? " OVERRIDE_IMMEDIATE" : "") ); #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 printf("rates 0x%04x default %u alt %u relays 0x%04x\n", @@ -2307,6 +2308,19 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg) } break; + case PWM_SERVO_SET_OVERRIDE_IMMEDIATE: + /* control whether override on FMU failure is + immediate or waits for override threshold on mode + switch */ + if (arg == 0) { + /* clear override immediate flag */ + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE, 0); + } else { + /* set override immediate flag */ + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE); + } + break; + case DSM_BIND_START: /* only allow DSM2, DSM-X and DSM-X with more than 7 channels */ diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index ad60ee03e0..e3cb8d820c 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -417,6 +417,15 @@ controls_tick() { if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && (REG_TO_SIGNED(rc_value_override) < RC_CHANNEL_LOW_THRESH)) override = true; + /* + if the FMU is dead then enable override if we have a + mixer and OVERRIDE_IMMEDIATE is set + */ + if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) && + (r_setup_arming & PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE) && + (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) + override = true; + if (override) { r_status_flags |= PX4IO_P_STATUS_FLAGS_OVERRIDE; diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index 9b2e047cbb..c7e9ae3eb3 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -183,6 +183,7 @@ #define PX4IO_P_SETUP_ARMING_LOCKDOWN (1 << 7) /* If set, the system operates normally, but won't actuate any servos */ #define PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE (1 << 8) /* If set, the system will always output the failsafe values */ #define PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE (1 << 9) /* If set, the system will never return from a failsafe, but remain in failsafe once triggered. */ +#define PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE (1 << 10) /* If set then on FMU failure override is immediate. Othewise it waits for the mode switch to go past the override thrshold */ #define PX4IO_P_SETUP_PWM_RATES 2 /* bitmask, 0 = low rate, 1 = high rate */ #define PX4IO_P_SETUP_PWM_DEFAULTRATE 3 /* 'low' PWM frame output rate in Hz */ diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 49c2a9f56b..fbfdd35db0 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -191,7 +191,8 @@ volatile uint16_t r_page_setup[] = PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED | \ PX4IO_P_SETUP_ARMING_LOCKDOWN | \ PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE | \ - PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE) + PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE | \ + PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE) #define PX4IO_P_SETUP_RATES_VALID ((1 << PX4IO_SERVO_COUNT) - 1) #define PX4IO_P_SETUP_RELAYS_VALID ((1 << PX4IO_RELAY_CHANNELS) - 1) From f533c362515e31b536c34d83342c396e54a7d652 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 5 Nov 2014 21:20:32 +1100 Subject: [PATCH 109/172] px4io: added RC_CONFIG, SET_OVERRIDE_OK and CLEAR_OVERRIDE_OK ioctls this allows for full setup of RC override without needing param_get() to PX4 specific parameters Conflicts: src/drivers/drv_pwm_output.h --- src/drivers/drv_pwm_output.h | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index edb72f04ed..b10c3e18a9 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -231,16 +231,16 @@ ORB_DECLARE(output_pwm); #define PWM_SERVO_SET_TERMINATION_FAILSAFE _IOC(_PWM_SERVO_BASE, 25) /** force safety switch on (to enable use of safety switch) */ -#define PWM_SERVO_SET_FORCE_SAFETY_ON _IOC(_PWM_SERVO_BASE, 26) +#define PWM_SERVO_SET_FORCE_SAFETY_ON _IOC(_PWM_SERVO_BASE, 26) /** set RC config for a channel. This takes a pointer to pwm_output_rc_config */ -#define PWM_SERVO_SET_RC_CONFIG _IOC(_PWM_SERVO_BASE, 27) +#define PWM_SERVO_SET_RC_CONFIG _IOC(_PWM_SERVO_BASE, 27) /** set the 'OVERRIDE OK' bit, which allows for RC control on FMU loss */ -#define PWM_SERVO_SET_OVERRIDE_OK _IOC(_PWM_SERVO_BASE, 28) +#define PWM_SERVO_SET_OVERRIDE_OK _IOC(_PWM_SERVO_BASE, 28) /** clear the 'OVERRIDE OK' bit, which allows for RC control on FMU loss */ -#define PWM_SERVO_CLEAR_OVERRIDE_OK _IOC(_PWM_SERVO_BASE, 29) +#define PWM_SERVO_CLEAR_OVERRIDE_OK _IOC(_PWM_SERVO_BASE, 29) /** setup OVERRIDE_IMMEDIATE behaviour on FMU fail */ #define PWM_SERVO_SET_OVERRIDE_IMMEDIATE _IOC(_PWM_SERVO_BASE, 30) From 852fa611730be2433156b010c6162606ec857ef3 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Sun, 16 Nov 2014 14:12:58 -0800 Subject: [PATCH 110/172] Correct setting of DSM and ST24 flags --- src/modules/px4iofirmware/controls.c | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 3fd73fc601..75d80ba5d4 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -71,7 +71,6 @@ bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated) uint8_t *bytes; *dsm_updated = dsm_input(r_raw_rc_values, &temp_count, &n_bytes, &bytes); if (*dsm_updated) { - r_raw_rc_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM; r_raw_rc_count = temp_count & 0x7fff; if (temp_count & 0x8000) r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_RC_DSM11; @@ -172,6 +171,12 @@ controls_tick() { perf_begin(c_gather_dsm); bool dsm_updated, st24_updated; (void)dsm_port_input(&rssi, &dsm_updated, &st24_updated); + if (dsm_updated) { + r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM; + } + if (st24_updated) { + r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_ST24; + } perf_end(c_gather_dsm); perf_begin(c_gather_sbus); From 52d5a7c00aa74fe0d4241f7e6636dfd6e48d0acf Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 16 Nov 2014 23:24:52 +0100 Subject: [PATCH 111/172] Fix PWM command --- src/systemcmds/pwm/pwm.c | 68 ++++++++++++++++++++-------------------- 1 file changed, 34 insertions(+), 34 deletions(-) diff --git a/src/systemcmds/pwm/pwm.c b/src/systemcmds/pwm/pwm.c index 478c2a772f..eeba89fa85 100644 --- a/src/systemcmds/pwm/pwm.c +++ b/src/systemcmds/pwm/pwm.c @@ -74,34 +74,34 @@ usage(const char *reason) "usage:\n" "pwm arm|disarm|rate|failsafe|disarmed|min|max|test|info ...\n" "\n" - " arm Arm output\n" - " disarm Disarm output\n" + "arm\t\t\t\tArm output\n" + "disarm\t\t\t\tDisarm output\n" "\n" - " rate ... Configure PWM rates\n" - " [-g ] Channel group that should update at the alternate rate\n" - " [-m ] Directly supply channel mask\n" - " [-a] Configure all outputs\n" - " -r PWM rate (50 to 400 Hz)\n" + "rate ...\t\t\tConfigure PWM rates\n" + "\t[-g ]\t(e.g. 0,1,2)\n" + "\t[-m ]\t(e.g. 0xF)\n" + "\t[-a]\t\t\tConfigure all outputs\n" + "\t-r \t\tPWM rate (50 to 400 Hz)\n" "\n" - " failsafe ... Configure failsafe PWM values\n" - " disarmed ... Configure disarmed PWM values\n" - " min ... Configure minimum PWM values\n" - " max ... Configure maximum PWM values\n" - " [-c ] Supply channels (e.g. 1234)\n" - " [-m ] Directly supply channel mask (e.g. 0xF)\n" - " [-a] Configure all outputs\n" - " -p PWM value\n" + "failsafe ...\t\t\tFailsafe PWM\n" + "disarmed ...\t\t\tDisarmed PWM\n" + "min ...\t\t\t\tMinimum PWM\n" + "max ...\t\t\t\tMaximum PWM\n" + "\t[-c ]\t\t(e.g. 1234)\n" + "\t[-m ]\t(e.g. 0xF)\n" + "\t[-a]\t\t\tConfigure all outputs\n" + "\t-p \t\tPWM value\n" "\n" - " test ... Directly set PWM values\n" - " [-c ] Supply channels (e.g. 1234)\n" - " [-m ] Directly supply channel mask (e.g. 0xF)\n" - " [-a] Configure all outputs\n" - " -p PWM value\n" + "test ...\t\t\tDirectly set PWM\n" + "\t[-c ]\t\t(e.g. 1234)\n" + "\t[-m ]\t(e.g. 0xF)\n" + "\t[-a]\t\t\tConfigure all outputs\n" + "\t-p \t\tPWM value\n" "\n" - " info Print information about the PWM device\n" + "info\t\t\t\tPrint information\n" "\n" - " -v Print verbose information\n" - " -d PWM output device (defaults to " PWM_OUTPUT_DEVICE_PATH ")\n" + "\t-v\t\t\tVerbose\n" + "\t-d \t\t(default " PWM_OUTPUT_DEVICE_PATH ")\n" ); } @@ -123,7 +123,7 @@ pwm_main(int argc, char *argv[]) unsigned single_ch = 0; unsigned pwm_value = 0; - if (argc < 1) + if (argc < 2) usage(NULL); while ((ch = getopt(argc-1, &argv[1], "d:vc:g:m:ap:r:")) != EOF) { @@ -165,7 +165,7 @@ pwm_main(int argc, char *argv[]) /* Read in mask directly */ set_mask = strtoul(optarg, &ep, 0); if (*ep != '\0') - usage("bad set_mask value"); + usage("BAD set_mask VAL"); break; case 'a': @@ -176,12 +176,12 @@ pwm_main(int argc, char *argv[]) case 'p': pwm_value = strtoul(optarg, &ep, 0); if (*ep != '\0') - usage("bad PWM value provided"); + usage("BAD PWM VAL"); break; case 'r': alt_rate = strtoul(optarg, &ep, 0); if (*ep != '\0') - usage("bad alternative rate provided"); + usage("BAD rate VAL"); break; default: break; @@ -189,7 +189,7 @@ pwm_main(int argc, char *argv[]) } if (print_verbose && set_mask > 0) { - warnx("Chose channels: "); + warnx("Channels: "); printf(" "); for (unsigned i = 0; i Date: Mon, 17 Nov 2014 14:07:13 +0100 Subject: [PATCH 112/172] Fix output of ver hwcmd call --- src/systemcmds/ver/ver.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/systemcmds/ver/ver.c b/src/systemcmds/ver/ver.c index c794f5819e..2ead3e6323 100644 --- a/src/systemcmds/ver/ver.c +++ b/src/systemcmds/ver/ver.c @@ -82,8 +82,8 @@ int ver_main(int argc, char *argv[]) if (ret == 0) { printf("ver hwcmp match: %s\n", HW_ARCH); - return ret; } + return ret; } else { errx(1, "Not enough arguments, try 'ver hwcmp PX4FMU_V2'"); From f20f85f0e3c61ce754fd6d9d73745eaa925b3b04 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 17 Nov 2014 14:07:33 +0100 Subject: [PATCH 113/172] Do not spam filter resets in static mode --- .../ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 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 685f5e12f1..00900c995f 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 @@ -613,8 +613,11 @@ FixedwingEstimator::check_filter_state() warn_index = max_warn_index; } - warnx("reset: %s", feedback[warn_index]); - mavlink_log_critical(_mavlink_fd, "[ekf check] %s", feedback[warn_index]); + // Do not warn about accel offset if we have no position updates + if (!(warn_index == 5 && _ekf->staticMode)) { + warnx("reset: %s", feedback[warn_index]); + mavlink_log_critical(_mavlink_fd, "[ekf check] %s", feedback[warn_index]); + } } struct estimator_status_report rep; From 41fe04776f362cf9f0fae6451550df8d5eaf8d99 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 17 Nov 2014 14:59:36 +0100 Subject: [PATCH 114/172] Fix up stack sizes --- src/modules/bottle_drop/bottle_drop.cpp | 2 +- src/modules/bottle_drop/module.mk | 2 ++ .../ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 2 +- src/modules/mavlink/mavlink_main.cpp | 2 +- src/modules/navigator/navigator_main.cpp | 2 +- 5 files changed, 6 insertions(+), 4 deletions(-) diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp index 6d24e5d2d1..e0bcbc6e9e 100644 --- a/src/modules/bottle_drop/bottle_drop.cpp +++ b/src/modules/bottle_drop/bottle_drop.cpp @@ -223,7 +223,7 @@ BottleDrop::start() _main_task = task_spawn_cmd("bottle_drop", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT + 15, - 2048, + 1500, (main_t)&BottleDrop::task_main_trampoline, nullptr); diff --git a/src/modules/bottle_drop/module.mk b/src/modules/bottle_drop/module.mk index 9602288790..df9f5473b0 100644 --- a/src/modules/bottle_drop/module.mk +++ b/src/modules/bottle_drop/module.mk @@ -41,3 +41,5 @@ SRCS = bottle_drop.cpp \ bottle_drop_params.c MAXOPTIMIZATION = -Os + +MODULE_STACKSIZE = 1200 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 00900c995f..e7805daa91 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 @@ -1560,7 +1560,7 @@ FixedwingEstimator::start() _estimator_task = task_spawn_cmd("ekf_att_pos_estimator", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 40, - 5000, + 7500, (main_t)&FixedwingEstimator::task_main_trampoline, nullptr); diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index c251dd3b2c..fb9f65cf50 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1636,7 +1636,7 @@ Mavlink::start(int argc, char *argv[]) task_spawn_cmd(buf, SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, - 2900, + 2800, (main_t)&Mavlink::start_helper, (const char **)argv); diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 3ca6ac2c0e..10a4ee88f6 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -499,7 +499,7 @@ Navigator::start() _navigator_task = task_spawn_cmd("navigator", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT + 20, - 2000, + 1800, (main_t)&Navigator::task_main_trampoline, nullptr); From 97a1410ec99e880207e4ee6d2a03451c2e11f4cf Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 12 Nov 2014 09:03:50 +0100 Subject: [PATCH 115/172] Toolchain: Allow GCC 4.7 and 4.8 variants --- makefiles/toolchain_gnu-arm-eabi.mk | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/makefiles/toolchain_gnu-arm-eabi.mk b/makefiles/toolchain_gnu-arm-eabi.mk index 71c7fb49fa..84e5cd08a2 100644 --- a/makefiles/toolchain_gnu-arm-eabi.mk +++ b/makefiles/toolchain_gnu-arm-eabi.mk @@ -50,11 +50,11 @@ OBJDUMP = $(CROSSDEV)objdump # Check if the right version of the toolchain is available # -CROSSDEV_VER_SUPPORTED = 4.7 +CROSSDEV_VER_SUPPORTED = 4.7.4 4.7.5 4.7.6 4.8.4 CROSSDEV_VER_FOUND = $(shell $(CC) -dumpversion) -ifeq (,$(findstring $(CROSSDEV_VER_SUPPORTED),$(CROSSDEV_VER_FOUND))) -$(error Unsupported version of $(CC), found: $(CROSSDEV_VER_FOUND) instead of $(CROSSDEV_VER_SUPPORTED).x) +ifeq (,$(findstring $(CROSSDEV_VER_FOUND), $(CROSSDEV_VER_SUPPORTED))) +$(error Unsupported version of $(CC), found: $(CROSSDEV_VER_FOUND) instead of one in: $(CROSSDEV_VER_SUPPORTED)) endif From 5f6d03099e07e20abe9cec685fee1244d7c9a0d4 Mon Sep 17 00:00:00 2001 From: Steven Blass Date: Mon, 17 Nov 2014 19:58:18 -0500 Subject: [PATCH 116/172] 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 117/172] 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 118/172] 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 119/172] 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 120/172] 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 5c34f03c4ef1d0b928c8ad32f8038bb15dba7c11 Mon Sep 17 00:00:00 2001 From: philipoe Date: Thu, 20 Nov 2014 17:25:30 +0100 Subject: [PATCH 121/172] 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 122/172] 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 123/172] 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 124/172] 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 125/172] 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 126/172] 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 127/172] 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 128/172] 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 129/172] 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 130/172] 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 131/172] 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 132/172] 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 133/172] 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 134/172] 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 135/172] 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 136/172] 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 137/172] 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 138/172] 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 139/172] 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 140/172] 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 141/172] 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 142/172] 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 143/172] 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 144/172] 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 145/172] 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 146/172] 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 147/172] 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 148/172] 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 149/172] 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 150/172] 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 151/172] 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 152/172] 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 153/172] 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 154/172] 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 155/172] 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 156/172] 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 157/172] 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 158/172] 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 159/172] 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 160/172] 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 161/172] 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 162/172] 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 163/172] 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 164/172] 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 1da7ca7f78dfa6cd441df5df259b3c191f77fecb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 28 Nov 2014 14:23:08 +0100 Subject: [PATCH 165/172] 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 166/172] 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 e51f72000bb850f9933a5ba9c024dddb6248440c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 1 Dec 2014 11:27:59 +0100 Subject: [PATCH 167/172] 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 168/172] 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 169/172] 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 170/172] 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 7a344b933738d9caedc0fe177cc4615c294eaf51 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Wed, 3 Dec 2014 10:30:49 +0100 Subject: [PATCH 171/172] 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 172/172] 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