diff options
author | Thomas Gubler <thomasgubler@gmail.com> | 2015-01-28 08:15:01 +0100 |
---|---|---|
committer | Thomas Gubler <thomasgubler@gmail.com> | 2015-01-28 08:15:01 +0100 |
commit | 0974a9515b780768d5329dd20996767956b3066a (patch) | |
tree | 1439017e74398bd649f919b88d548b519be5ade4 | |
parent | cfe2609ab63bf88f3b251eaf35d10c664efdbd9e (diff) | |
parent | 52f3fe1d9af53e5f1dd40714aa177fcc4c5e0047 (diff) | |
download | px4-firmware-0974a9515b780768d5329dd20996767956b3066a.tar.gz px4-firmware-0974a9515b780768d5329dd20996767956b3066a.tar.bz2 px4-firmware-0974a9515b780768d5329dd20996767956b3066a.zip |
Merge remote-tracking branch 'upstream/master' into ros_messagelayer_merge_nuttx_bringupros_messagelayer_merge_nuttx_bringup
Conflicts:
nuttx-configs/px4fmu-v1/nsh/defconfig
-rw-r--r-- | Documentation/px4_block_diagram.odg | bin | 42200 -> 91712 bytes | |||
-rw-r--r-- | ROMFS/px4fmu_common/init.d/rcS | 26 | ||||
-rw-r--r-- | makefiles/config_px4fmu-v1_default.mk | 1 | ||||
-rw-r--r-- | nuttx-configs/px4fmu-v1/nsh/defconfig | 2 | ||||
-rw-r--r-- | src/drivers/drv_range_finder.h | 3 | ||||
-rw-r--r-- | src/drivers/hmc5883/hmc5883.cpp | 4 | ||||
-rw-r--r-- | src/drivers/mb12xx/mb12xx.cpp | 189 | ||||
-rw-r--r-- | src/drivers/mpu6000/mpu6000.cpp | 38 | ||||
-rw-r--r-- | src/drivers/px4flow/i2c_frame.h | 82 | ||||
-rw-r--r-- | src/drivers/px4flow/px4flow.cpp | 88 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_receiver.cpp | 2 | ||||
-rw-r--r-- | src/modules/mc_pos_control/mc_pos_control_main.cpp | 9 | ||||
-rw-r--r-- | src/modules/navigator/geofence.cpp | 116 | ||||
-rw-r--r-- | src/modules/position_estimator_inav/position_estimator_inav_main.c | 3 | ||||
-rw-r--r-- | src/modules/position_estimator_inav/position_estimator_inav_params.c | 12 | ||||
-rw-r--r-- | src/modules/position_estimator_inav/position_estimator_inav_params.h | 2 | ||||
-rw-r--r-- | src/systemcmds/nshterm/module.mk | 2 | ||||
-rw-r--r-- | src/systemcmds/nshterm/nshterm.c | 6 |
18 files changed, 393 insertions, 192 deletions
diff --git a/Documentation/px4_block_diagram.odg b/Documentation/px4_block_diagram.odg Binary files differindex 95b6f600b..e66e1b1cd 100644 --- a/Documentation/px4_block_diagram.odg +++ b/Documentation/px4_block_diagram.odg diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 043e0bc17..d14d6e48d 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -6,6 +6,11 @@ # # +# Start CDC/ACM serial driver +# +sercon + +# # Default to auto-start mode. # set MODE autostart @@ -43,29 +48,8 @@ else fi unset FRC -# if this is an APM build then there will be a rc.APM script -# from an EXTERNAL_SCRIPTS build option -if [ -f /etc/init.d/rc.APM ] -then - if sercon - then - echo "[i] USB interface connected" - fi - - echo "[i] Running rc.APM" - # if APM startup is successful then nsh will exit - sh /etc/init.d/rc.APM -fi - if [ $MODE == autostart ] then - echo "[i] AUTOSTART mode" - - # - # Start CDC/ACM serial driver - # - sercon - # Try to get an USB console nshterm /dev/ttyACM0 & diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index bd601aad3..e3bbe3a03 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -62,6 +62,7 @@ MODULES += modules/commander MODULES += modules/navigator MODULES += modules/mavlink MODULES += modules/gpio_led +MODULES += modules/land_detector # # Estimation modules (EKF / other filters) diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig index 5bb7bed15..eae796a9c 100644 --- a/nuttx-configs/px4fmu-v1/nsh/defconfig +++ b/nuttx-configs/px4fmu-v1/nsh/defconfig @@ -113,7 +113,7 @@ CONFIG_ARCH_FPU=y # CONFIG_ARMV7M_TOOLCHAIN_CODEREDL is not set # CONFIG_ARMV7M_TOOLCHAIN_CODESOURCERYL is not set CONFIG_ARMV7M_TOOLCHAIN_GNU_EABIL=y -CONFIG_ARMV7M_STACKCHECK=n +CONFIG_ARMV7M_STACKCHECK=y # CONFIG_ARMV7M_ITMSYSLOG is not set # diff --git a/src/drivers/drv_range_finder.h b/src/drivers/drv_range_finder.h index 0f8362f58..12d51aeaa 100644 --- a/src/drivers/drv_range_finder.h +++ b/src/drivers/drv_range_finder.h @@ -45,6 +45,7 @@ #include "drv_orb_dev.h" #define RANGE_FINDER_DEVICE_PATH "/dev/range_finder" +#define MB12XX_MAX_RANGEFINDERS 12 //Maximum number of RangeFinders that can be connected enum RANGE_FINDER_TYPE { RANGE_FINDER_TYPE_LASER = 0, @@ -67,6 +68,8 @@ struct range_finder_report { float minimum_distance; /**< minimum distance the sensor can measure */ float maximum_distance; /**< maximum distance the sensor can measure */ uint8_t valid; /**< 1 == within sensor range, 0 = outside sensor range */ + float distance_vector[MB12XX_MAX_RANGEFINDERS]; /** in meters */ + uint8_t just_updated; /** number of the most recent measurement sensor */ }; /** diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 95fbed0ba..a06be72d9 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -1351,13 +1351,15 @@ start_bus(struct hmc5883_bus_option &bus, enum Rotation rotation) } int fd = open(bus.devpath, O_RDONLY); - if (fd < 0) + if (fd < 0) { return false; + } if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { close(fd); errx(1,"Failed to setup poll rate"); } + close(fd); return true; } diff --git a/src/drivers/mb12xx/mb12xx.cpp b/src/drivers/mb12xx/mb12xx.cpp index 9cf568658..a4fb7bcc2 100644 --- a/src/drivers/mb12xx/mb12xx.cpp +++ b/src/drivers/mb12xx/mb12xx.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -34,6 +34,7 @@ /** * @file mb12xx.cpp * @author Greg Hulands + * @author Jon Verbeke <jon.verbeke@kuleuven.be> * * Driver for the Maxbotix sonar range finders connected via I2C. */ @@ -54,6 +55,7 @@ #include <stdio.h> #include <math.h> #include <unistd.h> +#include <vector> #include <nuttx/arch.h> #include <nuttx/wqueue.h> @@ -72,7 +74,7 @@ #include <board_config.h> /* Configuration Constants */ -#define MB12XX_BUS PX4_I2C_BUS_EXPANSION +#define MB12XX_BUS PX4_I2C_BUS_EXPANSION #define MB12XX_BASEADDR 0x70 /* 7-bit address. 8-bit address is 0xE0 */ #define MB12XX_DEVICE_PATH "/dev/mb12xx" @@ -83,10 +85,12 @@ #define MB12XX_SET_ADDRESS_2 0xA5 /* Change address 2 Register */ /* Device limits */ -#define MB12XX_MIN_DISTANCE (0.20f) -#define MB12XX_MAX_DISTANCE (7.65f) +#define MB12XX_MIN_DISTANCE (0.20f) +#define MB12XX_MAX_DISTANCE (7.65f) + +#define MB12XX_CONVERSION_INTERVAL 100000 /* 60ms for one sonar */ +#define TICKS_BETWEEN_SUCCESIVE_FIRES 100000 /* 30ms between each sonar measurement (watch out for interference!) */ -#define MB12XX_CONVERSION_INTERVAL 60000 /* 60ms */ /* oddly, ERROR is not defined for c++ */ #ifdef ERROR @@ -133,12 +137,19 @@ private: perf_counter_t _comms_errors; perf_counter_t _buffer_overflows; + uint8_t _cycle_counter; /* counter in cycle to change i2c adresses */ + int _cycling_rate; /* */ + uint8_t _index_counter; /* temporary sonar i2c address */ + std::vector<uint8_t> addr_ind; /* temp sonar i2c address vector */ + std::vector<float> _latest_sonar_measurements; /* vector to store latest sonar measurements in before writing to report */ + + /** * Test whether the device supported by the driver is present at a * specific address. * * @param address The I2C bus address to probe. - * @return True if the device is present. + * @return True if the device is present. */ int probe_address(uint8_t address); @@ -178,7 +189,7 @@ private: * * @param arg Instance pointer for the driver that is polling. */ - static void cycle_trampoline(void *arg); + static void cycle_trampoline(void *arg); }; @@ -200,12 +211,16 @@ MB12XX::MB12XX(int bus, int address) : _range_finder_topic(-1), _sample_perf(perf_alloc(PC_ELAPSED, "mb12xx_read")), _comms_errors(perf_alloc(PC_COUNT, "mb12xx_comms_errors")), - _buffer_overflows(perf_alloc(PC_COUNT, "mb12xx_buffer_overflows")) + _buffer_overflows(perf_alloc(PC_COUNT, "mb12xx_buffer_overflows")), + _cycle_counter(0), /* initialising counter for cycling function to zero */ + _cycling_rate(0), /* initialising cycling rate (which can differ depending on one sonar or multiple) */ + _index_counter(0) /* initialising temp sonar i2c address to zero */ + { - // enable debug() calls + /* enable debug() calls */ _debug_enabled = false; - // work_cancel in the dtor will explode if we don't do this... + /* work_cancel in the dtor will explode if we don't do this... */ memset(&_work, 0, sizeof(_work)); } @@ -223,7 +238,7 @@ MB12XX::~MB12XX() unregister_class_devname(RANGE_FINDER_DEVICE_PATH, _class_instance); } - // free perf counters + /* free perf counters */ perf_free(_sample_perf); perf_free(_comms_errors); perf_free(_buffer_overflows); @@ -242,6 +257,9 @@ MB12XX::init() /* allocate basic report buffers */ _reports = new RingBuffer(2, sizeof(range_finder_report)); + _index_counter = MB12XX_BASEADDR; /* set temp sonar i2c address to base adress */ + set_address(_index_counter); /* set I2c port to temp sonar i2c adress */ + if (_reports == nullptr) { goto out; } @@ -250,16 +268,51 @@ MB12XX::init() if (_class_instance == CLASS_DEVICE_PRIMARY) { /* get a publish handle on the range finder topic */ - struct range_finder_report rf_report; - measure(); - _reports->get(&rf_report); + struct range_finder_report rf_report = {}; + _range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &rf_report); if (_range_finder_topic < 0) { - debug("failed to create sensor_range_finder object. Did you start uOrb?"); + log("failed to create sensor_range_finder object. Did you start uOrb?"); + } + } + + // XXX we should find out why we need to wait 200 ms here + usleep(200000); + + /* check for connected rangefinders on each i2c port: + We start from i2c base address (0x70 = 112) and count downwards + So second iteration it uses i2c address 111, third iteration 110 and so on*/ + for (unsigned counter = 0; counter <= MB12XX_MAX_RANGEFINDERS; counter++) { + _index_counter = MB12XX_BASEADDR - counter; /* set temp sonar i2c address to base adress - counter */ + set_address(_index_counter); /* set I2c port to temp sonar i2c adress */ + int ret2 = measure(); + + if (ret2 == 0) { /* sonar is present -> store address_index in array */ + addr_ind.push_back(_index_counter); + debug("sonar added"); + _latest_sonar_measurements.push_back(200); } } + _index_counter = MB12XX_BASEADDR; + set_address(_index_counter); /* set i2c port back to base adress for rest of driver */ + + /* if only one sonar detected, no special timing is required between firing, so use default */ + if (addr_ind.size() == 1) { + _cycling_rate = MB12XX_CONVERSION_INTERVAL; + + } else { + _cycling_rate = TICKS_BETWEEN_SUCCESIVE_FIRES; + } + + /* show the connected sonars in terminal */ + for (unsigned i = 0; i < addr_ind.size(); i++) { + log("sonar %d with address %d added", (i + 1), addr_ind[i]); + } + + debug("Number of sonars connected: %d", addr_ind.size()); + ret = OK; /* sensor is ok, but we don't really know if it is within range */ _sensor_ok = true; @@ -325,11 +378,12 @@ MB12XX::ioctl(struct file *filp, int cmd, unsigned long arg) bool want_start = (_measure_ticks == 0); /* set interval for next measurement to minimum legal value */ - _measure_ticks = USEC2TICK(MB12XX_CONVERSION_INTERVAL); + _measure_ticks = USEC2TICK(_cycling_rate); /* if we need to start the poll state machine, do it */ if (want_start) { start(); + } return OK; @@ -341,10 +395,10 @@ MB12XX::ioctl(struct file *filp, int cmd, unsigned long arg) bool want_start = (_measure_ticks == 0); /* convert hz to tick interval via microseconds */ - unsigned ticks = USEC2TICK(1000000 / arg); + int ticks = USEC2TICK(1000000 / arg); /* check against maximum rate */ - if (ticks < USEC2TICK(MB12XX_CONVERSION_INTERVAL)) { + if (ticks < USEC2TICK(_cycling_rate)) { return -EINVAL; } @@ -414,6 +468,7 @@ MB12XX::ioctl(struct file *filp, int cmd, unsigned long arg) ssize_t MB12XX::read(struct file *filp, char *buffer, size_t buflen) { + unsigned count = buflen / sizeof(struct range_finder_report); struct range_finder_report *rbuf = reinterpret_cast<struct range_finder_report *>(buffer); int ret = 0; @@ -453,7 +508,7 @@ MB12XX::read(struct file *filp, char *buffer, size_t buflen) } /* wait for it to complete */ - usleep(MB12XX_CONVERSION_INTERVAL); + usleep(_cycling_rate * 2); /* run the collection phase */ if (OK != collect()) { @@ -474,17 +529,19 @@ MB12XX::read(struct file *filp, char *buffer, size_t buflen) int MB12XX::measure() { + int ret; /* * Send the command to begin a measurement. */ + uint8_t cmd = MB12XX_TAKE_RANGE_REG; ret = transfer(&cmd, 1, nullptr, 0); if (OK != ret) { perf_count(_comms_errors); - log("i2c::transfer returned %d", ret); + debug("i2c::transfer returned %d", ret); return ret; } @@ -506,7 +563,7 @@ MB12XX::collect() ret = transfer(nullptr, 0, &val[0], 2); if (ret < 0) { - log("error reading from sensor: %d", ret); + debug("error reading from sensor: %d", ret); perf_count(_comms_errors); perf_end(_sample_perf); return ret; @@ -519,7 +576,39 @@ MB12XX::collect() /* this should be fairly close to the end of the measurement, so the best approximation of the time */ report.timestamp = hrt_absolute_time(); report.error_count = perf_event_count(_comms_errors); - report.distance = si_units; + + /* if only one sonar, write it to the original distance parameter so that it's still used as altitude sonar */ + if (addr_ind.size() == 1) { + report.distance = si_units; + + for (unsigned i = 0; i < (MB12XX_MAX_RANGEFINDERS); i++) { + report.distance_vector[i] = 0; + } + + report.just_updated = 0; + + } else { + /* for multiple sonars connected */ + + /* don't use the orginial single sonar variable */ + report.distance = 0; + + /* intermediate vector _latest_sonar_measurements is used to store the measurements as every cycle the other sonar values of the report are thrown away and/or filled in with garbage. We don't want this. We want the report to give the latest value for each connected sonar */ + _latest_sonar_measurements[_cycle_counter] = si_units; + + for (unsigned i = 0; i < (_latest_sonar_measurements.size()); i++) { + report.distance_vector[i] = _latest_sonar_measurements[i]; + } + + /* a just_updated variable is added to indicate to autopilot (ardupilot or whatever) which sonar has most recently been collected as this could be of use for Kalman filters */ + report.just_updated = _index_counter; + + /* Make sure all elements of the distance vector for which no sonar is connected are zero to prevent strange numbers */ + for (unsigned i = 0; i < (MB12XX_MAX_RANGEFINDERS - addr_ind.size()); i++) { + report.distance_vector[addr_ind.size() + i] = 0; + } + } + report.minimum_distance = get_minimum_distance(); report.maximum_distance = get_maximum_distance(); report.valid = si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0; @@ -545,12 +634,13 @@ MB12XX::collect() void MB12XX::start() { + /* reset the report ring and state machine */ _collect_phase = false; _reports->flush(); /* schedule a cycle to start things */ - work_queue(HPWORK, &_work, (worker_t)&MB12XX::cycle_trampoline, this, 1); + work_queue(HPWORK, &_work, (worker_t)&MB12XX::cycle_trampoline, this, 5); /* notify about state change */ struct subsystem_info_s info = { @@ -564,8 +654,10 @@ MB12XX::start() if (pub > 0) { orb_publish(ORB_ID(subsystem_info), pub, &info); + } else { pub = orb_advertise(ORB_ID(subsystem_info), &info); + } } @@ -578,21 +670,24 @@ MB12XX::stop() void MB12XX::cycle_trampoline(void *arg) { + MB12XX *dev = (MB12XX *)arg; dev->cycle(); + } void MB12XX::cycle() { - /* collection phase? */ if (_collect_phase) { + _index_counter = addr_ind[_cycle_counter]; /*sonar from previous iteration collect is now read out */ + set_address(_index_counter); /* perform collection */ if (OK != collect()) { - log("collection error"); - /* restart the measurement state machine */ + debug("collection error"); + /* if error restart the measurement state machine */ start(); return; } @@ -600,25 +695,37 @@ MB12XX::cycle() /* next phase is measurement */ _collect_phase = false; - /* - * Is there a collect->measure gap? - */ - if (_measure_ticks > USEC2TICK(MB12XX_CONVERSION_INTERVAL)) { + /* change i2c adress to next sonar */ + _cycle_counter = _cycle_counter + 1; + + if (_cycle_counter >= addr_ind.size()) { + _cycle_counter = 0; + } + + /* Is there a collect->measure gap? Yes, and the timing is set equal to the cycling_rate + Otherwise the next sonar would fire without the first one having received its reflected sonar pulse */ + + if (_measure_ticks > USEC2TICK(_cycling_rate)) { /* schedule a fresh cycle call when we are ready to measure again */ work_queue(HPWORK, &_work, (worker_t)&MB12XX::cycle_trampoline, this, - _measure_ticks - USEC2TICK(MB12XX_CONVERSION_INTERVAL)); - + _measure_ticks - USEC2TICK(_cycling_rate)); return; } } - /* measurement phase */ + /* Measurement (firing) phase */ + + /* ensure sonar i2c adress is still correct */ + _index_counter = addr_ind[_cycle_counter]; + set_address(_index_counter); + + /* Perform measurement */ if (OK != measure()) { - log("measure error"); + debug("measure error sonar adress %d", _index_counter); } /* next phase is collection */ @@ -629,7 +736,8 @@ MB12XX::cycle() &_work, (worker_t)&MB12XX::cycle_trampoline, this, - USEC2TICK(MB12XX_CONVERSION_INTERVAL)); + USEC2TICK(_cycling_rate)); + } void @@ -750,7 +858,7 @@ test() } warnx("single read"); - warnx("measurement: %0.2f m", (double)report.distance); + warnx("measurement: %0.2f of sonar %d", (double)report.distance_vector[report.just_updated], report.just_updated); warnx("time: %lld", report.timestamp); /* start the sensor polling at 2Hz */ @@ -779,7 +887,12 @@ test() } warnx("periodic read %u", i); - warnx("measurement: %0.3f", (double)report.distance); + + /* Print the sonar rangefinder report sonar distance vector */ + for (uint8_t count = 0; count < MB12XX_MAX_RANGEFINDERS; count++) { + warnx("measurement: %0.3f of sonar %u", (double)report.distance_vector[count], count + 1); + } + warnx("time: %lld", report.timestamp); } @@ -830,7 +943,7 @@ info() exit(0); } -} // namespace +} /* namespace */ int mb12xx_main(int argc, char *argv[]) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 168b34ea9..2be758244 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -921,26 +921,50 @@ MPU6000::gyro_self_test() if (self_test()) return 1; - /* evaluate gyro offsets, complain if offset -> zero or larger than 6 dps */ - if (fabsf(_gyro_scale.x_offset) > 0.1f || fabsf(_gyro_scale.x_offset) < 0.000001f) + /* + * Maximum deviation of 20 degrees, according to + * http://www.invensense.com/mems/gyro/documents/PS-MPU-6000A-00v3.4.pdf + * Section 6.1, initial ZRO tolerance + */ + const float max_offset = 0.34f; + /* 30% scale error is chosen to catch completely faulty units but + * to let some slight scale error pass. Requires a rate table or correlation + * with mag rotations + data fit to + * calibrate properly and is not done by default. + */ + const float max_scale = 0.3f; + + /* evaluate gyro offsets, complain if offset -> zero or larger than 20 dps. */ + if (fabsf(_gyro_scale.x_offset) > max_offset) return 1; - if (fabsf(_gyro_scale.x_scale - 1.0f) > 0.3f) + + /* evaluate gyro scale, complain if off by more than 30% */ + if (fabsf(_gyro_scale.x_scale - 1.0f) > max_scale) return 1; - if (fabsf(_gyro_scale.y_offset) > 0.1f || fabsf(_gyro_scale.y_offset) < 0.000001f) + if (fabsf(_gyro_scale.y_offset) > max_offset) return 1; - if (fabsf(_gyro_scale.y_scale - 1.0f) > 0.3f) + if (fabsf(_gyro_scale.y_scale - 1.0f) > max_scale) return 1; - if (fabsf(_gyro_scale.z_offset) > 0.1f || fabsf(_gyro_scale.z_offset) < 0.000001f) + if (fabsf(_gyro_scale.z_offset) > max_offset) return 1; - if (fabsf(_gyro_scale.z_scale - 1.0f) > 0.3f) + if (fabsf(_gyro_scale.z_scale - 1.0f) > max_scale) return 1; + /* check if all scales are zero */ + if ((fabsf(_gyro_scale.x_offset) < 0.000001f) && + (fabsf(_gyro_scale.y_offset) < 0.000001f) && + (fabsf(_gyro_scale.z_offset) < 0.000001f)) { + /* if all are zero, this device is not calibrated */ + return 1; + } + return 0; } + /* perform a self-test comparison to factory trim values. This takes about 200ms and will return OK if the current values are within 14% diff --git a/src/drivers/px4flow/i2c_frame.h b/src/drivers/px4flow/i2c_frame.h new file mode 100644 index 000000000..b391b1f6a --- /dev/null +++ b/src/drivers/px4flow/i2c_frame.h @@ -0,0 +1,82 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file i2c_frame.h + * Definition of i2c frames. + * @author Thomas Boehm <thomas.boehm@fortiss.org> + * @author James Goppert <james.goppert@gmail.com> + */ + +#ifndef I2C_FRAME_H_ +#define I2C_FRAME_H_ +#include <inttypes.h> + + +typedef struct i2c_frame +{ + uint16_t frame_count; + int16_t pixel_flow_x_sum; + int16_t pixel_flow_y_sum; + int16_t flow_comp_m_x; + int16_t flow_comp_m_y; + int16_t qual; + int16_t gyro_x_rate; + int16_t gyro_y_rate; + int16_t gyro_z_rate; + uint8_t gyro_range; + uint8_t sonar_timestamp; + int16_t ground_distance; +} i2c_frame; + +#define I2C_FRAME_SIZE (sizeof(i2c_frame)) + + +typedef struct i2c_integral_frame +{ + uint16_t frame_count_since_last_readout; + int16_t pixel_flow_x_integral; + int16_t pixel_flow_y_integral; + int16_t gyro_x_rate_integral; + int16_t gyro_y_rate_integral; + int16_t gyro_z_rate_integral; + uint32_t integration_timespan; + uint32_t sonar_timestamp; + uint16_t ground_distance; + int16_t gyro_temperature; + uint8_t qual; +} i2c_integral_frame; + +#define I2C_INTEGRAL_FRAME_SIZE (sizeof(i2c_integral_frame)) + +#endif /* I2C_FRAME_H_ */ diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp index 9c9c1b0f8..bb0cdbbb6 100644 --- a/src/drivers/px4flow/px4flow.cpp +++ b/src/drivers/px4flow/px4flow.cpp @@ -93,38 +93,11 @@ static const int ERROR = -1; # error This requires CONFIG_SCHED_WORKQUEUE. #endif -struct i2c_frame { - uint16_t frame_count; - int16_t pixel_flow_x_sum; - int16_t pixel_flow_y_sum; - int16_t flow_comp_m_x; - int16_t flow_comp_m_y; - int16_t qual; - int16_t gyro_x_rate; - int16_t gyro_y_rate; - int16_t gyro_z_rate; - uint8_t gyro_range; - uint8_t sonar_timestamp; - int16_t ground_distance; -}; -struct i2c_frame f; +#include "i2c_frame.h" -struct i2c_integral_frame { - uint16_t frame_count_since_last_readout; - int16_t pixel_flow_x_integral; - int16_t pixel_flow_y_integral; - int16_t gyro_x_rate_integral; - int16_t gyro_y_rate_integral; - int16_t gyro_z_rate_integral; - uint32_t integration_timespan; - uint32_t time_since_last_sonar_update; - uint16_t ground_distance; - int16_t gyro_temperature; - uint8_t qual; -} __attribute__((packed)); +struct i2c_frame f; struct i2c_integral_frame f_integral; - class PX4FLOW: public device::I2C { public: @@ -150,8 +123,7 @@ private: RingBuffer *_reports; bool _sensor_ok; int _measure_ticks; - bool _collect_phase; - + bool _collect_phase; orb_advert_t _px4flow_topic; perf_counter_t _sample_perf; @@ -261,10 +233,10 @@ out: int PX4FLOW::probe() { - uint8_t val[22]; + uint8_t val[I2C_FRAME_SIZE]; // to be sure this is not a ll40ls Lidar (which can also be on - // 0x42) we check if a 22 byte transfer works from address + // 0x42) we check if a I2C_FRAME_SIZE byte transfer works from address // 0. The ll40ls gives an error for that, whereas the flow // happily returns some data if (transfer(nullptr, 0, &val[0], 22) != OK) { @@ -469,16 +441,16 @@ PX4FLOW::collect() int ret = -EIO; /* read from the sensor */ - uint8_t val[47] = { 0 }; + uint8_t val[I2C_FRAME_SIZE + I2C_INTEGRAL_FRAME_SIZE] = { 0 }; perf_begin(_sample_perf); if (PX4FLOW_REG == 0x00) { - ret = transfer(nullptr, 0, &val[0], 47); // read 47 bytes (22+25 : frame1 + frame2) + ret = transfer(nullptr, 0, &val[0], I2C_FRAME_SIZE + I2C_INTEGRAL_FRAME_SIZE); } if (PX4FLOW_REG == 0x16) { - ret = transfer(nullptr, 0, &val[0], 25); // read 25 bytes (only frame2) + ret = transfer(nullptr, 0, &val[0], I2C_INTEGRAL_FRAME_SIZE); } if (ret < 0) { @@ -489,46 +461,12 @@ PX4FLOW::collect() } if (PX4FLOW_REG == 0) { - f.frame_count = val[1] << 8 | val[0]; - f.pixel_flow_x_sum = val[3] << 8 | val[2]; - f.pixel_flow_y_sum = val[5] << 8 | val[4]; - f.flow_comp_m_x = val[7] << 8 | val[6]; - f.flow_comp_m_y = val[9] << 8 | val[8]; - f.qual = val[11] << 8 | val[10]; - f.gyro_x_rate = val[13] << 8 | val[12]; - f.gyro_y_rate = val[15] << 8 | val[14]; - f.gyro_z_rate = val[17] << 8 | val[16]; - f.gyro_range = val[18]; - f.sonar_timestamp = val[19]; - f.ground_distance = val[21] << 8 | val[20]; - - f_integral.frame_count_since_last_readout = val[23] << 8 | val[22]; - f_integral.pixel_flow_x_integral = val[25] << 8 | val[24]; - f_integral.pixel_flow_y_integral = val[27] << 8 | val[26]; - f_integral.gyro_x_rate_integral = val[29] << 8 | val[28]; - f_integral.gyro_y_rate_integral = val[31] << 8 | val[30]; - f_integral.gyro_z_rate_integral = val[33] << 8 | val[32]; - f_integral.integration_timespan = val[37] << 24 | val[36] << 16 - | val[35] << 8 | val[34]; - f_integral.time_since_last_sonar_update = val[41] << 24 | val[40] << 16 - | val[39] << 8 | val[38]; - f_integral.ground_distance = val[43] << 8 | val[42]; - f_integral.gyro_temperature = val[45] << 8 | val[44]; - f_integral.qual = val[46]; + memcpy(&f, val, I2C_FRAME_SIZE); + memcpy(&f_integral, &(val[I2C_FRAME_SIZE]), I2C_INTEGRAL_FRAME_SIZE); } if (PX4FLOW_REG == 0x16) { - f_integral.frame_count_since_last_readout = val[1] << 8 | val[0]; - f_integral.pixel_flow_x_integral = val[3] << 8 | val[2]; - f_integral.pixel_flow_y_integral = val[5] << 8 | val[4]; - f_integral.gyro_x_rate_integral = val[7] << 8 | val[6]; - f_integral.gyro_y_rate_integral = val[9] << 8 | val[8]; - f_integral.gyro_z_rate_integral = val[11] << 8 | val[10]; - f_integral.integration_timespan = val[15] << 24 | val[14] << 16 | val[13] << 8 | val[12]; - f_integral.time_since_last_sonar_update = val[19] << 24 | val[18] << 16 | val[17] << 8 | val[16]; - f_integral.ground_distance = val[21] << 8 | val[20]; - f_integral.gyro_temperature = val[23] << 8 | val[22]; - f_integral.qual = val[24]; + memcpy(&f_integral, val, I2C_INTEGRAL_FRAME_SIZE); } @@ -544,7 +482,7 @@ PX4FLOW::collect() report.gyro_y_rate_integral = static_cast<float>(f_integral.gyro_y_rate_integral) / 10000.0f; //convert to radians report.gyro_z_rate_integral = static_cast<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.time_since_last_sonar_update = f_integral.sonar_timestamp;//microseconds report.gyro_temperature = f_integral.gyro_temperature;//Temperature * 100 in centi-degrees Celsius report.sensor_id = 0; @@ -828,7 +766,7 @@ test() warnx("ground_distance: %0.2f m", (double) f_integral.ground_distance / 1000); warnx("time since last sonar update [us]: %i", - f_integral.time_since_last_sonar_update); + f_integral.sonar_timestamp); warnx("quality integration average : %i", f_integral.qual); warnx("quality : %i", f.qual); diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index f4e3dc441..2e28a6d2c 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -435,7 +435,7 @@ MavlinkReceiver::handle_message_hil_optical_flow(mavlink_message_t *msg) /* Use distance value for range finder report */ struct range_finder_report r; - memset(&r, 0, sizeof(f)); + memset(&r, 0, sizeof(r)); r.timestamp = hrt_absolute_time(); r.error_count = 0; 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 bf65d2805..a00f29bbc 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -727,11 +727,18 @@ MulticopterPositionControl::control_auto(float dt) reset_alt_sp(); } + //Poll position setpoint bool updated; orb_check(_pos_sp_triplet_sub, &updated); - if (updated) { orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet); + + //Make sure that the position setpoint is valid + if (!isfinite(_pos_sp_triplet.current.lat) || + !isfinite(_pos_sp_triplet.current.lon) || + !isfinite(_pos_sp_triplet.current.alt)) { + _pos_sp_triplet.current.valid = false; + } } if (_pos_sp_triplet.current.valid) { diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index 4482fb36b..9bc9be245 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -58,17 +58,17 @@ static const int ERROR = -1; Geofence::Geofence() : - SuperBlock(NULL, "GF"), - _fence_pub(-1), - _altitude_min(0), - _altitude_max(0), - _verticesCount(0), - _param_geofence_on(this, "ON"), - _param_altitude_mode(this, "ALTMODE"), - _param_source(this, "SOURCE"), - _param_counter_threshold(this, "COUNT"), - _outside_counter(0), - _mavlinkFd(-1) + SuperBlock(NULL, "GF"), + _fence_pub(-1), + _altitude_min(0), + _altitude_max(0), + _verticesCount(0), + _param_geofence_on(this, "ON"), + _param_altitude_mode(this, "ALTMODE"), + _param_source(this, "SOURCE"), + _param_counter_threshold(this, "COUNT"), + _outside_counter(0), + _mavlinkFd(-1) { /* Load initial params */ updateParams(); @@ -92,22 +92,26 @@ bool Geofence::inside(const struct vehicle_global_position_s &global_position, f bool Geofence::inside(const struct vehicle_global_position_s &global_position, - const struct vehicle_gps_position_s &gps_position,float baro_altitude_amsl) { + const struct vehicle_gps_position_s &gps_position, float baro_altitude_amsl) +{ updateParams(); if (getAltitudeMode() == Geofence::GF_ALT_MODE_WGS84) { if (getSource() == Geofence::GF_SOURCE_GLOBALPOS) { return inside(global_position); + } else { return inside((double)gps_position.lat * 1.0e-7, (double)gps_position.lon * 1.0e-7, - (double)gps_position.alt * 1.0e-3); + (double)gps_position.alt * 1.0e-3); } + } else { if (getSource() == Geofence::GF_SOURCE_GLOBALPOS) { return inside(global_position, baro_altitude_amsl); + } else { return inside((double)gps_position.lat * 1.0e-7, (double)gps_position.lon * 1.0e-7, - baro_altitude_amsl); + baro_altitude_amsl); } } } @@ -120,9 +124,12 @@ bool Geofence::inside(double lat, double lon, float altitude) _outside_counter = 0; return inside_fence; } { + _outside_counter++; - if(_outside_counter > _param_counter_threshold.get()) { + + if (_outside_counter > _param_counter_threshold.get()) { return inside_fence; + } else { return true; } @@ -133,8 +140,9 @@ bool Geofence::inside(double lat, double lon, float altitude) bool Geofence::inside_polygon(double lat, double lon, float altitude) { /* Return true if geofence is disabled */ - if (_param_geofence_on.get() != 1) + if (_param_geofence_on.get() != 1) { return true; + } if (valid()) { @@ -159,20 +167,22 @@ bool Geofence::inside_polygon(double lat, double lon, float altitude) if (dm_read(DM_KEY_FENCE_POINTS, i, &temp_vertex_i, sizeof(struct fence_vertex_s)) != sizeof(struct fence_vertex_s)) { break; } + if (dm_read(DM_KEY_FENCE_POINTS, j, &temp_vertex_j, sizeof(struct fence_vertex_s)) != sizeof(struct fence_vertex_s)) { break; } // skip vertex 0 (return point) if (((double)temp_vertex_i.lon >= lon) != ((double)temp_vertex_j.lon >= lon) && - (lat <= (double)(temp_vertex_j.lat - temp_vertex_i.lat) * (lon - (double)temp_vertex_i.lon) / - (double)(temp_vertex_j.lon - temp_vertex_i.lon) + (double)temp_vertex_i.lat)) { - c = !c; + (lat <= (double)(temp_vertex_j.lat - temp_vertex_i.lat) * (lon - (double)temp_vertex_i.lon) / + (double)(temp_vertex_j.lon - temp_vertex_i.lon) + (double)temp_vertex_i.lat)) { + c = !c; } } return c; + } else { /* Empty fence --> accept all points */ return true; @@ -188,8 +198,9 @@ bool Geofence::valid() { // NULL fence is valid - if (isEmpty()) + if (isEmpty()) { return true; + } // Otherwise if ((_verticesCount < 4) || (_verticesCount > GEOFENCE_MAX_VERTICES)) { @@ -214,26 +225,33 @@ Geofence::addPoint(int argc, char *argv[]) return; } - if (argc < 3) + if (argc < 3) { errx(1, "Specify: -clear | sequence latitude longitude [-publish]"); + } ix = atoi(argv[0]); - if (ix >= DM_KEY_FENCE_POINTS_MAX) + + if (ix >= DM_KEY_FENCE_POINTS_MAX) { errx(1, "Sequence must be less than %d", DM_KEY_FENCE_POINTS_MAX); + } lat = strtod(argv[1], &end); lon = strtod(argv[2], &end); last = 0; - if ((argc > 3) && (strcmp(argv[3], "-publish") == 0)) + + if ((argc > 3) && (strcmp(argv[3], "-publish") == 0)) { last = 1; + } vertex.lat = (float)lat; vertex.lon = (float)lon; if (dm_write(DM_KEY_FENCE_POINTS, ix, DM_PERSIST_POWER_ON_RESET, &vertex, sizeof(vertex)) == sizeof(vertex)) { - if (last) + if (last) { publishFence((unsigned)ix + 1); + } + return; } @@ -243,10 +261,12 @@ Geofence::addPoint(int argc, char *argv[]) void Geofence::publishFence(unsigned vertices) { - if (_fence_pub == -1) + if (_fence_pub == -1) { _fence_pub = orb_advertise(ORB_ID(fence), &vertices); - else + + } else { orb_publish(ORB_ID(fence), _fence_pub, &vertices); + } } int @@ -257,26 +277,29 @@ Geofence::loadFromFile(const char *filename) int pointCounter = 0; bool gotVertical = false; const char commentChar = '#'; + int rc = ERROR; /* Make sure no data is left in the datamanager */ clearDm(); /* open the mixer definition file */ fp = fopen(GEOFENCE_FILENAME, "r"); + if (fp == NULL) { return ERROR; } /* create geofence points from valid lines and store in DM */ for (;;) { - /* get a line, bail on error/EOF */ - if (fgets(line, sizeof(line), fp) == NULL) + if (fgets(line, sizeof(line), fp) == NULL) { break; + } /* Trim leading whitespace */ size_t textStart = 0; - while((textStart < sizeof(line)/sizeof(char)) && isspace(line[textStart])) textStart++; + + while ((textStart < sizeof(line) / sizeof(char)) && isspace(line[textStart])) { textStart++; } /* if the line starts with #, skip */ if (line[textStart] == commentChar) { @@ -299,55 +322,56 @@ Geofence::loadFromFile(const char *filename) if (sscanf(line, "DMS %f %f %f %f %f %f", &lat_d, &lat_m, &lat_s, &lon_d, &lon_m, &lon_s) != 6) { warnx("Scanf to parse DMS geofence vertex failed."); - return ERROR; + goto error; } // warnx("Geofence DMS: %.5f %.5f %.5f ; %.5f %.5f %.5f", (double)lat_d, (double)lat_m, (double)lat_s, (double)lon_d, (double)lon_m, (double)lon_s); - vertex.lat = lat_d + lat_m/60.0f + lat_s/3600.0f; - vertex.lon = lon_d + lon_m/60.0f + lon_s/3600.0f; + vertex.lat = lat_d + lat_m / 60.0f + lat_s / 3600.0f; + vertex.lon = lon_d + lon_m / 60.0f + lon_s / 3600.0f; } else { /* Handle decimal degree format */ if (sscanf(line, "%f %f", &(vertex.lat), &(vertex.lon)) != 2) { warnx("Scanf to parse geofence vertex failed."); - return ERROR; + goto error; } } - if (dm_write(DM_KEY_FENCE_POINTS, pointCounter, DM_PERSIST_POWER_ON_RESET, &vertex, sizeof(vertex)) != sizeof(vertex)) - return ERROR; + if (dm_write(DM_KEY_FENCE_POINTS, pointCounter, DM_PERSIST_POWER_ON_RESET, &vertex, sizeof(vertex)) != sizeof(vertex)) { + goto error; + } - warnx("Geofence: point: %d, lat %.5f: lon: %.5f", pointCounter, (double)vertex.lat, (double)vertex.lon); + warnx("Geofence: point: %d, lat %.5f: lon: %.5f", pointCounter, (double)vertex.lat, (double)vertex.lon); pointCounter++; + } else { /* Parse the line as the vertical limits */ - if (sscanf(line, "%f %f", &_altitude_min, &_altitude_max) != 2) - return ERROR; - + if (sscanf(line, "%f %f", &_altitude_min, &_altitude_max) != 2) { + goto error; + } warnx("Geofence: alt min: %.4f, alt_max: %.4f", (double)_altitude_min, (double)_altitude_max); gotVertical = true; } - - } - fclose(fp); - /* Check if import was successful */ - if(gotVertical && pointCounter > 0) - { + if (gotVertical && pointCounter > 0) { _verticesCount = pointCounter; warnx("Geofence: imported successfully"); mavlink_log_info(_mavlinkFd, "Geofence imported"); + rc = OK; + } else { warnx("Geofence: import error"); mavlink_log_critical(_mavlinkFd, "#audio: Geofence import error"); } - return ERROR; +error: + fclose(fp); + return rc; } int Geofence::clearDm() 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 6de283396..ec297e7f0 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -877,6 +877,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float w_xy_gps_p = params.w_xy_gps_p * w_gps_xy; float w_xy_gps_v = params.w_xy_gps_v * w_gps_xy; float w_z_gps_p = params.w_z_gps_p * w_gps_z; + float w_z_gps_v = params.w_z_gps_v * w_gps_z; float w_xy_vision_p = params.w_xy_vision_p; float w_xy_vision_v = params.w_xy_vision_v; @@ -907,6 +908,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (use_gps_z) { accel_bias_corr[2] -= corr_gps[2][0] * w_z_gps_p * w_z_gps_p; + accel_bias_corr[2] -= corr_gps[2][1] * w_z_gps_v; } /* transform error vector from NED frame to body frame */ @@ -991,6 +993,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) epv = fminf(epv, gps.epv); inertial_filter_correct(corr_gps[2][0], dt, z_est, 0, w_z_gps_p); + inertial_filter_correct(corr_gps[2][1], dt, z_est, 1, w_z_gps_v); } if (use_vision_z) { diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.c b/src/modules/position_estimator_inav/position_estimator_inav_params.c index b7f6509d7..5387b7e87 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -64,6 +64,17 @@ PARAM_DEFINE_FLOAT(INAV_W_Z_BARO, 0.5f); PARAM_DEFINE_FLOAT(INAV_W_Z_GPS_P, 0.005f); /** + * Z velocity weight for GPS + * + * Weight (cutoff frequency) for GPS altitude velocity measurements. + * + * @min 0.0 + * @max 10.0 + * @group Position Estimator INAV + */ +PARAM_DEFINE_FLOAT(INAV_W_Z_GPS_V, 0.0f); + +/** * Z axis weight for vision * * Weight (cutoff frequency) for vision altitude measurements. vision altitude data is very noisy and should be used only as slow correction for baro offset. @@ -281,6 +292,7 @@ int parameters_init(struct position_estimator_inav_param_handles *h) { h->w_z_baro = param_find("INAV_W_Z_BARO"); h->w_z_gps_p = param_find("INAV_W_Z_GPS_P"); + h->w_z_gps_v = param_find("INAV_W_Z_GPS_V"); h->w_z_vision_p = param_find("INAV_W_Z_VIS_P"); h->w_z_sonar = param_find("INAV_W_Z_SONAR"); h->w_xy_gps_p = param_find("INAV_W_XY_GPS_P"); diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.h b/src/modules/position_estimator_inav/position_estimator_inav_params.h index d0a65e42e..51bbda412 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.h +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.h @@ -44,6 +44,7 @@ struct position_estimator_inav_params { float w_z_baro; float w_z_gps_p; + float w_z_gps_v; float w_z_vision_p; float w_z_sonar; float w_xy_gps_p; @@ -68,6 +69,7 @@ struct position_estimator_inav_params { struct position_estimator_inav_param_handles { param_t w_z_baro; param_t w_z_gps_p; + param_t w_z_gps_v; param_t w_z_vision_p; param_t w_z_sonar; param_t w_xy_gps_p; diff --git a/src/systemcmds/nshterm/module.mk b/src/systemcmds/nshterm/module.mk index a12bc369e..4e2710572 100644 --- a/src/systemcmds/nshterm/module.mk +++ b/src/systemcmds/nshterm/module.mk @@ -38,7 +38,7 @@ MODULE_COMMAND = nshterm SRCS = nshterm.c -MODULE_STACKSIZE = 1600 +MODULE_STACKSIZE = 1500 MAXOPTIMIZATION = -Os diff --git a/src/systemcmds/nshterm/nshterm.c b/src/systemcmds/nshterm/nshterm.c index ceaea35b6..50547a562 100644 --- a/src/systemcmds/nshterm/nshterm.c +++ b/src/systemcmds/nshterm/nshterm.c @@ -50,6 +50,7 @@ #include <apps/nsh.h> #include <fcntl.h> #include <systemlib/err.h> +#include <drivers/drv_hrt.h> #include <uORB/topics/actuator_armed.h> @@ -67,6 +68,11 @@ nshterm_main(int argc, char *argv[]) int armed_fd = orb_subscribe(ORB_ID(actuator_armed)); struct actuator_armed_s armed; + /* back off 800 ms to avoid running into the USB setup timing */ + while (hrt_absolute_time() < 800U * 1000U) { + usleep(50000); + } + /* try to bring up the console - stop doing so if the system gets armed */ while (true) { |