diff options
-rw-r--r-- | ROMFS/px4fmu_common/init.d/rc.mc_defaults | 3 | ||||
-rw-r--r-- | ROMFS/px4fmu_common/init.d/rcS | 90 | ||||
-rw-r--r-- | makefiles/config_px4fmu-v2_default.mk | 1 | ||||
-rw-r--r-- | makefiles/config_px4fmu-v2_test.mk | 1 | ||||
-rw-r--r-- | src/drivers/drv_io_expander.h | 71 | ||||
-rw-r--r-- | src/drivers/gps/ubx.cpp | 4 | ||||
-rw-r--r-- | src/drivers/pca8574/module.mk | 6 | ||||
-rw-r--r-- | src/drivers/pca8574/pca8574.cpp | 554 | ||||
-rw-r--r-- | src/modules/commander/commander.cpp | 2 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_main.cpp | 45 | ||||
-rw-r--r-- | src/modules/navigator/navigator_main.cpp | 42 | ||||
-rw-r--r-- | src/modules/uORB/topics/actuator_armed.h | 20 |
12 files changed, 749 insertions, 90 deletions
diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_defaults b/ROMFS/px4fmu_common/init.d/rc.mc_defaults index b6b0a5b4e..127e42164 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.mc_defaults @@ -37,10 +37,11 @@ then param set MPC_LAND_SPEED 1.0 param set PE_VELNE_NOISE 0.5 - param set PE_VELNE_NOISE 0.7 + param set PE_VELD_NOISE 0.7 param set PE_POSNE_NOISE 0.5 param set PE_POSD_NOISE 1.0 + param set NAV_ACCEPT_RAD 2.0 fi set PWM_RATE 400 diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 8c413e087..6d06f897a 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -65,12 +65,12 @@ then # Start CDC/ACM serial driver # sercon - + # # Start the ORB (first app to start) # uorb start - + # # Load parameters # @@ -79,7 +79,7 @@ then then set PARAM_FILE /fs/mtd_params fi - + param select $PARAM_FILE if param load then @@ -87,21 +87,25 @@ then else echo "[init] ERROR: Params loading failed: $PARAM_FILE" fi - + # # Start system state indicator # if rgbled start then - echo "[init] Using external RGB Led" + echo "[init] RGB Led" else if blinkm start then - echo "[init] Using blinkm" + echo "[init] BlinkM" blinkm systemstate fi fi - + + if pca8574 start + then + fi + # # Set default values # @@ -122,7 +126,7 @@ then set LOAD_DEFAULT_APPS yes set GPS yes set GPS_FAKE no - + # # Set DO_AUTOCONFIG flag to use it in AUTOSTART scripts # @@ -132,7 +136,7 @@ then else set DO_AUTOCONFIG no fi - + # # Set USE_IO flag # @@ -142,7 +146,7 @@ then else set USE_IO no fi - + # # Set parameters and env variables for selected AUTOSTART # @@ -172,9 +176,9 @@ then param set SYS_AUTOCONFIG 0 param save fi - + set IO_PRESENT no - + if [ $USE_IO == yes ] then # @@ -186,19 +190,19 @@ then else set IO_FILE /etc/extras/px4io-v1_default.bin fi - + 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 - + if px4io forceupdate 14662 $IO_FILE then usleep 500000 @@ -207,7 +211,7 @@ 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" @@ -220,14 +224,14 @@ then tone_alarm $TUNE_OUT_ERROR fi fi - + if [ $IO_PRESENT == no ] then echo "[init] ERROR: PX4IO not found" tone_alarm $TUNE_OUT_ERROR fi fi - + # # Set default output if not set # @@ -246,7 +250,7 @@ then # Need IO for output but it not present, disable output set OUTPUT_MODE none echo "[init] ERROR: PX4IO not found, disabling output" - + # Avoid using ttyS0 for MAVLink on FMUv1 if ver hwcmp PX4FMU_V1 then @@ -270,17 +274,17 @@ then # Try to get an USB console nshterm /dev/ttyACM0 & - + # # Start the Commander (needs to be this early for in-air-restarts) # commander start - + # # Start primary output # set TTYS1_BUSY no - + # If OUTPUT_MODE == none then something is wrong with setup and we shouldn't try to enable output if [ $OUTPUT_MODE != none ] then @@ -296,7 +300,7 @@ then tone_alarm $TUNE_OUT_ERROR fi fi - + if [ $OUTPUT_MODE == fmu -o $OUTPUT_MODE == ardrone ] then echo "[init] Use FMU as primary output" @@ -307,7 +311,7 @@ then echo "[init] ERROR: FMU mode_$FMU_MODE start failed" tone_alarm $TUNE_OUT_ERROR fi - + if ver hwcmp PX4FMU_V1 then if [ $FMU_MODE == pwm -o $FMU_MODE == gpio ] @@ -320,7 +324,7 @@ then fi fi fi - + if [ $OUTPUT_MODE == mkblctrl ] then echo "[init] Use MKBLCTRL as primary output" @@ -333,7 +337,7 @@ then then set MKBLCTRL_ARG "-mkmode +" fi - + if mkblctrl $MKBLCTRL_ARG then echo "[init] MKBLCTRL started" @@ -341,9 +345,9 @@ then echo "[init] ERROR: MKBLCTRL start failed" tone_alarm $TUNE_OUT_ERROR fi - + fi - + if [ $OUTPUT_MODE == hil ] then echo "[init] Use HIL as primary output" @@ -355,7 +359,7 @@ then tone_alarm $TUNE_OUT_ERROR fi fi - + # # Start IO or FMU for RC PPM input if needed # @@ -382,7 +386,7 @@ then echo "[init] ERROR: FMU mode_$FMU_MODE start failed" tone_alarm $TUNE_OUT_ERROR fi - + if ver hwcmp PX4FMU_V1 then if [ $FMU_MODE == pwm -o $FMU_MODE == gpio ] @@ -397,7 +401,7 @@ then fi fi fi - + # # MAVLink # @@ -418,7 +422,7 @@ then fi mavlink start $MAVLINK_FLAGS - + # # Sensors, Logging, GPS # @@ -429,7 +433,7 @@ then echo "[init] Start logging" sh /etc/init.d/rc.logging fi - + if [ $GPS == yes ] then echo "[init] Start GPS" @@ -439,7 +443,7 @@ then gps start -f else gps start - fi + fi fi # @@ -456,24 +460,24 @@ then if [ $VEHICLE_TYPE == fw ] then echo "[init] Vehicle type: FIXED WING" - + if [ $MIXER == none ] then # Set default mixer for fixed wing if not defined set MIXER FMU_AERT fi - + if [ $MAV_TYPE == none ] then # Use MAV_TYPE = 1 (fixed wing) if not defined set MAV_TYPE 1 fi - + param set MAV_TYPE $MAV_TYPE - + # Load mixer and configure outputs sh /etc/init.d/rc.interface - + # Start standard fixedwing apps if [ $LOAD_DEFAULT_APPS == yes ] then @@ -521,7 +525,7 @@ then set MAV_TYPE 14 fi fi - + # Still no MAV_TYPE found if [ $MAV_TYPE == none ] then @@ -529,10 +533,10 @@ then else param set MAV_TYPE $MAV_TYPE fi - + # Load mixer and configure outputs sh /etc/init.d/rc.interface - + # Start standard multicopter apps if [ $LOAD_DEFAULT_APPS == yes ] then diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index d6e88859c..a68cc32f4 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -40,6 +40,7 @@ MODULES += drivers/meas_airspeed MODULES += drivers/frsky_telemetry MODULES += modules/sensors MODULES += drivers/mkblctrl +MODULES += drivers/pca8574 # Needs to be burned to the ground and re-written; for now, diff --git a/makefiles/config_px4fmu-v2_test.mk b/makefiles/config_px4fmu-v2_test.mk index 84274bf75..66b2157ed 100644 --- a/makefiles/config_px4fmu-v2_test.mk +++ b/makefiles/config_px4fmu-v2_test.mk @@ -24,6 +24,7 @@ MODULES += drivers/lsm303d MODULES += drivers/l3gd20 MODULES += drivers/hmc5883 MODULES += drivers/ms5611 +MODULES += drivers/pca8574 MODULES += systemcmds/perf MODULES += systemcmds/reboot MODULES += systemcmds/tests diff --git a/src/drivers/drv_io_expander.h b/src/drivers/drv_io_expander.h new file mode 100644 index 000000000..106354377 --- /dev/null +++ b/src/drivers/drv_io_expander.h @@ -0,0 +1,71 @@ +/**************************************************************************** + * + * 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 drv_io_expander.h + * + * IO expander device API + */ + +#pragma once + +#include <stdint.h> +#include <sys/ioctl.h> + +/* + * ioctl() definitions + */ + +#define _IOXIOCBASE (0x2800) +#define _IOXIOC(_n) (_IOC(_IOXIOCBASE, _n)) + +/** set a bitmask (non-blocking) */ +#define IOX_SET_MASK _IOXIOC(1) + +/** get a bitmask (blocking) */ +#define IOX_GET_MASK _IOXIOC(2) + +/** set device mode (non-blocking) */ +#define IOX_SET_MODE _IOXIOC(3) + +/** set constant values (non-blocking) */ +#define IOX_SET_VALUE _IOXIOC(4) + +/* ... to IOX_SET_VALUE + 8 */ + +/* enum passed to RGBLED_SET_MODE ioctl()*/ +enum IOX_MODE { + IOX_MODE_OFF, + IOX_MODE_ON, + IOX_MODE_TEST_OUT +}; diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index 19cf5beec..7502809bd 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -447,8 +447,8 @@ UBX::handle_message() gps_bin_nav_sol_packet_t *packet = (gps_bin_nav_sol_packet_t *) _rx_buffer; _gps_position->fix_type = packet->gpsFix; - _gps_position->s_variance_m_s = packet->sAcc; - _gps_position->p_variance_m = packet->pAcc; + _gps_position->s_variance_m_s = (float)packet->sAcc * 1e-2f; // from cm/s to m/s + _gps_position->p_variance_m = (float)packet->pAcc * 1e-2f; // from cm to m _gps_position->timestamp_variance = hrt_absolute_time(); ret = 1; diff --git a/src/drivers/pca8574/module.mk b/src/drivers/pca8574/module.mk new file mode 100644 index 000000000..825ee9bb7 --- /dev/null +++ b/src/drivers/pca8574/module.mk @@ -0,0 +1,6 @@ +# +# PCA8574 driver for RGB LED +# + +MODULE_COMMAND = pca8574 +SRCS = pca8574.cpp diff --git a/src/drivers/pca8574/pca8574.cpp b/src/drivers/pca8574/pca8574.cpp new file mode 100644 index 000000000..904ce18e8 --- /dev/null +++ b/src/drivers/pca8574/pca8574.cpp @@ -0,0 +1,554 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * 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 pca8574.cpp + * + * Driver for an 8 I/O controller (PC8574) connected via I2C. + * + * @author Lorenz Meier <lm@inf.ethz.ch> + * @author Julian Oes <joes@student.ethz.ch> + * @author Anton Babushkin <anton.babushkin@me.com> + */ + +#include <nuttx/config.h> + +#include <drivers/device/i2c.h> + +#include <sys/types.h> +#include <stdint.h> +#include <string.h> +#include <stdlib.h> +#include <stdbool.h> +#include <fcntl.h> +#include <unistd.h> +#include <stdio.h> +#include <ctype.h> + +#include <nuttx/wqueue.h> + +#include <systemlib/perf_counter.h> +#include <systemlib/err.h> +#include <systemlib/systemlib.h> + +#include <board_config.h> + +#include <drivers/drv_io_expander.h> + +#define PCA8574_ONTIME 120 +#define PCA8574_OFFTIME 120 +#define PCA8574_DEVICE_PATH "/dev/pca8574" + +#define ADDR 0x20 ///< I2C adress of PCA8574 (default, A0-A2 pulled to GND) + +class PCA8574 : public device::I2C +{ +public: + PCA8574(int bus, int pca8574); + virtual ~PCA8574(); + + + virtual int init(); + virtual int probe(); + virtual int info(); + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + bool is_running() { return _running; } + +private: + work_s _work; + + uint8_t _values_out; + uint8_t _values_in; + uint8_t _blinking; + uint8_t _blink_phase; + + enum IOX_MODE _mode; + bool _running; + int _led_interval; + bool _should_run; + bool _update_out; + int _counter; + + static void led_trampoline(void *arg); + void led(); + + int send_led_enable(uint8_t arg); + int send_led_values(); + + int get(uint8_t &vals); +}; + +/* for now, we only support one PCA8574 */ +namespace +{ +PCA8574 *g_pca8574; +} + +void pca8574_usage(); + +extern "C" __EXPORT int pca8574_main(int argc, char *argv[]); + +PCA8574::PCA8574(int bus, int pca8574) : + I2C("pca8574", PCA8574_DEVICE_PATH, bus, pca8574, 100000), + _values_out(0), + _values_in(0), + _blinking(0), + _blink_phase(0), + _mode(IOX_MODE_OFF), + _running(false), + _led_interval(80), + _should_run(false), + _update_out(false), + _counter(0) +{ + memset(&_work, 0, sizeof(_work)); +} + +PCA8574::~PCA8574() +{ +} + +int +PCA8574::init() +{ + int ret; + ret = I2C::init(); + + if (ret != OK) { + return ret; + } + + return OK; +} + +int +PCA8574::probe() +{ + uint8_t val; + return get(val); +} + +int +PCA8574::info() +{ + int ret = OK; + + return ret; +} + +int +PCA8574::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + int ret = ENOTTY; + + switch (cmd) { + case IOX_SET_VALUE ...(IOX_SET_VALUE + 8): { + // set the specified on / off state + uint8_t position = (1 << (cmd - IOX_SET_VALUE)); + uint8_t prev = _values_out; + + if (arg) { + _values_out |= position; + + } else { + _values_out &= ~(position); + } + + if (_values_out != prev) { + if (_values_out) { + _mode = IOX_MODE_ON; + } + send_led_values(); + } + + return OK; + } + + case IOX_SET_MASK: + send_led_enable(arg); + return OK; + + case IOX_GET_MASK: { + uint8_t val; + ret = get(val); + + if (ret == OK) { + return val; + + } else { + return -1; + } + } + + case IOX_SET_MODE: + + if (_mode != (IOX_MODE)arg) { + + switch ((IOX_MODE)arg) { + case IOX_MODE_OFF: + _values_out = 0xFF; + break; + + case IOX_MODE_ON: + _values_out = 0; + break; + + case IOX_MODE_TEST_OUT: + break; + + default: + return -1; + } + + _mode = (IOX_MODE)arg; + send_led_values(); + } + + return OK; + + default: + // see if the parent class can make any use of it + ret = CDev::ioctl(filp, cmd, arg); + break; + } + + return ret; +} + + +void +PCA8574::led_trampoline(void *arg) +{ + PCA8574 *rgbl = reinterpret_cast<PCA8574 *>(arg); + + rgbl->led(); +} + +/** + * Main loop function + */ +void +PCA8574::led() +{ + if (_mode == IOX_MODE_TEST_OUT) { + + // we count only seven states + _counter &= 0xF; + _counter++; + + for (int i = 0; i < 8; i++) { + if (i < _counter) { + _values_out |= (1 << i); + + } else { + _values_out &= ~(1 << i); + } + } + + _update_out = true; + _should_run = true; + } else if (_mode == IOX_MODE_OFF) { + _update_out = true; + _should_run = false; + } else { + + // Any of the normal modes + if (_blinking > 0) { + /* we need to be running to blink */ + _should_run = true; + } else { + _should_run = false; + } + } + + if (_update_out) { + uint8_t msg; + + if (_blinking) { + msg = (_values_out & _blinking & _blink_phase); + + // wipe out all positions that are marked as blinking + msg &= ~(_blinking); + + // fill blink positions + msg |= ((_blink_phase) ? _blinking : 0); + + _blink_phase = !_blink_phase; + } else { + msg = _values_out; + } + + int ret = transfer(&msg, sizeof(msg), nullptr, 0); + + if (!ret) { + _update_out = false; + } + } + + // check if any activity remains, else stp + if (!_should_run) { + _running = false; + return; + } + + // re-queue ourselves to run again later + _running = true; + work_queue(LPWORK, &_work, (worker_t)&PCA8574::led_trampoline, this, _led_interval); +} + +/** + * Sent ENABLE flag to LED driver + */ +int +PCA8574::send_led_enable(uint8_t arg) +{ + + int ret = transfer(&arg, sizeof(arg), nullptr, 0); + + return ret; +} + +/** + * Send 8 outputs + */ +int +PCA8574::send_led_values() +{ + _update_out = true; + + // if not active, kick it + if (!_running) { + _running = true; + work_queue(LPWORK, &_work, (worker_t)&PCA8574::led_trampoline, this, 1); + } + + return 0; +} + +int +PCA8574::get(uint8_t &vals) +{ + uint8_t result; + int ret; + + ret = transfer(nullptr, 0, &result, 1); + + if (ret == OK) { + _values_in = result; + vals = result; + } + + return ret; +} + +void +pca8574_usage() +{ + warnx("missing command: try 'start', 'test', 'info', 'off', 'stop', 'val 0 1'"); + warnx("options:"); + warnx(" -b i2cbus (%d)", PX4_I2C_BUS_LED); + warnx(" -a addr (0x%x)", ADDR); +} + +int +pca8574_main(int argc, char *argv[]) +{ + int i2cdevice = -1; + int pca8574adr = ADDR; // 7bit + + int ch; + + // jump over start/off/etc and look at options first + while ((ch = getopt(argc, argv, "a:b:")) != EOF) { + switch (ch) { + case 'a': + pca8574adr = strtol(optarg, NULL, 0); + break; + + case 'b': + i2cdevice = strtol(optarg, NULL, 0); + break; + + default: + pca8574_usage(); + exit(0); + } + } + + if (optind >= argc) { + pca8574_usage(); + exit(1); + } + + const char *verb = argv[optind]; + + int fd; + int ret; + + if (!strcmp(verb, "start")) { + if (g_pca8574 != nullptr) { + errx(1, "already started"); + } + + if (i2cdevice == -1) { + // try the external bus first + i2cdevice = PX4_I2C_BUS_EXPANSION; + g_pca8574 = new PCA8574(PX4_I2C_BUS_EXPANSION, pca8574adr); + + if (g_pca8574 != nullptr && OK != g_pca8574->init()) { + delete g_pca8574; + g_pca8574 = nullptr; + } + + if (g_pca8574 == nullptr) { + // fall back to default bus + if (PX4_I2C_BUS_LED == PX4_I2C_BUS_EXPANSION) { + errx(1, "init failed"); + } + + i2cdevice = PX4_I2C_BUS_LED; + } + } + + if (g_pca8574 == nullptr) { + g_pca8574 = new PCA8574(i2cdevice, pca8574adr); + + if (g_pca8574 == nullptr) { + errx(1, "new failed"); + } + + if (OK != g_pca8574->init()) { + delete g_pca8574; + g_pca8574 = nullptr; + errx(1, "init failed"); + } + } + + exit(0); + } + + // need the driver past this point + if (g_pca8574 == nullptr) { + warnx("not started, run pca8574 start"); + exit(1); + } + + if (!strcmp(verb, "test")) { + fd = open(PCA8574_DEVICE_PATH, 0); + + if (fd == -1) { + errx(1, "Unable to open " PCA8574_DEVICE_PATH); + } + + ret = ioctl(fd, IOX_SET_MODE, (unsigned long)IOX_MODE_TEST_OUT); + + close(fd); + exit(ret); + } + + if (!strcmp(verb, "info")) { + g_pca8574->info(); + exit(0); + } + + if (!strcmp(verb, "off")) { + fd = open(PCA8574_DEVICE_PATH, 0); + + if (fd < 0) { + errx(1, "Unable to open " PCA8574_DEVICE_PATH); + } + + ret = ioctl(fd, IOX_SET_MODE, (unsigned long)IOX_MODE_OFF); + close(fd); + exit(ret); + } + + if (!strcmp(verb, "stop")) { + fd = open(PCA8574_DEVICE_PATH, 0); + + if (fd == -1) { + errx(1, "Unable to open " PCA8574_DEVICE_PATH); + } + + ret = ioctl(fd, IOX_SET_MODE, (unsigned long)IOX_MODE_OFF); + close(fd); + + // wait until we're not running any more + for (unsigned i = 0; i < 15; i++) { + if (!g_pca8574->is_running()) { + break; + } + + usleep(50000); + printf("."); + fflush(stdout); + } + printf("\n"); + fflush(stdout); + + if (!g_pca8574->is_running()) { + delete g_pca8574; + g_pca8574 = nullptr; + exit(0); + } else { + warnx("stop failed."); + exit(1); + } + } + + if (!strcmp(verb, "val")) { + if (argc < 4) { + errx(1, "Usage: pca8574 val <channel> <0 or 1>"); + } + + fd = open(PCA8574_DEVICE_PATH, 0); + + if (fd == -1) { + errx(1, "Unable to open " PCA8574_DEVICE_PATH); + } + + unsigned channel = strtol(argv[2], NULL, 0); + unsigned val = strtol(argv[3], NULL, 0); + + if (channel < 8) { + ret = ioctl(fd, (IOX_SET_VALUE + channel), val); + } else { + ret = -1; + } + close(fd); + exit(ret); + } + + pca8574_usage(); + exit(0); +} diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 5c0628a16..65922b2a5 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1408,7 +1408,7 @@ int commander_thread_main(int argc, char *argv[]) home.alt = global_position.alt; warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", home.lat, home.lon, (double)home.alt); - mavlink_log_info(mavlink_fd, "#audio: home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt); + mavlink_log_info(mavlink_fd, "home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt); /* announce new home position */ if (home_pub > 0) { diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index bb1ad86ef..304a02c29 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -105,7 +105,8 @@ static struct file_operations fops; */ extern "C" __EXPORT int mavlink_main(int argc, char *argv[]); -static uint64_t last_write_times[6] = {0}; +static uint64_t last_write_success_times[6] = {0}; +static uint64_t last_write_try_times[6] = {0}; /* * Internal function to send the bytes through the right serial port @@ -166,38 +167,40 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length if (instance->get_flow_control_enabled() && ioctl(uart, FIONWRITE, (unsigned long)&buf_free) == 0) { - if (buf_free == 0) { - - if (last_write_times[(unsigned)channel] != 0 && - hrt_elapsed_time(&last_write_times[(unsigned)channel]) > 500 * 1000UL) { - - warnx("DISABLING HARDWARE FLOW CONTROL"); - instance->enable_flow_control(false); - } - - } else { - - /* apparently there is space left, although we might be - * partially overflooding the buffer already */ - last_write_times[(unsigned)channel] = hrt_absolute_time(); + /* Disable hardware flow control: + * if no successful write since a defined time + * and if the last try was not the last successful write + */ + if (last_write_try_times[(unsigned)channel] != 0 && + hrt_elapsed_time(&last_write_success_times[(unsigned)channel]) > 500 * 1000UL && + last_write_success_times[(unsigned)channel] != + last_write_try_times[(unsigned)channel]) + { + warnx("DISABLING HARDWARE FLOW CONTROL"); + instance->enable_flow_control(false); } + } /* If the wait until transmit flag is on, only transmit after we've received messages. Otherwise, transmit all the time. */ if (instance->should_transmit()) { + last_write_try_times[(unsigned)channel] = hrt_absolute_time(); /* check if there is space in the buffer, let it overflow else */ if (!ioctl(uart, FIONWRITE, (unsigned long)&buf_free)) { - if (desired > buf_free) { - desired = buf_free; + if (buf_free < desired) { + /* we don't want to send anything just in half, so return */ + return; } } ssize_t ret = write(uart, ch, desired); if (ret != desired) { warnx("TX FAIL"); + } else { + last_write_success_times[(unsigned)channel] = last_write_try_times[(unsigned)channel]; } } @@ -222,6 +225,8 @@ Mavlink::Mavlink() : _subscriptions(nullptr), _streams(nullptr), _mission_pub(-1), + _mode(MAVLINK_MODE_NORMAL), + _total_counter(0), _verbose(false), _forwarding_on(false), _passing_on(false), @@ -891,6 +896,7 @@ int Mavlink::map_mavlink_mission_item_to_mission_item(const mavlink_mission_item default: mission_item->acceptance_radius = mavlink_mission_item->param2; + mission_item->time_inside = mavlink_mission_item->param1; break; } @@ -899,7 +905,6 @@ int Mavlink::map_mavlink_mission_item_to_mission_item(const mavlink_mission_item mission_item->loiter_direction = (mavlink_mission_item->param3 > 0) ? 1 : -1; /* 1 if positive CW, -1 if negative CCW */ mission_item->nav_cmd = (NAV_CMD)mavlink_mission_item->command; - mission_item->time_inside = mavlink_mission_item->param1; mission_item->autocontinue = mavlink_mission_item->autocontinue; // mission_item->index = mavlink_mission_item->seq; mission_item->origin = ORIGIN_MAVLINK; @@ -918,11 +923,12 @@ int Mavlink::map_mission_item_to_mavlink_mission_item(const struct mission_item_ switch (mission_item->nav_cmd) { case NAV_CMD_TAKEOFF: - mavlink_mission_item->param2 = mission_item->pitch_min; + mavlink_mission_item->param1 = mission_item->pitch_min; break; default: mavlink_mission_item->param2 = mission_item->acceptance_radius; + mavlink_mission_item->param1 = mission_item->time_inside; break; } @@ -933,7 +939,6 @@ int Mavlink::map_mission_item_to_mavlink_mission_item(const struct mission_item_ mavlink_mission_item->param4 = mission_item->yaw * M_RAD_TO_DEG_F; mavlink_mission_item->param3 = mission_item->loiter_radius * (float)mission_item->loiter_direction; mavlink_mission_item->command = mission_item->nav_cmd; - mavlink_mission_item->param1 = mission_item->time_inside; mavlink_mission_item->autocontinue = mission_item->autocontinue; // mavlink_mission_item->seq = mission_item->index; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 401d50f7e..06e0c5904 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -1458,7 +1458,6 @@ Navigator::check_mission_item_reached() /* XXX TODO count turns */ if ((_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || - _mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || _mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED) && _mission_item.loiter_radius > 0.01f) { @@ -1477,27 +1476,27 @@ Navigator::check_mission_item_reached() acceptance_radius = _parameters.acceptance_radius; } - float dist = -1.0f; - float dist_xy = -1.0f; - float dist_z = -1.0f; - - /* calculate AMSL altitude for this waypoint */ - float wp_alt_amsl = _mission_item.altitude; - - if (_mission_item.altitude_is_relative) - wp_alt_amsl += _home_pos.alt; - - dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, wp_alt_amsl, - (double)_global_pos.lat, (double)_global_pos.lon, _global_pos.alt, - &dist_xy, &dist_z); - if (_do_takeoff) { - if (_global_pos.alt > wp_alt_amsl - acceptance_radius) { - /* require only altitude for takeoff */ + /* require only altitude for takeoff */ + if (_global_pos.alt > _pos_sp_triplet.current.alt - acceptance_radius) { _waypoint_position_reached = true; } } else { + float dist = -1.0f; + float dist_xy = -1.0f; + float dist_z = -1.0f; + + /* calculate AMSL altitude for this waypoint */ + float wp_alt_amsl = _mission_item.altitude; + + if (_mission_item.altitude_is_relative) + wp_alt_amsl += _home_pos.alt; + + dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, wp_alt_amsl, + (double)_global_pos.lat, (double)_global_pos.lon, _global_pos.alt, + &dist_xy, &dist_z); + if (dist >= 0.0f && dist <= acceptance_radius) { _waypoint_position_reached = true; } @@ -1567,7 +1566,14 @@ Navigator::on_mission_item_reached() } if (_mission.current_mission_available()) { - set_mission_item(); + if (_mission_item.autocontinue) { + /* continue mission */ + set_mission_item(); + + } else { + /* autocontinue disabled for this item */ + request_loiter_or_ready(); + } } else { /* if no more mission items available then finish mission */ diff --git a/src/modules/uORB/topics/actuator_armed.h b/src/modules/uORB/topics/actuator_armed.h index 6e944ffee..a98d3fc3a 100644 --- a/src/modules/uORB/topics/actuator_armed.h +++ b/src/modules/uORB/topics/actuator_armed.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * 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 @@ -44,15 +44,25 @@ #include <stdint.h> #include "../uORB.h" +/** + * @addtogroup topics + * @{ + */ + /** global 'actuator output is live' control. */ struct actuator_armed_s { - uint64_t timestamp; - bool armed; /**< Set to true if system is armed */ - bool ready_to_arm; /**< Set to true if system is ready to be armed */ - bool lockdown; /**< Set to true if actuators are forced to being disabled (due to emergency or HIL) */ + uint64_t timestamp; /**< Microseconds since system boot */ + bool armed; /**< Set to true if system is armed */ + bool ready_to_arm; /**< Set to true if system is ready to be armed */ + bool lockdown; /**< Set to true if actuators are forced to being disabled (due to emergency or HIL) */ }; +/** + * @} + */ + +/* register this as object request broker structure */ ORB_DECLARE(actuator_armed); #endif
\ No newline at end of file |